text:
//             -- fcgi/request_service.ipp --
//
//            Copyright (c) Darren Garvey 2007.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
////////////////////////////////////////////////////////////////
#ifndef CGI_FCGI_REQUEST_SERVICE_IPP_INCLUDED__
#define CGI_FCGI_REQUEST_SERVICE_IPP_INCLUDED__

#include <vector>

#include <boost/fusion/support.hpp>
#include <boost/fusion/include/algorithm.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/system/error_code.hpp>
////////////////////////////////////////////////////////////////
#include "boost/cgi/fcgi/error.hpp"
#include "boost/cgi/fcgi/client.hpp"
#include "boost/cgi/fcgi/request_service.hpp"
#include "boost/cgi/common/map.hpp"
#include "boost/cgi/common/tags.hpp"
#include "boost/cgi/common/role_type.hpp"
#include "boost/cgi/common/form_parser.hpp"
#include "boost/cgi/common/source_enums.hpp"
#include "boost/cgi/common/request_base.hpp"
#include "boost/cgi/common/parse_options.hpp"
#include "boost/cgi/http/status_code.hpp"
#include "boost/cgi/import/read.hpp"
#include "boost/cgi/import/io_service.hpp"
#include "boost/cgi/detail/service_base.hpp"
#include "boost/cgi/detail/throw_error.hpp"
#include "boost/cgi/config.hpp"

BOOST_CGI_NAMESPACE_BEGIN

   namespace detail {

     /// Helper function to determine the fcgi param length of a parameter (for get values request).
     template <class String>
     std::string param_length(String const &str) {
       std::string result;
       boost::uint32_t length = boost::uint32_t(str.size());
       if (length < 128)
       {
         unsigned char c(length & 0x0000007F);
         result += reinterpret_cast<char const&>(c);
       }
       else
       {
         unsigned char c(((length & 0xFF000000) >> 24) | 0x80);
         result += reinterpret_cast<char const&>(c);
         c = ((length & 0x00FF0000) >> 16);
         result += reinterpret_cast<char const&>(c);
         c = ((length & 0x0000FF00) >> 8);
         result += reinterpret_cast<char const&>(c);
         c = (length & 0x000000FF);
         result += reinterpret_cast<char const&>(c);
       }

       return result;
     }

     /// Helper class to asynchronously load a request.
     /**
      * This is just a function object that can be posted to another
      * thread to do the actual work of loading. It's operator()() does
      * the work.
      */
     template<typename T, typename Handler>
     struct async_load_helper
     {
       async_load_helper(T& t, typename T::implementation_type& impl
                        , common::parse_options opts, Handler h)
         : type(t)
         , impl_(impl)
         , parse_opts_(opts)
         , handler_(h)
       {
       }

       void operator()()
       {
         boost::system::error_code ec;
         type.load(impl_, parse_opts_, ec);
         handler_(ec);
       }

       T& type;
       typename T::implementation_type& impl_;
       common::parse_options parse_opts_;
       Handler handler_;
     };

     struct clear_data
     {
       template<typename T>
       void operator()(T& x) const {
         x.clear();
       }
     };

  } // namespace detail

  namespace fcgi {

    /// Close the request.
    template<typename Protocol>
    BOOST_CGI_INLINE int
    fcgi_request_service<Protocol>::close(
        implementation_type& impl
      , http::status_code hsc
      , int program_status)
    {
      boost::system::error_code ec;
      close(impl, hsc, program_status, ec);
      detail::throw_error(ec);
      return program_status;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE int
    fcgi_request_service<Protocol>::close(
        implementation_type& impl
      , http::status_code hsc __attribute__((__unused__))
      , int program_status
      , boost::system::error_code& ec)
    {
      /**
       * Apache on Windows with mod_fcgid requires that all of the
       * pending data for the connection is read before the request
       * is closed.
       */
      while(!ec
        && impl.client_.status() < common::stdin_read
        && impl.request_status_ != common::loaded)
      {
        parse_packet(impl, ec);
      }

      impl.all_done_ = true;
      impl.client_.close(program_status, ec);
      impl.request_status_ = common::closed;
      return program_status;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE void
    fcgi_request_service<Protocol>::clear(implementation_type& impl)
    {
      impl.post_buffer_.clear();
      impl.param_buffer_.clear();
      // Clear all request data.
      boost::fusion::for_each(impl.vars_, detail::clear_data());
      impl.stdin_parsed_ = false;
      impl.http_status_ = common::http::no_content;
      impl.request_status_ = common::null;
      impl.request_role_ = spec_detail::NONE;
      impl.all_done_ = false;
      impl.client_.status_ = common::none_;
      impl.client_.request_id_ = -1;
	  impl.id_ = 0;
    }

    /// Load the request to a point where it can be usefully used.
    /**
     * FastCGI:
     * --------
     *
     *  - Calls client::construct() to claim a request id from the server.
     *  - Reads, parses and handles all packets until the closing `PARAMS`
     *    packet for this request arrives from the server.
     *
     */
    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::load(
        implementation_type& impl, common::parse_options opts
      , boost::system::error_code& ec)
    {
      impl.client_.construct(impl, ec);
      // Bomb out if the client isn't open here.
      if (!impl.client_.connection()->is_open())
          return error::client_not_open;

      while(!ec)
      {
        if (read_header(impl, ec))
          break;

        int id(spec::get_request_id(impl.header_buf_));
        if (id == spec::null_request_id::value)
        {
          ec = handle_admin_request(impl, ec);
        }
        else if (impl.id_ && impl.id_ != id)
        {
          // The library doesn't support multiplexed connections yet,
          // mainly because I've never had access to a server that
          // supports it.
          //
          // If you have one, can I use it?
          ec = error::multiplexing_not_supported;
        }
        else if (spec::get_type(impl.header_buf_) == spec::begin_request::value)
        {
          impl.id_ = id;
          impl.client_.request_id_ = id;
          if (!read_header(impl, ec))
          {
            spec::begin_request packet(impl.header_buf_);
            impl.request_role_ = packet.role();
            impl.client_.keep_connection_
              = packet.flags() & spec::keep_connection;
            break;
          }
        }
        else
        {
          handle_other_request_header(impl);
        }
      }

      if (ec)
      {
        impl.client_.close();
        impl.client_.connection()->close();
        impl.request_status_ = common::closed;
        return ec;
      }

      if (opts & common::parse_env)
      {
        read_env_vars(impl, ec);
      }

      string_type const &request_method = env_vars(impl.vars_)["REQUEST_METHOD"];
      if (request_method == "GET")
      {
        if (common::request_base<Protocol>::parse_get_vars(impl, ec))
          return ec;
      }
      else
      if ((request_method == "POST" || request_method == "PUT")
          && opts & common::parse_post_only)
      {
        if (opts & common::parse_post_only)
        {
          while(!ec
            && impl.client_.status() < common::stdin_read
            && impl.request_status_ != common::loaded)
          {
            parse_packet(impl, ec);
          }
        }

        if (parse_post_vars(impl, ec))
          return ec;
      }

      if (opts & common::parse_cookies_only)
          common::request_base<Protocol>::parse_cookie_vars(impl, "HTTP_COOKIE", ec);

      return ec;
    }

    template<typename Protocol>
    template<typename Handler>
    BOOST_CGI_INLINE
    void fcgi_request_service<Protocol>::do_load(
        implementation_type& impl, common::parse_options opts,
        Handler handler, boost::system::error_code& ec
      )
    {
      impl.client_.construct(impl, ec);
      // Bomb out if the client isn't open here.
      if (!impl.client_.connection()->is_open())
          ec = error::client_not_open;

    }

    template<typename Protocol>
    template<typename Handler>
    BOOST_CGI_INLINE
    void fcgi_request_service<Protocol>::handle_read_header(
        implementation_type& impl,
        common::parse_options opts,
        Handler handler,
        boost::system::error_code& ec,
        const std::size_t bytes_transferred
      )
    {
      if(ec)
        handler(ec);
      else
      {
        int id(spec::get_request_id(impl.header_buf_));
        if (id == spec::null_request_id::value)
          ec = handle_admin_request(impl, ec);
        else
        if (impl.id_ && impl.id_ != id)
        {
          // The library doesn't "officially" support multiplexed
          // connections yet, because I've never had access to a server
          // that supports it.
          //
          // If you have one, can I use it?
          handler(error::multiplexing_not_supported);
        }
        else
        if (spec::get_type(impl.header_buf_)
            == spec::begin_request::value)
        {
          impl.id_ = id;
          impl.client_.request_id_ = id;
          if (!read_header(impl, ec))
          {
            spec::begin_request packet(impl.header_buf_);
            impl.request_role_ = packet.role();
            impl.client_.keep_connection_
              = packet.flags() & spec::keep_connection;
            strand_.post(&self_type::handle_begin_request_header,
                this, boost::ref(impl), handler, _1
              );
          }
        }else
          handle_other_request_header(impl);
      }
    }

    template<typename Protocol>
    template<typename Handler>
    BOOST_CGI_INLINE
    void fcgi_request_service<Protocol>::handle_begin_request_header(
        implementation_type& impl,
        common::parse_options opts,
        Handler handler,
        boost::system::error_code& ec
      )
    {
      if (//impl.request_status_ < common::env_read &&
          opts & common::parse_env)
      {
        read_env_vars(impl, ec);
        //impl.request_status_ = common::env_read;
      }

      string_type const&
        request_method (env_vars(impl.vars_)["REQUEST_METHOD"]);

      if (request_method == "GET")
      {
        if (common::request_base<Protocol>::parse_get_vars(impl, ec))
          return;
      }
      else
      if ((request_method == "POST" || request_method == "PUT")
          && opts & common::parse_post_only)
      {
        //std::cerr<< "Parsing post vars now.\n";

        if (opts & common::parse_post_only)
        {
          while(!ec
            && impl.client_.status() < common::stdin_read
            && impl.request_status_ != common::loaded)
          {
            parse_packet(impl, ec);
          }
        }

        if (parse_post_vars(impl, ec))
	      return;
      }
      if (opts & common::parse_cookies_only)
          common::request_base<Protocol>::parse_cookie_vars(impl, ec);

      if (ec == error::eof) {
        ec = boost::system::error_code();
      }
    }

    // **FIXME**
    template<typename Protocol>
    template<typename Handler> BOOST_CGI_INLINE
    void fcgi_request_service<Protocol>::async_load(
        implementation_type& impl, common::parse_options opts, Handler handler)
    {
      /*
      strand_.post(
          boost::bind(&self_type::do_load<Handler>,
              this, boost::ref(impl), opts, handler
            )
        );
      */

      strand_.post(
        detail::async_load_helper<self_type, Handler>(this, opts, handler)
      );
    }

    /// Check if the params have been read (ie. FCGI_PARAMS packets)
    template<typename Protocol>
    BOOST_CGI_INLINE
    bool fcgi_request_service<Protocol>::params_read(implementation_type& impl)
    {
      return impl.client_.status() >= common::params_read;
    }

    /// Read and parse the cgi POST meta variables (greedily)
    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code&
    fcgi_request_service<Protocol>::parse_post_vars(
        implementation_type& impl
      , boost::system::error_code& ec)
    {
      impl.client_.bytes_left_
         = boost::lexical_cast<std::size_t>(
             env_vars(impl.vars_)["CONTENT_LENGTH"]);

      return base_type::parse_post_vars(
          impl,
          detail::callback_functor<implementation_type, self_type>(impl, this),
          ec
        );
    }

    /// Read and parse a single cgi POST meta variable (greedily)
    template<typename Protocol>
    template<typename RequestImpl> BOOST_CGI_INLINE
    boost::system::error_code&
    fcgi_request_service<Protocol>::parse_one_post_var(
        implementation_type& impl, boost::system::error_code& ec)
    {
      //#     error "Not implemented"
      return ec;
    }

    /// Read in the FastCGI (env) params
    // **FIXME**
    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::read_env_vars(
        implementation_type& impl, boost::system::error_code& ec)
    {
      while(!ec && !(common::request_base<Protocol>::status(impl) & common::env_read))
      {
        if (this->read_header(impl, ec))
          return ec;

        boost::tribool state = this->parse_header(impl);

        if (state)
        { // the header has been handled and all is ok; continue.
          impl.client_.status(common::params_read);
          common::request_base<Protocol>::status(impl, (common::request_status)(common::request_base<Protocol>::status(impl) | common::env_read));
        }
        else
        if (!state)
        { // The header is confusing; something's wrong. Abort.
          ec = error::bad_header_type;
        }
        else // => (state == boost::indeterminate)
        {
          std::size_t remaining(
            fcgi::spec::get_length(impl.header_buf_));

          if (remaining)
          {
            mutable_buffers_type buf = impl.prepare_misc(remaining);

            if (this->read_body(impl, buf, ec))
              return ec;

            this->parse_body(impl, buf, ec);
          } else
            ec = error::couldnt_write_complete_packet;
        }

      } // while(!ec && !(status(impl) & common::env_read))
      return ec;
    }

    /// Read a single header, but do nothing with it.
    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::read_header(
        implementation_type& impl
      , boost::system::error_code& ec)
    {
      // clear the header first (might be unneccesary).
      impl.header_buf_ = header_buffer_type();
      this->get_io_context().poll();

      async_read(*impl.client_.connection(), buffer(impl.header_buf_), [&](boost::system::error_code const &e, std::size_t bytes_transfered __attribute__((__unused__))) -> void
      {
        ec = e;
      });

      while (impl.header_buf_ == header_buffer_type() && !ec)
      {
        this->get_io_context().run_one();
        if (this->get_io_context().stopped())
         this->get_io_context().restart();
      }

      return ec;
    }

    template<typename Protocol>
    template<typename Handler>
    BOOST_CGI_INLINE void
    fcgi_request_service<Protocol>::async_read_header(
        implementation_type& impl
      , common::parse_options opts
      , Handler handler
      , boost::system::error_code& ec)
    {
      // clear the header first (might be unneccesary).
      impl.header_buf_ = implementation_type::header_buffer_type();

      boost::asio::async_read(
          *impl.client_.connection(), buffer(impl.header_buf_)
        , boost::asio::transfer_all()
        , boost::asio::bind_executor(
              strand_,
              boost::bind(&self_type::handle_read_header,
                  this, boost::ref(impl), opts, handler,
                  boost::asio::placeholders::error,
                  boost::asio::placeholders::bytes_transferred
                )
            )
        );
    }

    /*** Various handlers go below here; they might find a
     * better place to live ***/

    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::handle_admin_request(implementation_type& impl, boost::system::error_code& ec)
    {
      switch(fcgi::spec::get_type(impl.header_buf_))
      {
      case 9: process_get_values(impl, fcgi::spec::get_request_id(impl.header_buf_), ec);
              break;
      default: break;
      }

      return ec;
    }

    // **FIXME**
    template<typename Protocol>
    BOOST_CGI_INLINE void
    fcgi_request_service<Protocol>::handle_other_request_header(implementation_type& /* impl */)
    {
      //std::cerr<< std::endl << "**FIXME** " << __FILE__ << ":" << __LINE__
      //  << " handle_other_request_header()" << std::endl;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::process_abort_request(
        implementation_type& impl, boost::uint16_t id
      , const unsigned char* buf __attribute__((__unused__)), std::size_t len __attribute__((__unused__))
      , boost::system::error_code& ec)
    {
      if (id == fcgi::spec::get_request_id(impl.header_buf_))
      {
        impl.request_status_ = common::aborted;
        return ec;
      }
      try {
        //std::cerr<< "**FIXME** request aborted (id = " << id
        //  << ") but request not notified." << std::endl;
        //impl.client_.connection_->requests_.at(id - 1)->abort();
      }catch(...){
        ec = error::abort_request_record_recieved_for_invalid_request;
      }
      return ec;
    }

    template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::process_params(
        implementation_type& impl, boost::uint16_t id __attribute__((__unused__))
      , const unsigned char* buf, std::size_t len
      , boost::system::error_code& ec)
    {
      if (0 == len)
      { // This is the final param record.

        impl.client_.status(common::params_read);
        return ec;
      }

      while(len)
      {
        boost::uint32_t   name_len, data_len;
        string_type name, data;
        if (*buf >> 7 == 0)
        {
            name_len = *(buf++);
            --len;
        }
        else
        {
            if (len < 4)
                break;
            name_len = ((buf[0] & 0x7F) << 24)
                     +  (buf[1]         << 16)
                     +  (buf[2]         <<  8)
                     +   buf[3];
            buf += 4;
            len -= 4;
        }
        if (0 == len)
            break;

        if (*buf >> 7 == 0)
        {
            data_len = *(buf++);
            --len;
        }
        else
        {
            if (len < 4)
                break;
            data_len = ((buf[0] & 0x7F) << 24)
                     +  (buf[1]         << 16)
                     +  (buf[2]         <<  8)
                     +   buf[3];
            buf += 4;
            len -= 4;
        }
        if (0 == len)
            break;

        name.assign(reinterpret_cast<const char*>(buf), name_len);
        data.assign(reinterpret_cast<const char*>(buf)+name_len, data_len);
        buf += (name_len + data_len);
        len -= (name_len + data_len);

        env_vars(impl.vars_)[name.c_str()] = data;
      }

      return ec;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::process_stdin(
        implementation_type& impl, boost::uint16_t id __attribute__((__unused__))
      , const unsigned char* buf __attribute__((__unused__)),std::size_t len
      , boost::system::error_code& ec)
    {
      if (0 == len)
      {
        impl.client_.status(common::stdin_read);
        return ec;
      }
      return ec;
    }

    /// Parse the current header
    template<typename Protocol>
    BOOST_CGI_INLINE boost::tribool
    fcgi_request_service<Protocol>::parse_header(implementation_type& impl)
    {
      BOOST_ASSERT(fcgi::spec::get_version(impl.header_buf_) == 1
                   && "This library is only compatible with FastCGI 1.0");

      using namespace fcgi::spec_detail;

      switch(fcgi::spec::get_type(impl.header_buf_))
      {
      case BEGIN_REQUEST:
      case DATA:
      case GET_VALUES:
        return boost::indeterminate;
      case STDIN:
        if (0 == fcgi::spec::get_length(impl.header_buf_)) {
          impl.client_.status(common::stdin_read);
          return true;
        } else
          return boost::indeterminate;
      case PARAMS:
        if (0 == fcgi::spec::get_length(impl.header_buf_)) {
          impl.client_.status(common::params_read);
          return true;
        } else
          return boost::indeterminate;
      case ABORT_REQUEST:
        return false;
      case UNKNOWN_TYPE:
      default:
        return true;
      }
    }

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::process_get_values(
        implementation_type& impl, boost::uint16_t id, boost::system::error_code& ec)
    {
      using namespace spec_detail;
      common::env_map vals;
      vals["FCGI_MAX_CONNS"] = "1";
      vals["FCGI_MPXS_CONNS"] = (impl.async_requests_ != 0) ? "1" : "0";
      vals["FCGI_MAX_REQS"] = boost::lexical_cast<std::string>(impl.async_requests_);

      std::vector<char> valuesbuff;
      std::for_each(std::begin(vals), std::end(vals), [&](std::pair<boost::cgi::common::name, std::string> const &pair)
      {
        std::string name_len = detail::param_length(pair.first);
        std::string value_len = detail::param_length(pair.second);

        std::copy(std::begin(name_len), std::end(name_len), std::back_inserter(valuesbuff));
        std::copy(std::begin(value_len), std::end(value_len), std::back_inserter(valuesbuff));
        std::copy(std::begin(pair.first), std::end(pair.first), std::back_inserter(valuesbuff));
        std::copy(std::begin(pair.second), std::end(pair.second), std::back_inserter(valuesbuff));
      });

      std::vector<char> buffer;
      Header hdr;
      hdr.reset(GET_VALUES_RESULT, id, valuesbuff.size());
      buffer.reserve(buffer.size() + sizeof(valuesbuff));
      char const* beg = reinterpret_cast<char const*>(&hdr);
      char const* end = beg + sizeof(hdr);
      std::copy(beg, end, std::back_inserter(buffer));
      std::copy(std::begin(valuesbuff), std::end(valuesbuff), std::back_inserter(buffer));

      std::vector<boost::asio::const_buffer> buffers;
      buffers.push_back(boost::asio::buffer(&(buffer.at(0)), buffer.size()));
      boost::asio::write(impl.client_.connection()->next_layer(), buffers, ec);
      return ec;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::parse_packet(
        implementation_type& impl, boost::system::error_code& ec)
    {
      if (this->read_header(impl, ec))
        return ec;

      boost::tribool state = this->parse_header(impl);

      if (state)
      {
        // the header has been handled and all is ok.
        // **NOOP**
      } else
      if (!state)
      { // The header is confusing; something's wrong. Abort.
        ec = error::bad_header_type;
      }
      else
      {
        std::size_t remaining(fcgi::spec::get_length(impl.header_buf_));
        if (remaining)
        {
          mutable_buffers_type buf = impl.prepare(remaining);

          if (this->read_body(impl, buf, ec))
            return ec;

          this->parse_body(impl, buf, ec);
        }
      }
      return ec;
    }

    /// Read the body of the current packet; do nothing with it.
    template<typename Protocol>
    template<typename MutableBuffersType> BOOST_CGI_INLINE
    boost::system::error_code
    fcgi_request_service<Protocol>::read_body(
        implementation_type& impl, const MutableBuffersType& buf
      , boost::system::error_code& ec)
    {
      std::size_t bytes_read
        = read(*impl.client_.connection(), buf
              , boost::asio::transfer_all(), ec);

      BOOST_ASSERT(bytes_read == fcgi::spec::get_length(impl.header_buf_)
                   && "Couldn't read all of the record body.");
      return ec;
    }

	template<typename Protocol>
    template<typename MutableBuffersType> BOOST_CGI_INLINE
    boost::system::error_code
    fcgi_request_service<Protocol>::parse_body(
        implementation_type& impl, const MutableBuffersType& buf
      , boost::system::error_code& ec)
    {
      switch(fcgi::spec::get_type(impl.header_buf_))
      {
      case 1: process_begin_request(impl, fcgi::spec::get_request_id(impl.header_buf_)
              , static_cast<unsigned char*>(buf.data())
              , boost::asio::buffer_size(buf), ec);
              break;
      case 2: process_abort_request(impl, fcgi::spec::get_request_id(impl.header_buf_)
              , static_cast<unsigned char*>(buf.data())
              , boost::asio::buffer_size(buf), ec);
              break;
      case 4: process_params(impl, fcgi::spec::get_request_id(impl.header_buf_)
              , static_cast<unsigned char*>(buf.data())
              , boost::asio::buffer_size(buf), ec);
              break;
      case 5: process_stdin(impl, fcgi::spec::get_request_id(impl.header_buf_)
              , static_cast<unsigned char*>(buf.data())
              , boost::asio::buffer_size(buf), ec);
              break;
      default: break;
      }
      return ec;
    }

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::begin_request_helper(
        implementation_type& impl
      , header_buffer_type& header
      , boost::system::error_code& ec)
    {
       impl.client_.request_id_ = fcgi::spec::get_request_id(header);

       // A begin request body is as long as a header, so we can optimise:
       if (read_header(impl, ec))
         return ec;

       spec::begin_request packet(impl.header_buf_);
       impl.request_role_ = packet.role();
       impl.client_.role_ = static_cast<common::role_type>(packet.role());
       impl.client_.keep_connection_
         = packet.flags() & spec::keep_connection;

       impl.client_.status_ = common::constructed;

       return ec;
    }

 } // namespace fcgi
BOOST_CGI_NAMESPACE_END

#include "boost/cgi/fcgi/request.hpp"
#include "boost/cgi/basic_request.hpp"

BOOST_CGI_NAMESPACE_BEGIN
 namespace fcgi {

	template<typename Protocol>
    BOOST_CGI_INLINE boost::system::error_code
    fcgi_request_service<Protocol>::process_begin_request(
        implementation_type& impl, boost::uint16_t id __attribute__((__unused__))
      , const unsigned char* buf __attribute__((__unused__)), std::size_t len __attribute__((__unused__))
      , boost::system::error_code& ec)
    {
      if (impl.client_.request_id_ == 0) // ie. hasn't been set yet.
      {
        begin_request_helper(impl, impl.header_buf_, ec);
      }
      else
      {
        //std::cerr<< "**FIXME** Role: "
        //  << fcgi::spec::begin_request::get_role(impl.header_buf_) << std::endl;

        //implementation_type::client_type::connection_type&
        //  conn = *impl.client_.connection();
      }
      return ec;
    }

 } // namespace fcgi
BOOST_CGI_NAMESPACE_END

#endif // CGI_FCGI_REQUEST_SERVICE_IPP_INCLUDED__

ext: ipp


text:
#include <path_follower/controller/robotcontroller_unicycle_inputscaling.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include <cslibs_navigation_utilities/MathHelper.h>
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>

#include <deque>

#include <limits>
#include <boost/algorithm/clamp.hpp>

#ifdef TEST_OUTPUT
#include <std_msgs/Float64MultiArray.h>
#endif

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_Unicycle_InputScaling, unicycle_inputscaling, default_collision_avoider);

RobotController_Unicycle_InputScaling::RobotController_Unicycle_InputScaling() :
    RobotController()
{

    double k = params_.k();
    setTuningParameters(k);
    setDirSign(1.f);

    ROS_INFO("Parameters: k=%f\n"
             "vehicle_length=%f\n"
             "goal_tolerance=%f\n"
             "max_angular_velocity=%f",
             params_.k(),
             params_.vehicle_length(),
             params_.goal_tolerance(),
             params_.max_angular_velocity());

#ifdef TEST_OUTPUT
    test_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("test_output", 100);
#endif
}

void RobotController_Unicycle_InputScaling::setTuningParameters(const double k) {
    k1_ = 1. * k * k * k;
    k2_ = 3. * k * k;
}

void RobotController_Unicycle_InputScaling::stopMotion() {

    move_cmd_.setVelocity(0.f);
    move_cmd_.setDirection(0.f);
    move_cmd_.setRotationalVelocity(0.f);

    MoveCommand cmd = move_cmd_;
    publishMoveCommand(cmd);
}

void RobotController_Unicycle_InputScaling::start() {

}

void RobotController_Unicycle_InputScaling::reset() {

    RobotController::reset();
}

void RobotController_Unicycle_InputScaling::setPath(Path::Ptr path) {
    RobotController::setPath(path);
}

RobotController::MoveCommandStatus RobotController_Unicycle_InputScaling::computeMoveCommand(
        MoveCommand* cmd) {

    if(path_interpol.n() <= 2)
        return RobotController::MoveCommandStatus::ERROR;

    const Eigen::Vector3d pose = pose_tracker_->getRobotPose();

    const geometry_msgs::Twist v_meas_twist = pose_tracker_->getVelocity();

    double velocity_measured = dir_sign_ * sqrt(v_meas_twist.linear.x * v_meas_twist.linear.x
                                                + v_meas_twist.linear.y * v_meas_twist.linear.y);

    RobotController::findOrthogonalProjection();
    double d = orth_proj_;

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    // draw a line to the orthogonal projection
    geometry_msgs::Point from, to;
    from.x = pose[0]; from.y = pose[1];
    to.x = path_interpol.p(proj_ind_); to.y = path_interpol.q(proj_ind_);
    visualizer_->drawLine(12341234, from, to, getFixedFrame(), "kinematic", 1, 0, 0, 1, 0.01);

    // theta_e = theta_vehicle - theta_path (orientation error)
    double theta_e = MathHelper::AngleDelta(path_interpol.theta_p(proj_ind_), pose[2]);

    // if dir_sign is negative, we drive backwards and set theta_e to the complementary angle
    if (getDirSign() < 0.) {
        theta_e = MathHelper::NormalizeAngle(M_PI + theta_e);
        d = -d;
    }

    // curvature and first two derivations
    const double c = path_interpol.curvature(proj_ind_);
    const double dc_ds = path_interpol.curvature_prim(proj_ind_);

    // 1 - dc(s)
    const double _1_dc = 1. - d * c;

    // cos, sin, tan of theta_e
    const double cos_theta_e = cos(theta_e);
    const double cos_theta_e_2 = cos_theta_e * cos_theta_e;

    const double sin_theta_e = sin(theta_e);
    const double sin_theta_e_2 = sin_theta_e * sin_theta_e;

    const double tan_theta_e = tan(theta_e);

    //	const double x1 = s;
    const double x2 = _1_dc * tan_theta_e;

    const double x3 = d;

    // u1, u2

    const double u1 = velocity_measured * cos_theta_e / _1_dc;
    const double u2 =
            - k1_ * u1 * x3
            - k2_ * fabs(u1) * x2;

    // longitudinal velocity
    const double v1 = velocity_;

    // angle velocity
    double v2 = u2 * cos_theta_e_2 / _1_dc +
            u1 * (c * (1 + sin_theta_e_2) + d * dc_ds * (sin_theta_e * cos_theta_e)/_1_dc);

    v2 = boost::algorithm::clamp(v2, -params_.max_angular_velocity(), params_.max_angular_velocity());

    double exp_factor = RobotController::exponentialSpeedControl();
    move_cmd_.setRotationalVelocity(v2);
    move_cmd_.setVelocity(getDirSign() * v1 * exp_factor);
    *cmd = move_cmd_;

#ifdef TEST_OUTPUT
    publishTestOutput(proj_ind_, d, theta_e, velocity_measured);
#endif

    *cmd = move_cmd_;

    return RobotController::MoveCommandStatus::OKAY;

}

void RobotController_Unicycle_InputScaling::publishMoveCommand(
        const MoveCommand& cmd) const {

    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getRotationalVelocity();

    cmd_pub_.publish(msg);
}

#ifdef TEST_OUTPUT
void RobotController_Unicycle_InputScaling::publishTestOutput(const unsigned int waypoint, const double d,
                                                              const double theta_e,
                                                              const double v) const {
    std_msgs::Float64MultiArray msg;

    msg.data.push_back((double) waypoint);
    msg.data.push_back(d);
    msg.data.push_back(theta_e);
    msg.data.push_back(v);

    test_pub_.publish(msg);
}
#endif

ext: cpp

max_stars_repo_name: sunarditay/gerona

max_issues_repo_name: sunarditay/gerona

max_forks_repo_name: sunarditay/gerona


text:
#include "ni/layers/depthgradient.h"

#include <opencv2/highgui/highgui.hpp>

#include "elm/core/debug_utils.h"
#include "elm/core/cv/mat_utils.h"
#include "elm/core/exception.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

using namespace std;
using namespace cv;
using namespace elm;
using namespace ni;

/** Define parameters, defaults and I/O keys
  */
// paramters
const std::string DepthGradient::PARAM_GRAD_WEIGHT = "grad_weight";

// defaults
const float DepthGradient::DEFAULT_GRAD_WEIGHT = 0.f;

// output keys
const std::string DepthGradient::KEY_OUTPUT_GRAD_X = "grad_x";
const std::string DepthGradient::KEY_OUTPUT_GRAD_Y = "grad_y";

/** @todo why does define guard lead to undefined reference error?
 */
//#ifdef __WITH_GTEST
#include <boost/assign/list_of.hpp>
template <>
elm::MapIONames LayerAttr_<DepthGradient>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(detail::BASE_SINGLE_INPUT_FEATURE_LAYER__KEY_INPUT_STIMULUS)
        ELM_ADD_OUTPUT_PAIR(DepthGradient::KEY_OUTPUT_GRAD_X)
        ELM_ADD_OUTPUT_PAIR(DepthGradient::KEY_OUTPUT_GRAD_Y)
        ;
//#endif // __WITH_GTEST

/* Create depth-gradient map from depth map
 *
 * Input:
 * vDepth - depth map
 * vnCloudIdx_d - indices of valid points
 * nDIdxCntTmp - count of valid points
 * nDGradConst - constant of the weighted depth
 * nDSegmDThres - threshold for very steep depth-gradient
 * nDGradNan - constant for the very steep depth-gradient
 * nDsWidth - width of depth map
 * nDGradXMin, nDGradXMax, nDGradYMin, nDGradYMax - max and min values of depth-gradient data
 *
 * Output:
 * vDGradX,vDGradY - depth-gradient maps
 */
//void MakeDGradMap (std::vector<float> vDepth, std::vector<int> vCloudIdx_d, int nDIdxCntTmp, float nDGradConst, float nDSegmDThres, float nDGradNan, int nDsWidth,
//                      float &nDGradXMin, float &nDGradXMax, float &nDGradYMin, float &nDGradYMax, std::vector<float> &vDGradX, std::vector<float> &vDGradY)
//{
//    /////// Processing depth map: make depth gradient map ////////////////////////////////////////////////
//    if (nDsWidth <= 320) {
//        for(int i = 0; i < nDIdxCntTmp; i++) {
//            int x, y, idx;
//            idx = vCloudIdx_d[i];
//            GetPixelPos(idx, nDsWidth, x, y);
//            if (!vDepth[idx]) continue;

//            if (y > 0) {
//                if (vDepth[idx-nDsWidth]) {
//                    //vDGradY[idx] = vDepth[idx] - vDepth[idx-nDsWidth];
//                    vDGradY[idx] = (vDepth[idx] - vDepth[idx-nDsWidth]) / (vDepth[idx] + nDGradConst);
//                    if (fabs(vDGradY[idx]) > nDSegmDThres) vDGradY[idx] = nDGradNan;
//                    else {
//                        if (vDGradY[idx] > nDGradYMax) nDGradYMax = vDGradY[idx];
//                        if (vDGradY[idx] < nDGradYMin) nDGradYMin = vDGradY[idx];
//                    }
//                }
//            }
//            if (x > 0) {
//                if (vDepth[idx-1]) {
//                    (vDGradX[idx] = vDepth[idx] - vDepth[idx-1]) / (vDepth[idx] + nDGradConst);;
//                    if (fabs(vDGradX[idx]) > nDSegmDThres) vDGradX[idx] = nDGradNan;
//                    else {
//                        if (vDGradX[idx] > nDGradXMax) nDGradXMax = vDGradX[idx];
//                        if (vDGradX[idx] < nDGradXMin) nDGradXMin = vDGradX[idx];
//                    }
//                }
//            }
//        }
//    }
//    else {
//        for(int i = 0; i < nDIdxCntTmp; i++) {
//            int x, y, idx;
//            idx = vCloudIdx_d[i];
//            GetPixelPos(idx, nDsWidth, x, y);
//            if (x%2) continue;
//            if (y%2) continue;
//            if (!vDepth[idx]) continue;

//            if (y > 0) {
//                if (vDepth[idx-2*nDsWidth]) {
//                    vDGradY[idx] = (vDepth[idx] - vDepth[idx-2*nDsWidth]) / (vDepth[idx] + nDGradConst);
//                    if (fabs(vDGradY[idx]) > nDSegmDThres) vDGradY[idx] = nDGradNan;
//                    else {
//                        if (vDGradY[idx] > nDGradYMax) nDGradYMax = vDGradY[idx];
//                        if (vDGradY[idx] < nDGradYMin) nDGradYMin = vDGradY[idx];
//                    }
//                    vDGradY[idx+1] = vDGradY[idx];
//                    vDGradY[idx+nDsWidth] = vDGradY[idx];
//                    vDGradY[idx+nDsWidth+1] = vDGradY[idx];
//                }
//            }
//            if (x > 0) {
//                if (vDepth[idx-2]) {
//                    (vDGradX[idx] = vDepth[idx] - vDepth[idx-2]) / (vDepth[idx] + nDGradConst);;
//                    if (fabs(vDGradX[idx]) > nDSegmDThres) vDGradX[idx] = nDGradNan;
//                    else {
//                        if (vDGradX[idx] > nDGradXMax) nDGradXMax = vDGradX[idx];
//                        if (vDGradX[idx] < nDGradXMin) nDGradXMin = vDGradX[idx];
//                    }
//                    vDGradX[idx+1] = vDGradX[idx];
//                    vDGradX[idx+nDsWidth] = vDGradX[idx];
//                    vDGradX[idx+nDsWidth+1] = vDGradX[idx];
//                }
//            }
//        }
//    }
//}

void DepthGradient::Clear()
{
    grad_x_ = Mat1f();
    grad_y_ = Mat1f();
}

void DepthGradient::Reset(const LayerConfig &config)
{
    Reconfigure(config);
}

void DepthGradient::Reconfigure(const LayerConfig &config)
{
    // params
    PTree params = config.Params();
    w_ = params.get<float>(PARAM_GRAD_WEIGHT, DEFAULT_GRAD_WEIGHT);
}

void DepthGradient::OutputNames(const LayerOutputNames &io)
{
    name_out_grad_x_ = io.Output(KEY_OUTPUT_GRAD_X);
    name_out_grad_y_ = io.Output(KEY_OUTPUT_GRAD_Y);
}

void DepthGradient::Activate(const Signal &signal)
{
    Mat1f in = signal.MostRecentMat1f(name_input_);

    ELM_THROW_BAD_DIMS_IF(in.cols < 2,
                          "Input must have > 1 cols to compute gradient in x direction.");

    // compute gradient in x direction:
    computeDerivative(in, 0, grad_x_);

    const float NaN = std::numeric_limits<float>::quiet_NaN();

    hconcat(Mat1f(grad_x_.rows, 1, NaN), grad_x_, grad_x_);

    ELM_THROW_BAD_DIMS_IF(in.rows < 2,
                          "Input must have > 1 rows to compute gradient in y direction.");

    // compute gradient in y direction:
    computeDerivative(in, 1, grad_y_);
    vconcat(Mat1f(1, grad_y_.cols, NaN), grad_y_, grad_y_);
}

void DepthGradient::Response(Signal &signal)
{
    signal.Append(name_out_grad_x_, grad_x_);
    signal.Append(name_out_grad_y_, grad_y_);
}

DepthGradient::DepthGradient()
    : base_SingleInputFeatureLayer()
{
    Clear();
}

void DepthGradient::computeDerivative(const Mat1f &src, int dim, Mat1f &dst) const
{
    Mat1f in_shift, in_crop, denom, diff;

    if(dim == 0) {

        // horizontal
        in_shift = src.colRange(1, src.cols);
        in_crop = src.colRange(0, src.cols-1);
    }
    else if(dim == 1) {

        // vertical
        in_shift = src.rowRange(1, src.rows);
        in_crop = src.rowRange(0, src.rows-1);
    }
    else {

        stringstream s;
        s << "Invalid dimension value (" << dim <<"). " <<
             "Expecting either 0 for vertical or 1 for horizontal.";
        ELM_THROW_VALUE_ERROR(s.str());
    }

    //diff = in_shift - in_crop;
    cv::subtract(in_shift, in_crop, diff);

    // gradient =  diff ./ (|in|+w)
    cv::add(in_shift, w_, denom);

    dst = Mat1f(in_shift.size());

    // Had to replace call to divide() with iterating over all elements
    // divide() was producing nan results instead of zeros
    // could not reproduce in unit test.
    //divide(diff, denom, dst);
    float *data_numer_ptr = reinterpret_cast<float*>(diff.data);
    float *data_numer_end = reinterpret_cast<float*>(diff.dataend);
    float *data_denom_ptr = reinterpret_cast<float*>(denom.data);
    float *data_dst_ptr = reinterpret_cast<float*>(dst.data);

    for(; data_numer_ptr != data_numer_end;
        ++data_numer_ptr, ++data_denom_ptr, ++data_dst_ptr) {

        *data_dst_ptr = *data_numer_ptr / (*data_denom_ptr);
    }
}


text:
#include <boost_python_exception/auto_translation/import.hpp>

#include <boost/python/import.hpp>

#include <boost_python_exception/get_exception_info.hpp>
#include <boost_python_exception/clear_exception.hpp>
#include <boost_python_exception/auto_translation/exception_translator.hpp>

namespace bp = boost::python;

namespace boost_python_exception { namespace auto_translation {

boost::python::object import(std::string const & module_name)
{
    try {
        return bp::import(module_name.c_str());
    } catch (bp::error_already_set const &) {
        exception_info const error = get_exception_info();
        clear_exception();
        exception_translator::translate(error);
    }
}

} }


text:
#include <geas/constraints/builtins.h>
#include <lazycbs/mapf/mapf-solver.h>
#include <tuple>
#include <boost/format.hpp>
#include "compute_heuristic.h"

// turn off in the future
#define DEBUG_UC
// #define MAPF_NO_RECTANGLES

namespace mapf {

static std::pair<int, bool*> mapf_get_res_table(MAPF_Solver* m, int excl) {
  return m->retrieve_reservation_table(excl);
}

MAPF_Solver::MAPF_Solver(const MapLoader& _ml, const AgentsLoader& _al, const EgraphReader& _egr, int UB)
  : ml(&_ml), al(&_al), egr(&_egr), map_size(ml->rows * ml->cols)
  , reservation_table(map_size, false), cmap(map_size, -1), nmap(map_size, -1)
  , agent_set(al->num_of_agents)
  , cost_ub(UB)
  , HL_conflicts(0) {

    int num_of_agents = al->num_of_agents;
    // int map_size = ml->rows*ml->cols;
    cost_lb = 0;

    for(int ai = 0; ai < num_of_agents; ++ai) {
      int init_loc = ml->linearize_coordinate((al->initial_locations[ai]).first, (al->initial_locations[ai]).second);
      int goal_loc = ml->linearize_coordinate((al->goal_locations[ai]).first, (al->goal_locations[ai]).second);
      ComputeHeuristic ch(goal_loc, ml->get_map(), map_size, ml->actions_offset, 1, egr);

      geas::intvar cv(s.new_intvar(0, UB));
      Agent_PF* pf(new Agent_PF(s.data, cv, init_loc, goal_loc, ch.getHVals(), ml->get_map(), map_size, ml->actions_offset, std::bind(mapf_get_res_table, this, ai)));
      pathfinders.push(pf);

      cost_lb += pf->pathCost();
      penalty_table.insert(std::make_pair(cv.p, penalties.size()));
      penalties.push(penalty { cv.p, geas::from_int(pf->pathCost()) });
    }
}

// Get the local reservation table for agent excl.
// This is kind of expensive; find a better way.
std::pair<int, bool*> MAPF_Solver::retrieve_reservation_table(int excl) {
  // Get the maximum path length
  int frame_sz = ml->rows*ml->cols;
  int cap = 0;
  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    if(ai == excl)
      continue;
    cap = std::max(cap, pathfinders[ai]->getPath().size());
  }
  // Clear the existing table. This _should_ be safe (assuming sizeof(bool) == 1).
  std::memset(reservation_table.begin(), 0, reservation_table.size());
  reservation_table.growTo(frame_sz * cap, false);

  // Now fill it
  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    if(ai == excl)
      continue;
    int base = 0;
    for(int p : pathfinders[ai]->getPath()) {
      reservation_table[base + p] = true;
      base += frame_sz;
    }
    int pE = pathfinders[ai]->getPath().last();
    for(; base < frame_sz * cap; base += frame_sz)
      reservation_table[base + pE] = true;
  }

  return std::make_pair(cap, reservation_table.begin());
}
bool apply_penalties(MAPF_Solver& mf) {
   for(MAPF_Solver::penalty& p : mf.penalties)  {
    geas::patom_t at(geas::le_atom(p.p, p.lb));
    if(!mf.s.assume(at))
      return false;
    if(mf.s.is_aborted())
      throw MAPF_Solver::SolveAborted { };
  }
  return true;
}

/*
bool apply_makespan(MAPF_Solver& mf, int lb) {
   for(Agent_PF* p : pathfinders) {
    geas::patom_t at(geas::le_atom(p.p, lb));
    if(!mf.s.assume(at))
      return false;
    if(mf.s.is_aborted())
      throw MAPF_Solver::SolveAborted { };
  }
  return true;
}
*/

void log_conflict(MAPF_Solver& mapf) {
  for(auto new_conflict : mapf.new_conflicts) {
    int a1(new_conflict.a1);
    int a2(new_conflict.a2);
    int t(new_conflict.timestamp);

    if(new_conflict.type == MAPF_Solver::C_BARRIER) {
      fprintf(stderr, "%%%% Adding rectangle: [%d, (%d, %d) |- (%d, %d), %d, %d]\n",
        t, mapf.row_of(new_conflict.b.s_loc), mapf.col_of(new_conflict.b.s_loc),
           mapf.row_of(new_conflict.b.e_loc), mapf.col_of(new_conflict.b.e_loc),
           a1, a2);
    } else {
      fprintf(stderr, "%%%% Adding conflict: [%d, (%d, %d), %d, %d, %d | (%d, %d -> %d, %d) | (%d, %d -> %d, %d) ]\n", new_conflict.timestamp, new_conflict.p.loc1 / mapf.ml->cols, new_conflict.p.loc1 % mapf.ml->cols, new_conflict.p.loc2, new_conflict.a1, new_conflict.a2,
        mapf.pathfinders[new_conflict.a1]->start_pos / mapf.ml->cols, mapf.pathfinders[new_conflict.a1]->start_pos % mapf.ml->cols,
        mapf.pathfinders[new_conflict.a1]->goal_pos / mapf.ml->cols, mapf.pathfinders[new_conflict.a1]->goal_pos % mapf.ml->cols,
        mapf.pathfinders[new_conflict.a2]->start_pos / mapf.ml->cols, mapf.pathfinders[new_conflict.a2]->start_pos % mapf.ml->cols,
        mapf.pathfinders[new_conflict.a2]->goal_pos / mapf.ml->cols, mapf.pathfinders[new_conflict.a2]->goal_pos % mapf.ml->cols);
    }
  }
}

bool MAPF_Solver::buildPlan(vec<geas::patom_t>& assumps) {
  s.clear_assumptions();

  // Apply the assumptions.
  for(geas::patom_t at : assumps) {
    if(!s.assume(at))
      return false;
    if(s.is_aborted())
      throw MAPF_Solver::SolveAborted { };
  }

retry:
  switch(s.solve()) {
    // We've proven the current subproblem is infeasible.
    case geas::solver::UNSAT:
      return false;
    // Candidate optimal solution. Check for conflicts.
    case geas::solver::SAT:
      // First, finesse the plan to avoid any remaining conflicts.
      if(!resolveConflicts()) {
        s.restart();
#ifdef DEBUG_UC
        log_conflict(*this);
#endif
        if(!addConflict())
          return false;
        goto retry;
      }
      // If we succeeded, done.
      return true;
    case geas::solver::UNKNOWN:
      throw SolveAborted { };
  }
  // Should be unreachable
  GEAS_ERROR;
}

bool MAPF_Solver::minimizeCost(void) {
  s.clear_assumptions();

  // Set up the penalties, and lower bound.
  cost_lb = 0;
  for(Agent_PF* p : pathfinders) {
    cost_lb += p->pathCost();
  }
#ifdef DEBUG_UC
  fprintf(stderr, "%%%% Initial bound: %d\n", cost_lb);
#endif

  vec<geas::patom_t> core;
  if(!apply_penalties(*this))
    return false;
  while(checkForConflicts()) {
#ifdef DEBUG_UC
    log_conflict(*this);
#endif
    // s.restart();
    if(!addConflict())
      return false;

    // s.clear_assumptions();
    while(!runUCIter()) {
      s.get_conflict(core);
      s.clear_assumptions();
      s.restart();
      /*
      if(!processCore(core))
        return false;
        */
      cost_lb += processCore(core);
      apply_penalties(*this);
#ifdef DEBUG_UC
      fprintf(stderr, "%%%% Found core of size (%d), current lower bound %d\n", core.size(), cost_lb);
#endif
    }
  }
  return true;
}

void MAPF_Solver::printStats(FILE* f) const {
  int LL_num_generated = 0;
  int LL_num_expanded = 0;
  int LL_executions = 0;
  for(Agent_PF* p : pathfinders) {
    LL_num_generated += p->num_generated;
    LL_num_expanded += p->num_expanded;
    LL_executions += p->num_executions;
  }
  fprintf(f, "%d ; %d ; %d ; %d ; %d ; %d\n", cost_lb, s.data->stats.conflicts, LL_num_expanded, LL_num_generated, HL_conflicts, LL_executions);
  // std::cout << cost_lb << " ; " << s.data->stats.conflicts << " ; " << LL_num_expanded << " ; " << LL_num_generated << " ; " << HL_conflicts << " ; " << LL_executions;
}

/*
This function prints the plan and constraints in the folloowing order:

1. The plan for each agent is printed in a separate line with the prefix "Agent a: " (a is the agent number), which followed by a space-delimited list of coordinates in the form (row,column).
2. The constraints are printed in seprate line with the prefix "Constraints: ", which is followed by a space-delimited list of constraints in the form [(row1,column1),(row2,column2),agent,time]
3. The barriers are printed in seprate line with the prefix "Barriers: ", which is followed by a space-delimited list of barriers in the form [type,direction,(row1,column1),(row2,column2),agent,time1,time2]
  * The type can be either ENTRY or EXIT
  * The direction can be UP, DOWN, LEFT or RIGHT
*/
string MAPF_Solver::printPaths() const {
  std::ostringstream output;
  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    output << boost::format("Agent %d:") % ai;
    for(int loc : pathfinders[ai]->getPath()) {
      output << boost::format(" (%d,%d)") % (row_of(loc) - 1) % (col_of(loc) - 1);
    }
    output << "\n";
  }
  output << "Constraints: ";
  for (auto c_key: cons_map) {
    int loc1 = c_key.first.loc1, loc2 = c_key.first.loc2, timestamp = c_key.first.timestamp;
    for (int agent = 0; agent < pathfinders.size(); agent++) {
      if (constraints[c_key.second].attached.elem(agent) && pathfinders[agent]->getPath()[timestamp] != loc1 && pathfinders[agent]->getPath()[timestamp] != loc2) {
        output << boost::format(" [(%d,%d),(%d,%d),%d,%d]") % (row_of(loc1) - 1) % (col_of(loc1) - 1) % (row_of(loc2) - 1) % (col_of(loc2) - 1) % agent % timestamp;
        break;
      }
    }
  }
  output << "\n";
  output << "Barriers: ";
  for (auto meta: b_metadata) {
    int agent = meta.agent;
    int startTime = meta.startTime;
    int endTime = startTime + meta.duration;
    int corner1 = meta.corner1;
    int corner2 = meta.corner2;
    string type = meta.type == ENTRY ? "ENTRY" : "EXIT";
    string direction = meta.direction == UP ? "UP" : meta.direction == DOWN ? "DOWN" : meta.direction == LEFT ? "LEFT" : "RIGHT";
    output << boost::format(" [%s,%s,(%d,%d),(%d,%d),%d,%d,%d]") % type % direction % (row_of(corner1) - 1) % (col_of(corner1) - 1) % (row_of(corner2) - 1) % (col_of(corner2) - 1) % agent % startTime % endTime;
  }
  return output.str();
}
// Returns maximum length
/*
int MAPF_Solver::extractPaths(void) {
  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    paths[ai].clear();
    pathfinders[ai]->extractPath(paths[ai]);
  }
  return 0;
}
*/

int MAPF_Solver::maxPathLength(void) const {
  int len = 0;
  for(Agent_PF* p : pathfinders)
    len = std::max(len, static_cast<int>(std::ceil(p->pathCost())));
  return len;
}

// Exactly as for the normal MAPF solver, check the current
// 'incumbent' for conflicts.
// FIXME: This is wasteful; use a sparse representation.
inline int agentPosition(Agent_PF* p, int t) {
  const vec<int>& P(p->getPath());
  return (t < P.size()) ? P[t] : P.last();
}

inline void clear_map(MAPF_Solver* s, vec<int>& map, int t) {
  for(Agent_PF* p : s->pathfinders) {
    map[agentPosition(p, t)] = -1;
  }
}

/*
void MAPF_Solver::printMonotoneSubchain(int dy, int dx, int ai, int t) {
  const vec<int>& P(pathfinders[ai]->getPath());

  char dx(0);
  char dy(0);

  int p(P[t]);
  int r(p / ml->cols);
  int c(p % ml->cols);

  int rp(r);
  int cp(c);
  // Run back in time.
  int s = t-1;
  for(; s >= 0; --s) {
    int rs(P[s] / ml->cols);
    int cs(P[s] % ml->cols);

    if(abs(dy - (rs - rp)) > 1)
      break;
    if(abs(dx - (cs - cp)) > 1)
      break;
    if(rs != rp)
      dy = (rs - rp);
    if(cs != cp)
      dy = (rs - rp);
    rp = rs;
    cp = cs;
  }
  ++s;

  dx = 0;
  dy = 0;
  rp = r;
  cp = c;
  for(++t; t < P.size(); ++t) {
    int rs(P[t] / ml->cols);
    int cs(P[t] % ml->cols);

    if(abs(dy - (rp - rs)) > 1)
      break;
    if(abs(dx - (cp - cs)) > 1)
      break;

    if(rs != rp)
      dy = (rp - rs);
    if(cs != cp)
      dy = (rp - rs);
    rp = rs;
    cp = cs;
  }

  fprintf(stderr, "%% {{%d [%d] :", ai, s);
  for(int p = s; p < t; ++p) {
    fprintf(stderr, " (%d, %d)", P[p] / ml->cols, P[p] % ml->cols);
  }
  fprintf(stderr, "\n");
}
*/
int MAPF_Solver::monotoneSubchainStart(int dy, int dx, int ai, int t) const {
  int p(agentPosition(pathfinders[ai], t));
  for(--t; t >= 0; --t) {
    int q(agentPosition(pathfinders[ai], t));
    if(p == q)
      return t+1;
    if(col_of(p) != col_of(q) && col_of(p) - col_of(q) != dx)
      return t+1;
    if(row_of(p) != row_of(q) && row_of(p) - row_of(q) != dy)
      return t+1;
    p = q;
  }
  return 0;
}

int MAPF_Solver::monotoneSubchainEnd(int dy, int dx, int ai, int t) const {
  int p(agentPosition(pathfinders[ai], t));
  int tMax(pathfinders[ai]->getPath().size());
  for(++t; t < tMax; ++t) {
    int q(agentPosition(pathfinders[ai], t));
    if(p == q)
      return t-1;
    if(col_of(p) != col_of(q) && col_of(q) - col_of(p) != dx)
      return t-1;
    if(row_of(p) != row_of(q) && row_of(q) - row_of(p) != dy)
      return t-1;
    p = q;
  }
  return tMax-1;
}

bool MAPF_Solver::resolveConflicts(void) {
  int pMax = maxPathLength();

  vec<bool> conflicting(pathfinders.size(), false);

  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    int loc = pathfinders[ai]->getPath()[0];
    assert(nmap[loc] < 0); // Shouldn't be any conflicts at t0.
    nmap[loc] = ai;
  }
  // Run through the candidate plan, identify agents with conflicts.
  for(int t = 1; t <= pMax; ++t) {
    std::swap(cmap, nmap);

    for(int ai = 0; ai < pathfinders.size(); ++ai) {
      int loc = agentPosition(pathfinders[ai], t);
      if(nmap[loc] >= 0) {
        // Edge conflict between agents ai and nmap[loc].
        conflicting[ai] = true;
        conflicting[nmap[loc]] = true;
      } else {
        nmap[loc] = ai;
      }
      if(cmap[loc] > 0 && cmap[loc] != ai) {
        // Get the new location of the agent we're replacing.
        int rloc = agentPosition(pathfinders[cmap[loc]], t);
        if(cmap[rloc] == ai) {
          // Edge conflict between agents ai and cmap[loc].
          conflicting[ai] = true;
          conflicting[cmap[loc]] = true;
        }
      }
    }
    // Now we zero out the previous cmap.
    clear_map(this, cmap, t-1);
  }
  clear_map(this, nmap, pMax);

  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    if(conflicting[ai]) {
      // Try re-routing agent ai, in the hopes of getting a better plan.
      pathfinders[ai]->find_bypass();
    }
  }
  return !checkForConflicts();
}

#if 0
bool MAPF_Solver::checkForConflicts(void) {
  int pMax = maxPathLength();

  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    int loc = pathfinders[ai]->getPath()[0];
    assert(nmap[loc] < 0); // Shouldn't be any conflicts at t0.
    nmap[loc] = ai;
  }
  for(int t = 1; t < pMax; ++t) {
    std::swap(cmap, nmap);

    for(int ai = 0; ai < pathfinders.size(); ++ai) {
      int loc = agentPosition(pathfinders[ai], t);
      if(nmap[loc] >= 0) {
        // Already occupied.
        int aj(nmap[loc]);
        int dy1 = row_of(agentPosition(pathfinders[ai], t)) - row_of(agentPosition(pathfinders[ai], t-1));
        int dx1 = col_of(agentPosition(pathfinders[ai], t)) - col_of(agentPosition(pathfinders[ai], t-1));
        int dy2 = row_of(agentPosition(pathfinders[aj], t)) - row_of(agentPosition(pathfinders[aj], t-1));
        int dx2 = col_of(agentPosition(pathfinders[aj], t)) - col_of(agentPosition(pathfinders[aj], t-1));
#ifdef MAPF_NO_RECTANGLES
        goto fallback;
#endif
        if(dx1 != dx2 && dy1 != dy2) {
          // This is a rectangle conflict
          int dy(dy1 + dy2);
          int dx(dx1 + dx2);

          // Make sure ai is the horizontal agent.
          if(dx2)
            std::swap(ai, aj);

          // Find the start positions
          int stH(monotoneSubchainStart(dy, dx, ai, t));
          int stV(monotoneSubchainStart(dy, dx, aj, t));

          int sH(agentPosition(pathfinders[ai], stH));
          int sV(agentPosition(pathfinders[aj], stV));

          // If there is overhang, adjust the locations.
          while(true) {
            if(dx * (col_of(sV) - col_of(sH)) < 0) {
              ++stV;
              sV = agentPosition(pathfinders[aj], stV);
              continue;
            }
            if(dy * (row_of(sH) - row_of(sV)) < 0) {
              ++stH;
              sH = agentPosition(pathfinders[ai], stH);
              continue;
            }
            break;
          }

          int etH(monotoneSubchainEnd(dy, dx, ai, t));
          int etV(monotoneSubchainEnd(dy, dx, aj, t));

          int eH(agentPosition(pathfinders[ai], etH));
          int eV(agentPosition(pathfinders[aj], etV));

          while(true) {
            if(dx * (col_of(eH) - col_of(eV)) < 0) {
              --etV;
              eV = agentPosition(pathfinders[aj], etV);
              continue;
            }
            if(dy * (row_of(eV) - row_of(eH)) < 0) {
              --etH;
              eH = agentPosition(pathfinders[ai], etH);
              continue;
            }
            break;
          }
          assert(stH <= t);
          assert(stV <= t);
          assert(t <= etH);
          assert(t <= etV);
          assert(dy * (row_of(sH) - row_of(sV)) >= 0);
          assert(dx * (col_of(sV) - col_of(sH)) >= 0);
          assert(dy * (row_of(eV) - row_of(eH)) >= 0);
          assert(dx * (col_of(eH) - col_of(eV)) >= 0);

          int locS(ml->linearize_coordinate(row_of(sV), col_of(sH)));
          int locE(ml->linearize_coordinate(row_of(eV), col_of(eH)));
          int t0(stH - abs(row_of(sH) - row_of(locS)));
          assert(t0 == stV - abs(col_of(sV) - col_of(locS)));
          new_conflicts.push(conflict::barrier(t0, ai, aj, locS, locE));
        } else {
#ifdef MAPF_NO_RECTANGLES
        fallback:
#endif
          new_conflicts.push(conflict(t, ai, nmap[loc], loc, -1));
        }

        clear_map(this, cmap, t-1);
        clear_map(this, nmap, t);
        return true;
      }
      nmap[loc] = ai;
      if(cmap[loc] > 0 && cmap[loc] != ai) {
        // Get the new location of the agent we're replacing.
        int rloc = agentPosition(pathfinders[cmap[loc]], t);
        if(cmap[rloc] == ai) {
          // Edge conflict
          new_conflicts.push(conflict(t-1, ai, cmap[loc], loc, rloc));
          clear_map(this, cmap, t-1);
          clear_map(this, nmap, t);
          return true;
        }
      }
    }
    // Now we zero out the previous cmap.
    clear_map(this, cmap, t-1);
  }
  clear_map(this, nmap, pMax-1);

  return false;
}
#else
// Multiple-conflict handling
bool MAPF_Solver::checkForConflicts(void) {
  int pMax = maxPathLength();

  for(int ai = 0; ai < pathfinders.size(); ++ai) {
    int loc = pathfinders[ai]->getPath()[0];
    assert(nmap[loc] < 0); // Shouldn't be any conflicts at t0.
    nmap[loc] = ai;
  }
  // All agents are interesting.
  agent_set.sz = pathfinders.size();

  for(int t = 1; t <= pMax; ++t) {
    std::swap(cmap, nmap);

    // for(int ai = 0; ai < pathfinders.size(); ++ai) {
    for(int ai : agent_set.rev()) {
      int loc = agentPosition(pathfinders[ai], t);
      if(nmap[loc] >= 0) {
        // Already occupied.
        int aj(nmap[loc]);
        int dy1 = row_of(agentPosition(pathfinders[ai], t)) - row_of(agentPosition(pathfinders[ai], t-1));
        int dx1 = col_of(agentPosition(pathfinders[ai], t)) - col_of(agentPosition(pathfinders[ai], t-1));
        int dy2 = row_of(agentPosition(pathfinders[aj], t)) - row_of(agentPosition(pathfinders[aj], t-1));
        int dx2 = col_of(agentPosition(pathfinders[aj], t)) - col_of(agentPosition(pathfinders[aj], t-1));
#ifdef MAPF_NO_RECTANGLES
        goto fallback;
#endif
        if(dx1 != dx2 && dy1 != dy2) {
          // This is a rectangle conflict
          int dy(dy1 + dy2);
          int dx(dx1 + dx2);

          // Make sure ai is the horizontal agent.
          if(dx2)
            std::swap(ai, aj);

          // Find the start positions
          int stH(monotoneSubchainStart(dy, dx, ai, t));
          int stV(monotoneSubchainStart(dy, dx, aj, t));

          int sH(agentPosition(pathfinders[ai], stH));
          int sV(agentPosition(pathfinders[aj], stV));

          // If there is overhang, adjust the locations.
          while(true) {
            if(dx * (col_of(sV) - col_of(sH)) < 0) {
              ++stV;
              sV = agentPosition(pathfinders[aj], stV);
              continue;
            }
            if(dy * (row_of(sH) - row_of(sV)) < 0) {
              ++stH;
              sH = agentPosition(pathfinders[ai], stH);
              continue;
            }
            break;
          }

          int etH(monotoneSubchainEnd(dy, dx, ai, t));
          int etV(monotoneSubchainEnd(dy, dx, aj, t));

          int eH(agentPosition(pathfinders[ai], etH));
          int eV(agentPosition(pathfinders[aj], etV));

          while(true) {
            if(dx * (col_of(eH) - col_of(eV)) < 0) {
              --etV;
              eV = agentPosition(pathfinders[aj], etV);
              continue;
            }
            if(dy * (row_of(eV) - row_of(eH)) < 0) {
              --etH;
              eH = agentPosition(pathfinders[ai], etH);
              continue;
            }
            break;
          }
          assert(stH <= t);
          assert(stV <= t);
          assert(t <= etH);
          assert(t <= etV);
          assert(dy * (row_of(sH) - row_of(sV)) >= 0);
          assert(dx * (col_of(sV) - col_of(sH)) >= 0);
          assert(dy * (row_of(eV) - row_of(eH)) >= 0);
          assert(dx * (col_of(eH) - col_of(eV)) >= 0);

          int locS(ml->linearize_coordinate(row_of(sV), col_of(sH)));
          int locE(ml->linearize_coordinate(row_of(eV), col_of(eH)));
          int t0(stH - abs(row_of(sH) - row_of(locS)));
          assert(t0 == stV - abs(col_of(sV) - col_of(locS)));
          new_conflicts.push(conflict::barrier(t0, ai, aj, locS, locE));
        } else {
#ifdef MAPF_NO_RECTANGLES
        fallback:
#endif
          new_conflicts.push(conflict(t, ai, nmap[loc], loc, -1));
        }

        clear_map(this, cmap, t-1);
        clear_map(this, nmap, t);
        agent_set.remove(ai);
        // return true;
        continue;
      }
      nmap[loc] = ai;
      if(cmap[loc] > 0 && cmap[loc] != ai) {
        // Get the new location of the agent we're replacing.
        int rloc = agentPosition(pathfinders[cmap[loc]], t);
        if(cmap[rloc] == ai) {
          // Edge conflict
          new_conflicts.push(conflict(t-1, ai, cmap[loc], loc, rloc));
          clear_map(this, cmap, t-1);
          clear_map(this, nmap, t);
          agent_set.remove(ai);
          // return true;
          continue;
        }
      }
    }
    // Now we zero out the previous cmap.
    clear_map(this, cmap, t-1);
  }
  clear_map(this, nmap, pMax);

  return new_conflicts.size() > 0;
}

#endif

bool MAPF_Solver::checkBarrierViolated(int ai, int t, int p, int delta, int dur) const {
  assert(t >= 0);
  const vec<int>& P(pathfinders[ai]->getPath());
  for(int dt = 0; dt < dur; ++dt, ++t) {
    if(P[t] == p)
      return true;
    p += delta;
  }
  return false;
}

//enum BarrierDir { UP = 0, LEFT = 1, DOWN = 2, RIGHT = 3 };
static int barrier_dx[4] = { 0, -1, 0, 1 };
static int barrier_dy[4] = { -1, 0, 1, 0 };

geas::patom_t MAPF_Solver::getBarrier(int ai, BarrierDir dir, int t, int p, int dur) {
  // If this barrier is trivially violated, just return F.
  int delta = ml->cols * barrier_dy[dir] + barrier_dx[dir];
  assert(t >= 0);
  if(t == 0 && p == pathfinders[ai]->engine.start_location)
    return geas::at_False;

  // Project the direction back in the appropriate direction, to see what set of barriers we're in.
  int p_ident;
  int t0;
  switch(dir) {
    case UP:
      p_ident = col_of(p);
      t0 = t - row_of(p);
      break;
    case LEFT:
      p_ident = row_of(p);
      t0 = t - col_of(p);
      break;
    case DOWN:
      p_ident = col_of(p);
      t0 = t - row_of(p);
      break;
    case RIGHT:
    default:
      p_ident = row_of(p);
      t0 = t - col_of(p);
      break;
  }
  barrier_key k { ai, dir, p_ident, t0 };
  auto it(barrier_map.find(k));
  int idx;
  if(it != barrier_map.end()) {
    idx = (*it).second;
  } else {
    idx = barriers.size();
    barriers.push();
    barrier_map.insert(std::make_pair(k, idx));
  }

  // Check if this barrier has already been generated.
  vec<barrier_data>& candidates(barriers[idx]);
  for(const barrier_data& b : candidates) {
    if(b.start == t && b.duration == dur) {
      // fprintf(stderr, "%%%% HIT!\n");
      return b.act;
    }
  }
  // If not, we'll create it.
  geas::patom_t act(s.new_boolvar());
  // Set up entailment relationships.
#ifdef BARRIER_ENTAIL
  int end = t + dur;
  for(const barrier_data& b : candidates) {
    if(b.start <= t && end <= b.start + b.duration) {
      // b subsumes this.
      // fprintf(stderr, "%% Found super-barrier\n");
      add_clause(s.data, ~b.act, act);
    }
    if(t <= b.start && b.start + b.duration <= end) {
      // fprintf(stderr, "%% Found sub-barrier\n");
      add_clause(s.data, act, ~b.act);
    }
  }
#endif
  pathfinders[ai]->register_barrier(act, t, p, delta, dur);
  candidates.push(barrier_data { act, t, dur });
  return act;
}

bool MAPF_Solver::addConflict(void) {
  HL_conflicts++;
  for(auto new_conflict : new_conflicts) {
    if(new_conflict.type == C_BARRIER) {
      int aH(new_conflict.a1);
      int aV(new_conflict.a2);
      int p_s(new_conflict.b.s_loc);
      int p_e(new_conflict.b.e_loc);

      vec<geas::clause_elt> barrier_atoms;

      // Entry barrier starts at top-left
      int s_time(new_conflict.timestamp);
      // fprintf(stderr, "%% Adding rectangle [%d] (%d, %d) -> (%d, %d)\n", s_time, row_of(p_s), col_of(p_s), row_of(p_e), col_of(p_e));
      int dt(std::min(0, s_time));
      int h_dur(1 + abs(row_of(p_e) - row_of(p_s)));
      int h_delta(row_of(p_s) < row_of(p_e) ? ml->cols : -ml->cols);

      // assert(checkBarrierViolated(aH, s_time - dt, p_s - dt*h_delta, h_delta, h_dur + dt));
      BarrierDir dH(row_of(p_s) < row_of(p_e) ? DOWN : UP);
      if(s_time > 0 || pathfinders[aH]->engine.start_location != p_s - dt*h_delta) {
        barrier_atoms.push(getBarrier(aH, dH, s_time - dt, p_s - dt*h_delta, h_dur + dt));
        b_metadata.push({aH, s_time - dt, h_dur + dt, p_s, p_e, dH, VERTICAL, ENTRY});
      }

      int eh_start(ml->linearize_coordinate(row_of(p_s), col_of(p_e)));
      int eh_time(s_time + abs(col_of(p_e) - col_of(p_s)));
      barrier_atoms.push(getBarrier(aH, dH, eh_time - dt, eh_start - dt*h_delta, h_dur+dt));
      b_metadata.push({aH, eh_time - dt, h_dur + dt, p_s, p_e, dH, VERTICAL, EXIT});

      int v_dur(1 + abs(col_of(p_e) - col_of(p_s)));
      int v_delta(col_of(p_s) < col_of(p_e) ? 1 : -1);
      BarrierDir dV(col_of(p_s) < col_of(p_e) ? RIGHT : LEFT);

      if(s_time > 0 || pathfinders[aV]->engine.start_location != p_s - dt*v_delta) {
        barrier_atoms.push(getBarrier(aV, dV, s_time - dt, p_s - dt*v_delta, v_dur+dt));
        b_metadata.push({aV, s_time - dt, v_dur + dt, p_s, p_e, dV, HORIZONTAL, ENTRY});
      }

      int ev_start(ml->linearize_coordinate(row_of(p_e), col_of(p_s)));
      int ev_time(s_time + abs(row_of(p_e) - row_of(p_s)));
      barrier_atoms.push(getBarrier(aV, dV, ev_time - dt, ev_start - dt*v_delta, v_dur+dt));
      b_metadata.push({aV, ev_time - dt, v_dur + dt, p_s, p_e, dV, HORIZONTAL, EXIT});

      // One of the barriers must be active
      add_clause(*s.data, barrier_atoms);

      /*
      if(new_conflict.timestamp == 0) {
        patom_t sel(s.new_boolvar());
        // Set up the horizontal exit barrier
        int h_start(ml->linearize_coordinate(row_of(p_h), col_of(p_e)));
        int h_time(abs(col_of(p_h) - col_of(p_e)));
        int h_dur(1 + abs(row_of(p_h) - row_of(p_e)));
        int h_delta(row_of(p_h) < row_of(p_e) ? ml->cols : -ml->cols);

        pathfinders[new_conflict.a1]->register_barrier(sel, h_time, h_start, h_delta, h_dur);

        // And repeat the same for the vertical exit barrier
        int v_start(ml->linearize_coordinate(row_of(p_e), col_of(p_v)));
        int v_time(abs(row_of(p_v) - row_of(p_e)));
        int v_dur(1 + abs(col_of(p_v) - col_of(p_e)));
        int v_delta(col_of(p_h) < col_of(p_e) ? 1 : -1);

        pathfinders[new_conflict.a2]->register_barrier(~sel, v_time, v_start, v_delta, v_dur);
      } else {
        // Need the entry and exit barriers
        // FIXME: Build a table of barriers, so we can re-use them between conflicts.
        patom_t s1(s.new_boolvar());
        patom_t e1(s.new_boolvar());
        patom_t s2(s.new_boolvar());
        patom_t e2(s.new_boolvar());

        // Entry barrier starts at top-left
        int p_s(ml->linearize_coordinate(row_of(p_v), col_of(p_h)));
        int s_time(new_conflict.timestamp - abs(row_of(p_s) - row_of(p_h)));
        int dt(std::min(0, s_time));

        int h_dur(1 + abs(row_of(p_e) - row_of(p_s)));
        int h_delta(row_of(p_s) < row_of(p_e) ? ml->cols : -ml->cols);

        pathfinders[new_conflict.a1]->register_barrier(s1, s_time - dt, p_s - dt*h_delta, h_delta, h_dur - dt);

        int eh_start(ml->linearize_coordinate(row_of(p_s), col_of(p_e)));
        int eh_time(s_time + abs(col_of(p_e) - col_of(p_s)));

        pathfinders[new_conflict.a1]->register_barrier(e1, eh_time - dt, eh_start - dt*h_delta, h_delta, h_dur - dt);

        int v_dur(1 + abs(col_of(p_e) - col_of(p_s)));
        int v_delta(col_of(p_s) < col_of(p_e) ? 1 : -1);

        pathfinders[new_conflict.a2]->register_barrier(s2, s_time - dt, p_s - dt*v_delta, v_delta, v_dur - dt);

        int ev_start(ml->linearize_coordinate(row_of(p_e), col_of(p_s)));
        int ev_time(s_time + abs(row_of(p_e) - row_of(p_s)));

        pathfinders[new_conflict.a2]->register_barrier(e2, ev_time - dt, ev_start - dt*v_delta, v_delta, v_dur - dt);

        // One of the barriers must be active
        add_clause(s.data, s1, e1, s2, e2);
      }
        */
    } else {
      int loc1 = new_conflict.p.loc1;
      int loc2 = new_conflict.p.loc2;
      if(loc2 > loc1)
        std::swap(loc1, loc2);

      cons_key k { new_conflict.timestamp, loc1, loc2 };
      auto it(cons_map.find(k));

      int idx;
      if(it != cons_map.end()) {
        idx = (*it).second;
      } else {
        idx = constraints.size();
        cons_map.insert(std::make_pair(k, idx));
        constraints.push(cons_data { s.new_intvar(0, pathfinders.size()-1), btset::bitset(pathfinders.size()) });
      }

      int a1(new_conflict.a1);
      int a2(new_conflict.a2);
      cons_data& c(constraints[idx]);
      if(!c.attached.elem(a1)) {
        patom_t at(c.sel != a1);
        while(s.level() > 0 && at.lb(s.data->ctx()))
          s.backtrack();
        pathfinders[a1]->register_obstacle(at, k.timestamp, k.loc1, k.loc2);
        c.attached.insert(a1);
      }
      if(!c.attached.elem(a2)) {
        patom_t at(c.sel != a2);
        while(s.level() > 0 && at.lb(s.data->ctx()))
          s.backtrack();
        pathfinders[a2]->register_obstacle(at, k.timestamp, k.loc1, k.loc2);
        c.attached.insert(a2);
      }
      // FIXME: Abstract properly
      //s.data->confl.pred_saved[c.sel.p>>1].val = geas::from_int((rand() % 2 ? a1 : a2));
    }
  }
  new_conflicts.clear();
  return true;
}

/*
Null integer-encoded locations must be negative.

Null time must be negative

Null cost must be negative

Agent must be non-negative

If time is non-negative, forbidden is true and at least one location is not nullish, Lazy CBS will return a path where the provided agent does not visit the given location(s).
*/
vec<mapf::patom_t> MAPF_Solver::createAssumption(int agent, tuple<int, int> locations, int time, int cost, bool forbidden) {
  vec<mapf::patom_t> output;
  if (cost > 0) {
    patom_t costAssumption = pathfinders[agent]->cost <= cost;
    output.push(costAssumption);
  }
  int loc1 = get<0>(locations);
  int loc2 = get<1>(locations);
  if (time >= 0 && !(loc1 == -1 && loc2 != -1)) {
    cons_key k { time, loc1, loc2 };
    auto it(cons_map.find(k));
    int idx;
    if(it != cons_map.end()) {
      idx = (*it).second;
    } else {
      idx = constraints.size();
      cons_map.insert(std::make_pair(k, idx));
      constraints.push(cons_data { s.new_intvar(0, pathfinders.size() - 1), btset::bitset(pathfinders.size()) });
    }
    cons_data& c(constraints[idx]);
    if (forbidden) {
      patom_t at(c.sel != agent);
      if (!c.attached.elem(agent)) {
        while (s.level() > 0 && at.lb(s.data->ctx())) s.backtrack();
        pathfinders[agent]->register_obstacle(at, time, loc1, loc2);
        c.attached.insert(agent);
      }
      output.push(at);
    } else {
      patom_t at(c.sel == agent);
      output.push(at);
    }
  }
  return output;
}

bool MAPF_Solver::processCore(vec<geas::patom_t>& core) {
  // If the core is empty, we're unsatisfiable.
  // Shouldn't usually happen, since everyone can just wait at the
  // start state.
  if(core.size() == 0)
    return false;

  // Collect info for each element of the core.
  vec<int> idxs;
  uint64_t Dmin = UINT64_MAX;

  for(geas::patom_t c : core) {
    // Every assumption should be in the table.
    int c_idx = (*penalty_table.find(c.pid)).second;
    assert(c.val > penalties[c_idx].lb);
    uint64_t c_delta = c.val - penalties[c_idx].lb;
    Dmin = std::min(Dmin, c_delta);
    idxs.push(c_idx);
  }
  // We can increase the lower bound by Dmin.
  // Introduce the new penalty terms, and increase the existing bounds by Dmin.
  vec<int> coeffs(idxs.size(), 1);
  for(uint64_t d = 1; d <= Dmin; ++d) {
    vec<geas::patom_t> slice;
    for(int ci : idxs) {
      slice.push(geas::ge_atom(penalties[ci].p, penalties[ci].lb + d));
    }
    // New penalty var.
    intvar p(s.new_intvar(0, slice.size()-1));
    geas::bool_linear_ge(s.data, geas::at_True, p, coeffs, slice, -1);
    penalty_table.insert(std::make_pair(p.p, penalties.size()));
    penalties.push(penalty { p.p, geas::from_int(0) });
  }

  //cost_lb += Dmin;
  for(int ci : idxs) {
    penalties[ci].lb += Dmin;
  }
  return Dmin;
}

bool MAPF_Solver::runUCIter(void) {
  /*
  for(penalty& p : penalties)  {
    geas::patom_t at(geas::le_atom(p.p, p.lb));
    if(!s.assume(at))
      return false;
    if(s.is_aborted())
      throw SolveAborted { };
  }
  */

//retry:
  switch(s.solve()) {
    // No solution, we've got a new core.
    case geas::solver::UNSAT:
      return false;
    // Candidate optimal solution. Check for conflicts
    case geas::solver::SAT:
      // extractPaths();
      //if(!checkForConflicts())
        return true;
      //if(!addConflict())
      //  return false;
      //goto retry;
    case geas::solver::UNKNOWN:
      throw SolveAborted { };
      // GEAS_ERROR;
  }
  // UNREACHABLE
  return false;
}

MAPF_Solver::~MAPF_Solver(void) {

}

/*
The constarints are in the form (agent, ((row1, column1), (row2, column2)), time, cost).

Null locations must have both row and column negative.

Null time must be negative

Null cost must be negative

Agent must be non-negative

If time is negative and at least one location is not nullish, Lazy CBS will return a path where the provided agent does not visit the given location(s).
*/
bool MAPF_MinCost(MAPF_Solver& mapf, vector<tuple<int, tuple<tuple<int, int>, tuple<int, int>>, int ,int>> assumptions) {
  // Reset any existing assumptions
  mapf.s.clear_assumptions();

  vec<MAPF_Solver::penalty> penalties;
  std::unordered_map<geas::pid_t, int> penalty_table;

  int cost_lb(0);
  for(Agent_PF* p : mapf.pathfinders) {
    cost_lb += p->pathCost();
    geas::pid_t id(p->cost.p);
    penalty_table.insert(std::make_pair(id, penalties.size()));
    penalties.push(MAPF_Solver::penalty { id, geas::from_int(p->pathCost()) });
  }
  int longterm_cost = cost_lb;
#ifdef DEBUG_UC
  fprintf(stderr, "%%%% Initial bound: %d\n", cost_lb);
#endif

  // insert timed constraints to assumps list
  vec<geas::patom_t> assumps;
  vec<mapf::patom_t> timeless_assumps;
  for (tuple<int, tuple<tuple<int, int>, tuple<int, int>>, int ,int> assumption: assumptions) {
    int agent1 = get<0>(assumption);
    tuple<tuple<int, int>, tuple<int, int>> locations = get<1>(assumption);
    tuple<int, int> location1 = get<0>(locations);
    int location1Encoded = mapf.loc_enc(location1);
    tuple<int, int> location2 = get<1>(locations);
    int location2Encoded = mapf.loc_enc(location2);
    int time1 = get<2>(assumption);
    int cost = get<3>(assumption);
    if (agent1 >= 0 && time1 >= 0 && (location1Encoded >= 0 || location2Encoded >= 0)) {
      vec<mapf::patom_t> assump_vec = mapf.createAssumption(agent1, {mapf.loc_enc(location1), mapf.loc_enc(location2)}, time1, cost, false);
      for (auto atom: assump_vec) {
        timeless_assumps.push(atom);
        assumps.push(atom);
      }
    }
  }
  for(MAPF_Solver::penalty& p : penalties)
    assumps.push(geas::le_atom(p.p, p.lb));

  vec<geas::patom_t> core;
  bool replan = true;
  while (replan) {
    printf("Replanning...\n");
    while (!mapf.buildPlan(assumps)) {
      core.clear();
      mapf.s.get_conflict(core);
      mapf.s.clear_assumptions();
      if(core.size() == 0) return false;
      // Look at the core elements.
      vec<int> idxs;
      uint64_t Dmin = UINT64_MAX;
      for(geas::patom_t c : core) {
        // Every assumption should be in the table.
        auto it = penalty_table.find(c.pid);
        if(it == penalty_table.end())
          continue;
        int c_idx = (*it).second;
        assert(c.val > penalties[c_idx].lb);
        uint64_t c_delta = c.val - penalties[c_idx].lb;
        Dmin = std::min(Dmin, c_delta);
        idxs.push(c_idx);
      }
      // We can increase the lower bound by Dmin.
      // Introduce the new penalty terms, and increase the existing bounds by Dmin.
      vec<int> coeffs(idxs.size(), 1);
      for(uint64_t d = 1; d <= Dmin; ++d) {
        vec<geas::patom_t> slice;
        for(int ci : idxs) {
          slice.push(geas::ge_atom(penalties[ci].p, penalties[ci].lb + d));
        }
        // New penalty var.
        intvar p(mapf.s.new_intvar(0, slice.size()-1));
        geas::bool_linear_ge(mapf.s.data, geas::at_True, p, coeffs, slice, -1);
        penalty_table.insert(std::make_pair(p.p, penalties.size()));
        penalties.push(MAPF_Solver::penalty { p.p, geas::from_int(0) });
      }
      // Update the existing penalties and bounds
      for(int ci : idxs) {
        penalties[ci].lb += Dmin;
      }
      cost_lb += Dmin;
      mapf.cost_lb = cost_lb;
    #ifdef DEBUG_UC
          fprintf(stderr, "%%%% Found core of size (%d), current lower bound %d\n", core.size(), cost_lb);
    #endif

      // Now set up the updated assumptions
      assumps.clear();
      // specific assumptions for violated timeless assumptions are stored in timeless_assumptions that are reinserted into assumps before each recomputation
      for (auto atom: timeless_assumps) {
        assumps.push(atom);
      }
      for(MAPF_Solver::penalty& p : penalties)
        assumps.push(geas::le_atom(p.p, p.lb));
    }
    // assert(!mapf.checkForConflicts());
    replan = false;
    for (tuple<int, tuple<tuple<int, int>, tuple<int, int>>, int ,int> assumption: assumptions) {
      int agent1 = get<0>(assumption);
      tuple<tuple<int, int>, tuple<int, int>> locations = get<1>(assumption);
      tuple<int, int> location1 = get<0>(locations);
      tuple<int, int> location2 = get<1>(locations);
      int time1 = get<2>(assumption);
      int cost = get<3>(assumption);
      // check if a timeless assumption is violated and add a specific time assumption
      if (agent1 >= 0 && !(mapf.loc_enc(location1) == -1 && mapf.loc_enc(location2) != -1) && time1 < 0) {
        for (int agent2 = 0; agent2 < mapf.pathfinders.size(); agent2++) {
          if (agent2 == agent1) {
            const vec<int> plan = mapf.pathfinders[agent2]->getPath();
            for (int time2 = 0; time2 < plan.size(); time2++) {
              // if the current time is equal to the long-term cost of the entire plan, the plan is decrelaed unfeasible.
              if (time2 == longterm_cost) {
                return false;
              }
              const int location = plan[time2];
              int row = mapf.row_of(location) - 1;
              int col = mapf.col_of(location) - 1;
              const bool intruded = mapf.eq_locs(location, location1) || mapf.eq_locs(location, location2);
              if (intruded) {
                mapf.timelessAssumptions.push({agent1, time2, location});
                tuple<int, int> locations2 = {location, -1};
                vec<mapf::patom_t> assump_vec = mapf.createAssumption(agent1, locations2, time2, cost, true);
                for (auto atom: assump_vec) {
                  timeless_assumps.push(atom);
                  assumps.push(atom);
                }
                printf("Agent %d cannot be at (%d, %d) at time %d\n", agent1, row, col, time2);
                replan = true;
              }
            }
          }
        }
      }
    }
  }
  // if no timeless assumption is violated and the plan is feasible, indicate success
  return true;
}

bool MAPF_MinMakespan(MAPF_Solver& mapf) {
  mapf.s.clear_assumptions();
  int makespan_lb(0);
  std::unordered_map<geas::pid_t, unsigned int> agent_table;
  unsigned int ai = 0;
  for(Agent_PF* p : mapf.pathfinders) {
    makespan_lb = std::max(makespan_lb, (int) p->pathCost());
    agent_table.insert(std::make_pair(p->cost.p, ai));
    ++ai;
  }

  vec<geas::patom_t> assumps;

  for(Agent_PF* p : mapf.pathfinders)
    assumps.push(p->cost <= makespan_lb);

  vec<geas::patom_t> core;
  mapf.cost_lb = makespan_lb;
  while(!mapf.buildPlan(assumps)) {
    core.clear();
    mapf.s.get_conflict(core);
    mapf.s.clear_assumptions();
    // Globally infeasible (somehow, probably should be unreachable).
    if(core.size() == 0)
      return false;
    // Otherwise, extract the new makespan from the core.
    int new_makespan(0);
    for(geas::patom_t at : core) {
      // Turn the core atoms back into bounds.
      auto it(agent_table.find(at.pid));
      assert(it != agent_table.end());
      const geas::intvar& x(mapf.pathfinders[(*it).second]->cost);
      new_makespan = std::max(new_makespan, x.lb_of_pval(at.val));
    }
    assert(new_makespan > makespan_lb);
    makespan_lb = mapf.cost_lb = new_makespan;
    assumps.clear();
    for(Agent_PF* p : mapf.pathfinders)
      assumps.push(p->cost <= makespan_lb);
  }

  return true;
}

bool MAPF_MaxDeadlines(MAPF_Solver& mapf, vec<unsigned int>& deadlines) {
  assert(mapf.pathfinders.size() == deadlines.size());
  mapf.s.clear_assumptions();
  int cost_lb(0);
  unsigned int ai = 0;
  // In the result, even values are thresholds,
  // odd values are counts.
  std::unordered_map<geas::pid_t, int> penalty_table;
  vec<bool> enforced(deadlines.size(), true);
  vec<MAPF_Solver::penalty> penalties;

  for(Agent_PF* p : mapf.pathfinders) {
    // If the deadline is infeasible, just add it to the set of penalties.
    if(p->pathCost() > deadlines[ai]) {
      cost_lb++;
      enforced[ai] = false;
    } else {
      geas::pid_t id(p->cost.p);
      penalty_table.insert(std::make_pair(id, ai<<1));
    }
  }

  // Initially, we only have thresholds.
  vec<geas::patom_t> assumps;
  for(int ai : irange(mapf.pathfinders.size())) {
    if(enforced[ai]) {
      const intvar& x(mapf.pathfinders[ai]->cost);
      assumps.push(x <= deadlines[ai]);
    }
  }

  vec<geas::patom_t> core;
  while(!mapf.buildPlan(assumps)) {
    core.clear();
    mapf.s.get_conflict(core);
    mapf.s.clear_assumptions();
    if(core.size() == 0)
      return false;
    // Look at the core elements.
    vec<int> idxs;
    uint64_t Dmin = UINT64_MAX;

    for(geas::patom_t c : core) {
      // Every assumption should be in the table.
      int c_tag = (*penalty_table.find(c.pid)).second;
      idxs.push(c_tag);
      if(!(c_tag & 1)) { // First bit is zero; original deadline.
        int ai(c_tag>>1);
        // If a deadline is in the core, we can relax by at most one step.
        Dmin = 1;
        enforced[ai] = 0;
      } else {
        int p_idx(c_tag>>1);
        assert(c.val > penalties[p_idx].lb);
        uint64_t c_delta = c.val - penalties[p_idx].lb;
        Dmin = std::min(Dmin, c_delta);
      }
    }
    // We can increase the lower bound by Dmin.
    // Introduce the new penalty terms, and increase the existing bounds by Dmin.
    vec<int> coeffs(idxs.size(), 1);
    for(uint64_t d = 1; d <= Dmin; ++d) {
      vec<geas::patom_t> slice;
      for(int c_tag : idxs) {
        if(!(c_tag & 1)) {
          int ai(c_tag>>1);
          slice.push(mapf.pathfinders[ai]->cost > deadlines[ai]);
        } else {
          int ci(c_tag>>1);
          slice.push(geas::ge_atom(penalties[ci].p, penalties[ci].lb + d));
          penalties[ci].lb += Dmin;
        }
      }
      // New penalty var.
      intvar p(mapf.s.new_intvar(0, slice.size()-1));
      geas::bool_linear_ge(mapf.s.data, geas::at_True, p, coeffs, slice, -1);
      penalty_table.insert(std::make_pair(p.p, penalties.size()));
      penalties.push(MAPF_Solver::penalty { p.p, geas::from_int(0) });
    }
    // Update the total cost.
    cost_lb += Dmin;

    // Now set up the updated assumptions
    assumps.clear();
    int ai = 0;
    for(Agent_PF* p : mapf.pathfinders) {
      if(enforced[ai])
        assumps.push(p->cost <= deadlines[ai]);
      ++ai;
    }
    for(MAPF_Solver::penalty& p : penalties)
      assumps.push(geas::le_atom(p.p, p.lb));
  }
  return true;
}

}

ext: cc


text: #include <boost/mpl/aux_/has_begin.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 *
 * Copyright (c) 2003
 * John Maddock
 *
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 */

 /*
  *   LOCATION:    see http://www.boost.org for most recent version.
  *   FILE         regex_traits.hpp
  *   VERSION      see <boost/version.hpp>
  *   DESCRIPTION: Declares regular expression traits classes.
  */

#ifndef BOOST_REGEX_TRAITS_HPP_INCLUDED
#define BOOST_REGEX_TRAITS_HPP_INCLUDED

#ifndef BOOST_REGEX_CONFIG_HPP
#include <boost/regex/config.hpp>
#endif
#ifndef BOOST_REGEX_WORKAROUND_HPP
#include <boost/regex/v4/regex_workaround.hpp>
#endif
#ifndef BOOST_REGEX_SYNTAX_TYPE_HPP
#include <boost/regex/v4/syntax_type.hpp>
#endif
#ifndef BOOST_REGEX_ERROR_TYPE_HPP
#include <boost/regex/v4/error_type.hpp>
#endif
#ifndef BOOST_REGEX_TRAITS_DEFAULTS_HPP_INCLUDED
#include <boost/regex/v4/regex_traits_defaults.hpp>
#endif
#ifndef BOOST_NO_STD_LOCALE
#  ifndef BOOST_CPP_REGEX_TRAITS_HPP_INCLUDED
#     include <boost/regex/v4/cpp_regex_traits.hpp>
#  endif
#endif
#if !BOOST_WORKAROUND(__BORLANDC__, < 0x560)
#  ifndef BOOST_C_REGEX_TRAITS_HPP_INCLUDED
#     include <boost/regex/v4/c_regex_traits.hpp>
#  endif
#endif
#if defined(_WIN32) && !defined(BOOST_REGEX_NO_W32)
#  ifndef BOOST_W32_REGEX_TRAITS_HPP_INCLUDED
#     include <boost/regex/v4/w32_regex_traits.hpp>
#  endif
#endif
#ifndef BOOST_REGEX_FWD_HPP_INCLUDED
#include <boost/regex_fwd.hpp>
#endif

#include "boost/mpl/has_xxx.hpp"
#include <boost/static_assert.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4103)
#endif
#ifdef BOOST_HAS_ABI_HEADERS
#  include BOOST_ABI_PREFIX
#endif
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

namespace boost{

template <class charT, class implementationT >
struct regex_traits : public implementationT
{
   regex_traits() : implementationT() {}
};

//
// class regex_traits_wrapper.
// this is what our implementation will actually store;
// it provides default implementations of the "optional"
// interfaces that we support, in addition to the
// required "standard" ones:
//
namespace re_detail{
#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !BOOST_WORKAROUND(__HP_aCC, < 60000)
BOOST_MPL_HAS_XXX_TRAIT_DEF(boost_extensions_tag)
#else
template<class T>
struct has_boost_extensions_tag
{
   BOOST_STATIC_CONSTANT(bool, value = false);
};
#endif

template <class BaseT>
struct default_wrapper : public BaseT
{
   typedef typename BaseT::char_type char_type;
   std::string error_string(::boost::regex_constants::error_type e)const
   {
      return ::boost::re_detail::get_default_error_string(e);
   }
   ::boost::regex_constants::syntax_type syntax_type(char_type c)const
   {
      return ((c & 0x7f) == c) ? get_default_syntax_type(static_cast<char>(c)) : ::boost::regex_constants::syntax_char;
   }
   ::boost::regex_constants::escape_syntax_type escape_syntax_type(char_type c)const
   {
      return ((c & 0x7f) == c) ? get_default_escape_syntax_type(static_cast<char>(c)) : ::boost::regex_constants::escape_type_identity;
   }
   int toi(const char_type*& p1, const char_type* p2, int radix)const
   {
      return ::boost::re_detail::global_toi(p1, p2, radix, *this);
   }
   char_type translate(char_type c, bool icase)const
   {
      return (icase ? this->translate_nocase(c) : this->translate(c));
   }
   char_type translate(char_type c)const
   {
      return BaseT::translate(c);
   }
   char_type tolower(char_type c)const
   {
      return ::boost::re_detail::global_lower(c);
   }
   char_type toupper(char_type c)const
   {
      return ::boost::re_detail::global_upper(c);
   }
};

template <class BaseT, bool has_extensions>
struct compute_wrapper_base
{
   typedef BaseT type;
};
#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !BOOST_WORKAROUND(__HP_aCC, < 60000)
template <class BaseT>
struct compute_wrapper_base<BaseT, false>
{
   typedef default_wrapper<BaseT> type;
};
#else
template <>
struct compute_wrapper_base<c_regex_traits<char>, false>
{
   typedef default_wrapper<c_regex_traits<char> > type;
};
#ifndef BOOST_NO_WREGEX
template <>
struct compute_wrapper_base<c_regex_traits<wchar_t>, false>
{
   typedef default_wrapper<c_regex_traits<wchar_t> > type;
};
#endif
#endif

} // namespace re_detail

template <class BaseT>
struct regex_traits_wrapper
   : public ::boost::re_detail::compute_wrapper_base<
               BaseT,
               ::boost::re_detail::has_boost_extensions_tag<BaseT>::value
            >::type
{
   regex_traits_wrapper(){}
private:
   regex_traits_wrapper(const regex_traits_wrapper&);
   regex_traits_wrapper& operator=(const regex_traits_wrapper&);
};

} // namespace boost

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4103)
#endif
#ifdef BOOST_HAS_ABI_HEADERS
#  include BOOST_ABI_SUFFIX
#endif
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif // include

max_stars_repo_name: multiprecision/samegame

max_issues_repo_name: isuhao/klib

max_forks_repo_name: isuhao/klib


text:
#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>

#include "utility/watchdog.h"
#include "str_util.h"
#include "common_test_utils.h"
#include <iostream>
#include <atomic>

using namespace reinforcement_learning;

constexpr int timeout = 200;
constexpr int fail_timeout = timeout * 2;
constexpr int safe_timeout = timeout / 8;

BOOST_AUTO_TEST_CASE(watchdog_fail_after_registration) {
  utility::watchdog watchdog(nullptr);
  watchdog.start(nullptr);

  // Verify it begins not in an error state.
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);

  std::this_thread::sleep_for(std::chrono::milliseconds(fail_timeout));

  // Verify after waiting with no registered threads there is still no error.
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);

  watchdog.register_thread(std::this_thread::get_id(), "Test thread 1", timeout);

  std::this_thread::sleep_for(std::chrono::milliseconds(fail_timeout));
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), true);
}

BOOST_AUTO_TEST_CASE(watchdog_unregister) {
  utility::watchdog watchdog(nullptr);
  watchdog.start(nullptr);

  watchdog.register_thread(std::this_thread::get_id(), "Test thread 1", timeout);
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);
  watchdog.unregister_thread(std::this_thread::get_id());
  std::this_thread::sleep_for(std::chrono::milliseconds(fail_timeout));
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);
}

BOOST_AUTO_TEST_CASE(watchdog_fail_after_several_iterations) {
  if (is_invoked_with("valgrind"))
  {
    // this test depends on clock timeouts, can't guarantee test success under valgrind
    std::cout << "skipping watchdog_fail_after_several_iterations test when running in valgrind" << std::endl;
    return;
  }

  utility::watchdog watchdog(nullptr);
  watchdog.start(nullptr);

  watchdog.register_thread(std::this_thread::get_id(), "Test thread 1", timeout);
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);

  for(auto i = 0; i < 5; i++) {
    watchdog.check_in(std::this_thread::get_id());
    BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);
    std::this_thread::sleep_for(std::chrono::milliseconds(safe_timeout));
  }

  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);
  std::this_thread::sleep_for(std::chrono::milliseconds(fail_timeout));
  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), true);
}

BOOST_AUTO_TEST_CASE(watchdog_report_with_error_handler) {
  std::atomic<int> atomic_counter;
  atomic_counter.store(0);

  auto const error_fn = [](const api_status&, void* arg) {
    auto counter = static_cast<std::atomic<int>*>(arg);
    counter->fetch_add(1);
  };

  error_callback_fn err_func(error_fn, &atomic_counter);

  utility::watchdog watchdog(&err_func);
  watchdog.start(nullptr);

  watchdog.register_thread(std::this_thread::get_id(), "Test thread 1", timeout);

  std::this_thread::sleep_for(std::chrono::milliseconds(fail_timeout));
  BOOST_CHECK_GT(atomic_counter.load(), 0);
}

BOOST_AUTO_TEST_CASE(watchdog_multiple_threads) {
  if (is_invoked_with("valgrind"))
  {
    // this test depends on clock timeouts, can't guarantee test success under valgrind
    std::cout << "skipping watchdog_multiple_threads test when running in valgrind" << std::endl;
    return;
  }
  utility::watchdog watchdog(nullptr);
  watchdog.start(nullptr);

  std::vector<std::thread> threads;
  const auto num_threads = 2;
  const auto num_iterations = 10;
  threads.reserve(num_threads);
  for(auto i = 0 ; i < num_threads; i++) {
    threads.emplace_back([&, i]() {
      watchdog.register_thread(std::this_thread::get_id(), utility::concat("Test thread ", i), timeout);

      for (auto j = 0; j < num_iterations; j++) {
        watchdog.check_in(std::this_thread::get_id());
        std::this_thread::sleep_for(std::chrono::milliseconds(safe_timeout));
      }

      watchdog.unregister_thread(std::this_thread::get_id());
    });
  }

  BOOST_CHECK_EQUAL(watchdog.has_background_error_been_reported(), false);

  for(auto& t : threads) {
    if(t.joinable()) {
      t.join();
    }
  }
}