#include <algorithm>
#include <chrono>
#include <cmath>
#include <exception>
#include <map>
#include <string>
#include <sstream>
#include <thread>
#include <utility>
#include <vector>

#include <boost/log/trivial.hpp>
#include <casadi/casadi.hpp>
#include <yavl-cpp/yavl.h>

#include "mimir/algorithm/PursePlannerFormulation.hpp"
#include "mimir/algorithm/PursePlanner.hpp"
#include "mimir/StateMachineFwd.hpp"
#include "mimir/FkinDds.hpp"

// ReaderLifespan
#include <dds/core/detail/ProprietaryApi.hpp>
#include <dds/sub/AnyDataReader.hpp>
#include <dds/pub/AnyDataWriter.hpp>

#ifdef MIMIR_WITH_GNUPLOT
#include "gnuplot-iostream.h"
#include <tuple>
#endif

namespace mimir
{
  namespace algorithm
  {

    class PursePlanner::Impl
    {
    public:
      Impl(
          const YAML::Node& conf,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber)
      {

        const YAML::Node config = conf["config"];
        const YAML::Node schema = conf["schema"];
        YAVL::Validator checkNode(schema, config);

        if(!checkNode.validate())
        {
          std::stringstream oss;
          oss << checkNode.get_errors();

          BOOST_LOG_TRIVIAL(debug) << "PursePlanner config:\n" << config;
          BOOST_LOG_TRIVIAL(debug) << "PursePlanner schema:\n" << schema;

          throw YAML::Exception(YAML::Mark(), oss.str());
        }

        // set initial condition for planner state vector
        x_0 = casadi::DM(config["initial_condition"]["x0"].as<std::vector<double>>());

        // Function for mapping gps to local coordinate system
        toNED = define_gps_to_ned();

        // === Create instance of the purse planner nlp formulation ===
        BOOST_LOG_TRIVIAL(trace) << "Instantiate formulation";
        formulation.reset(new PursePlannerFormulation(conf));

        decide_idx = formulation->nlp_builder.decision_parameter_slice;
        state1_idx = formulation->nlp_builder.state_slice.at("x_1");
        state2_idx = formulation->nlp_builder.state_slice.at("x_2");
        input1_idx = formulation->nlp_builder.input_slice.at("x_1");
        input2_idx = formulation->nlp_builder.input_slice.at("x_2");
        x1_dim = formulation->nlp_builder.state_dimension.at("x_1");
        x2_dim = formulation->nlp_builder.state_dimension.at("x_2");
        v_dim = formulation->nlp_builder.decision_parameter_dimension;

        if (x_0.nnz() != x2_dim)
        {
          BOOST_LOG_TRIVIAL(error)
           << "Mismatching dimensions: x_0 in config vs. expected from formulation: x_0:"
           << x_0.nnz() << " vs. " << x2_dim;
        }

        // === DDS subscribers for parameters defined in YAML config ===
        setup_parameters(config, subscriber);

        // === DDS subscribers for inputs defined in YAML config ===
        setup_inputs(config, subscriber);

        // === DDS publishers for outputs defined in YAML config ===
        setup_outputs(config, publisher);

        // === Enable gnuplot debug plotting ===
        plot_on = config["settings"]["plot"].as<bool>();

#ifdef MIMIR_WITH_GNUPLOT
        if(plot_on)
        {
          gnuplot //<< "set yrange [-200:200]\n"
                  << "set title 'PursePlanner'\n"
                  << "set ylabel 'Various [-]'\n"
                  << "set xlabel 'Time [s]'\n";

          ne_plot << "set title 'Map plot'\n"
                  << "set size square\n"
                  << "set size ratio -1\n"
                  << "set ylabel 'North [m]'\n"
                  << "set xlabel 'East [m]'\n";
        }
#endif

      }
      ~Impl() {}
      std::map<std::string, dds::sub::AnyDataReader> parameter_inputs;
      std::map<std::string, dds::sub::AnyDataReader> input_readers;
      std::map<std::string, dds::pub::AnyDataWriter> output_writers;
      std::map<std::string, casadi::Slice>
        decide_idx, param_idx, input1_idx, input2_idx, state1_idx, state2_idx;
      std::unique_ptr<PursePlannerFormulation> formulation;
      std::string outputId;              ///< DDS outputs identifier
      casadi::DM parameters;             ///< Parameters p
      casadi::DM x_0, x_k;               ///< State at t_0 and t_k
      double t_k;                        ///< Simulation time of x_k [seconds]
      casadi::DM T, X, Tu, U;            ///< Solution trajectories
      std::vector<double> lamb_g;        ///< Lagrange multipliers: inequality constraints
      casadi::DM gps_origin;             ///< GPS coordinate of NED origin reference frame
      casadi::DM T1, X1;                 ///< To be removed
      bool plot_on;                      ///< To plot with gnuplot or not
      casadi::Function toNED;
      casadi_int x1_dim, x2_dim, v_dim, p_dim;
#ifdef MIMIR_WITH_GNUPLOT
      Gnuplot gnuplot, ne_plot;
#endif
    private:

      casadi::Function define_gps_to_ned()
      {
        using casadi::SX;
        typedef std::vector<std::string> vstr;
        typedef std::vector<SX> SXvec;

        double PI180 = std::atan(1)*4./180.;

        SX lat = SX::sym("lat");
        SX lon = SX::sym("lon");
        SX h = SX::sym("height");

        SX mu = lat*PI180;
        SX l = lon*PI180;

        // Fossen 2002 p.42
        SX re = 6378137; // Equatorial radius of ellipsoid
        SX rp = 6356752; // Polar axis radius of ellipsoid

        SX re2 = re*re;
        SX rp2 = rp*rp;

        SX N = (re2)/(sqrt(re2*pow(cos(mu),2) + rp2*pow(sin(mu),2)));

        // ECEF
        SX xe = (N + h)*cos(mu)*cos(l);
        SX ye = (N + h)*cos(mu)*sin(l);
        SX ze = ((rp2/re2)*N + h)*sin(mu);

        SX ecef = vertcat(xe, ye, ze);

        casadi::Function toECEF("toECEF", SXvec{lat,lon,h}, SXvec{ecef});

        SX lat0 = SX::sym("lat0");
        SX lon0 = SX::sym("lon0");
        SX h0 = SX::sym("h0");

        SX cosl = cos(lon*PI180);
        SX sinl = sin(lon*PI180);
        SX cosmu = cos(lat*PI180);
        SX sinmu = sin(lat*PI180);

        SX Tne = SX::zeros(3,3);
        Tne(0,0) = -cosl*sinmu;
        Tne(0,1) = -sinl;
        Tne(0,2) = -cosl*cosmu;
        Tne(1,0) = -sinl*sinmu;
        Tne(1,1) = cosl;
        Tne(1,2) = -sinl*cosmu;
        Tne(2,0) = cosmu;
        Tne(2,2) = -sinmu;

        SX diff = toECEF(SXvec{lat,lon,0})[0] - toECEF(SXvec{lat0,lon0, 0})[0];

        auto ned = mtimes(Tne.T(), diff);

        auto ret = casadi::Function("toNED",
         {vertcat(lat, lon), vertcat(lat0, lon0)},
         {ned},
         vstr{"GPS", "GPS0"}, vstr{"NED"});

        /*
        typedef std::vector<double> vdob;
        auto res = ret(casadi::DMDict{{"GPS", casadi::DM(vdob{63.45917,    10.3683367})},
                                      {"GPS0", casadi::DM(vdob{63.4581027, 10.3683367})}});
        BOOST_LOG_TRIVIAL(info) << "Zero transform is: " << res["NED"];
        */

        return ret;

      }

      void setup_parameters(const YAML::Node& config, dds::sub::Subscriber subscriber)
      {
        BOOST_LOG_TRIVIAL(trace) << "setup parameters";
        typedef std::vector<double> vdob;
        using casadi::Slice;
        auto readerQos = subscriber.default_datareader_qos();
        const auto config_param = config["parameters"];

        p_dim = formulation->nlp_builder.parameter_dimension;
        parameters = casadi::DM::zeros(p_dim, 1);
        param_idx = formulation->nlp_builder.parameter_slice;

        parameters(param_idx.at("setting_speed_U_v"))   = config_param["setting_speed_U_v"]["default"].as<double>();
        parameters(param_idx.at("setting_radii"))       = config_param["setting_radii"]["default"].as<vdob>();
        parameters(param_idx.at("aim_distance_D_s"))    = config_param["aim_distance_D_s"]["default"].as<double>();
        parameters(param_idx.at("fish_margin_d_f"))     = config_param["fish_margin_d_f"]["default"].as<double>();
        parameters(param_idx.at("fish_depth_z_s"))      = config_param["fish_depth_z_s"]["default"].as<double>();
        parameters(param_idx.at("leadline_tau_ll_z_d")) = config_param["leadline_tau_ll_z_d"]["default"].as<vdob>();
        parameters(param_idx.at("sink_margin_z_min"))   = config_param["sink_margin_z_min"]["default"].as<double>();

        auto add_parameter_IdVec1d =
         [=](const std::string& variable){

           auto topicName = config_param[variable]["topic"].as<std::string>();
           auto id = config_param[variable]["id"].as<std::string>();
           parameter_inputs.emplace(
               variable,
               dds::sub::DataReader<fkin::IdVec1d>(
                   subscriber,
                   dds::topic::ContentFilteredTopic<fkin::IdVec1d>(
                       dds::topic::Topic<fkin::IdVec1d>(
                           subscriber.participant(),
                           topicName),
                       topicName + id,
                       dds::topic::Filter("id = %0", {id})),
                   readerQos));
         };

        auto add_parameter_IdVec2d =
         [=](const std::string& variable){

           auto topicName = config_param[variable]["topic"].as<std::string>();
           auto id = config_param[variable]["id"].as<std::string>();
           parameter_inputs.emplace(
               variable,
               dds::sub::DataReader<fkin::IdVec2d>(
                   subscriber,
                   dds::topic::ContentFilteredTopic<fkin::IdVec2d>(
                       dds::topic::Topic<fkin::IdVec2d>(
                           subscriber.participant(),
                           topicName),
                       topicName + id,
                       dds::topic::Filter("id = %0", {id})),
                   readerQos));
         };

        auto add_parameter_Double1 =
         [=](const std::string& variable){

           auto topicName = config_param[variable]["topic"].as<std::string>();
           parameter_inputs.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::DoubleVal>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::DoubleVal>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_parameter_Double2 =
         [=](const std::string& variable){

           auto topicName = config_param[variable]["topic"].as<std::string>();
           parameter_inputs.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::Double2>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::Double2>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_parameter_Double3 =
         [=](const std::string& variable){

           auto topicName = config_param[variable]["topic"].as<std::string>();
           parameter_inputs.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::Double3>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::Double3>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        // These are subscribed parameters for which we read from
        add_parameter_IdVec1d("setting_speed_U_v");
        add_parameter_Double2("current_surface");
        add_parameter_IdVec2d("setting_radii");
        add_parameter_IdVec1d("aim_distance_D_s");
        add_parameter_IdVec1d("fish_margin_d_f");
        add_parameter_Double2("fish_velocity_over_ground");
        add_parameter_Double2("current_fish");
        add_parameter_IdVec1d("fish_depth_z_s");
        add_parameter_IdVec2d("leadline_tau_ll_z_d");
        add_parameter_IdVec1d("sink_margin_z_min");

        // some parameters are not read directly from DDS, they are calculated based on other

        auto current_surface = casadi::DM(config_param["current_surface"]["default"].as<vdob>());
        double W = double(norm_2(current_surface));
        double alpha = std::atan2(
            double(current_surface(1)),
            double(current_surface(0))); // atan2(v_E, v_N)

        parameters(param_idx.at("current_speed_surface_W")) = W;
        parameters(param_idx.at("current_dir_surface_alpha")) = alpha;

        auto fish_NED_velocity =
         casadi::DM(config_param["fish_velocity_over_ground"]["default"].as<vdob>());
        auto current_fish =
         casadi::DM(config_param["current_fish"]["default"].as<vdob>());

        double V_s = double(norm_2(fish_NED_velocity));
        double chi_s = std::atan2(
            double(fish_NED_velocity(1)),
            double(fish_NED_velocity(0))); // atan2(v_sE, v_sN)

        // fish course in water frame (surface current)
        double psi_s = atan2(V_s*sin(chi_s)-W*sin(alpha),V_s*cos(chi_s)-W*cos(alpha));

        parameters(param_idx.at("fish_speed_NE_V_s")) = V_s;
        parameters(param_idx.at("fish_course_NED_chi_s")) = chi_s;
        parameters(param_idx.at("fish_course_WF_psi_s")) = psi_s;
      }

      void setup_inputs(const YAML::Node& config, dds::sub::Subscriber subscriber)
      {
        BOOST_LOG_TRIVIAL(trace) << "setup inputs";
        // inputs:
        // - GPS origin as Double2
        // - Vessel pos vel: PosInfo
        // - Vessel heading rot: GyroInfo
        // - Vessel water speed? LogInfo

        // - Fish pos vel: PosInfo
        // - Fish relative Double3
        // - Keep as Bit

        typedef std::vector<double> vdob;
        using casadi::Slice;
        auto readerQos = subscriber.default_datareader_qos();
        const auto config_input = config["inputs"];

        auto add_input_Bit =
         [=](const std::string& variable){

           auto topicName = config_input[variable]["topic"].as<std::string>();
           input_readers.emplace(
               variable,
               dds::sub::DataReader<fkin::Bit>(
                   subscriber,
                   dds::topic::Topic<fkin::Bit>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_input_Double2 =
         [=](const std::string& variable){

           auto topicName = config_input[variable]["topic"].as<std::string>();
           input_readers.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::Double2>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::Double2>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_input_Double3 =
         [=](const std::string& variable){

           auto topicName = config_input[variable]["topic"].as<std::string>();
           input_readers.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::Double3>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::Double3>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_input_PosInfo =
         [=](const std::string& variable){

           auto topicName = config_input[variable]["topic"].as<std::string>();
           input_readers.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::PosInfo>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::PosInfo>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        auto add_input_GyroInfo =
         [=](const std::string& variable){

           auto topicName = config_input[variable]["topic"].as<std::string>();
           input_readers.emplace(
               variable,
               dds::sub::DataReader<ratatosk::types::GyroInfo>(
                   subscriber,
                   dds::topic::Topic<ratatosk::types::GyroInfo>(
                       subscriber.participant(),
                       topicName),
                   readerQos));
         };

        add_input_Double2("GPS_origin");
        add_input_PosInfo("vessel_pos_info");
        add_input_GyroInfo("vessel_gyro_info");
        add_input_PosInfo("fish_pos_info");
        add_input_Double3("fish_relative_pos");
        add_input_Bit("keep_solution");

        gps_origin = casadi::DM(config_input["GPS_origin"]["default"].as<vdob>());

        // If sample with limited duration is to be used:
        //auto input_age = input_config["max_age_ms"].as<std::int64_t>();
        //readerQos << org::opensplice::core::policy::ReaderLifespan(
        //true,
        //    dds::core::Duration::from_millisecs(input_age));

      }

      void setup_outputs(const YAML::Node& config, dds::pub::Publisher publisher)
      {
        BOOST_LOG_TRIVIAL(trace) << "setup outputs";
        auto writerQos = publisher.default_datawriter_qos();
        outputId = config["outputs"]["id"].as<std::string>();

        output_writers.emplace(
            "vessel_speed",
            dds::pub::DataWriter<ratatosk::types::DoubleVal>(
                publisher,
                dds::topic::Topic<ratatosk::types::DoubleVal>(
                    publisher.participant(),
                    config["outputs"]["vessel_speed"]["topic"].as<std::string>()),
                writerQos));

        output_writers.emplace(
            "vessel_course_rate",
            dds::pub::DataWriter<ratatosk::types::DoubleVal>(
                publisher,
                dds::topic::Topic<ratatosk::types::DoubleVal>(
                    publisher.participant(),
                    config["outputs"]["vessel_course_rate"]["topic"].as<std::string>()),
                writerQos));

        output_writers.emplace(
            "deploy_position",
            dds::pub::DataWriter<ratatosk::types::Double2>(
                publisher,
                dds::topic::Topic<ratatosk::types::Double2>(
                    publisher.participant(),
                    config["outputs"]["deploy_position"]["topic"].as<std::string>()),
                writerQos));

        output_writers.emplace(
            "collide_position",
            dds::pub::DataWriter<ratatosk::types::Double2>(
                publisher,
                dds::topic::Topic<ratatosk::types::Double2>(
                    publisher.participant(),
                    config["outputs"]["collide_position"]["topic"].as<std::string>()),
                writerQos));

        output_writers.emplace(
            "deploy_time",
            dds::pub::DataWriter<ratatosk::types::DoubleVal>(
                publisher,
                dds::topic::Topic<ratatosk::types::DoubleVal>(
                    publisher.participant(),
                    config["outputs"]["deploy_time"]["topic"].as<std::string>()),
                writerQos));

        // x(t) for t: [t0, tf] for vessel
        output_writers.emplace(
            "trajectory_vessel",
            dds::pub::DataWriter<fkin::BatchKinematics2D>(
                publisher,
                dds::topic::Topic<fkin::BatchKinematics2D>(
                    publisher.participant(),
                    config["outputs"]["trajectory_vessel"]["topic"].as<std::string>()),
                writerQos));

        // rate of turn for t: [t0, tf] for vessel
        output_writers.emplace(
            "trajectory_vessel_rot",
            dds::pub::DataWriter<fkin::BatchIdVec1d>(
                publisher,
                dds::topic::Topic<fkin::BatchIdVec1d>(
                    publisher.participant(),
                    config["outputs"]["trajectory_vessel_rot"]["topic"].as<std::string>()),
                writerQos));

        // x(t) for t: [t0, tf] for fish
        output_writers.emplace(
            "trajectory_fish",
            dds::pub::DataWriter<fkin::BatchKinematics2D>(
                publisher,
                dds::topic::Topic<fkin::BatchKinematics2D>(
                    publisher.participant(),
                    config["outputs"]["trajectory_fish"]["topic"].as<std::string>()),
                writerQos));

        // nlp configuration info
        output_writers.emplace(
            "nlp_config",
            dds::pub::DataWriter<fkin::NlpConfig>(
                publisher,
                dds::topic::Topic<fkin::NlpConfig>(
                    publisher.participant(),
                    config["outputs"]["nlp_config"]["topic"].as<std::string>()),
                writerQos));

        // nlp statistics
        output_writers.emplace(
            "nlp_stats",
            dds::pub::DataWriter<fkin::OptiStats>(
                publisher,
                dds::topic::Topic<fkin::OptiStats>(
                    publisher.participant(),
                    config["outputs"]["nlp_stats"]["topic"].as<std::string>()),
                writerQos));

      }
    };

    PursePlanner::PursePlanner(
        const YAML::Node& config,
        boost::statechart::fifo_scheduler<> & scheduler,
        boost::statechart::fifo_scheduler<>::processor_handle machine,
        dds::pub::Publisher publisher,
        dds::sub::Subscriber subscriber) :
      m_impl( new PursePlanner::Impl(config, publisher, subscriber)),
      m_scheduler(scheduler),
      m_stateMachine(machine),
      m_time_step(std::chrono::milliseconds(
           config["config"]["settings"]["time_step_ms"].as<std::int32_t>())),
      m_next_step(std::chrono::steady_clock::now()),
      m_config(config),
      m_retries(10),
      m_keep_solution(false)
    {}

    void PursePlanner::solve(const std::atomic<bool>& cancel_token)
    {
      // solve() performs the following :
      // I.    Fetches DDS input and parameters, x(t0), p into local variables
      // II.   Shifts solution from previous step for warm starting optimization problem
      // III.  Sets initial condition for x(t0) in optimization problem
      // IV.   Solves the optimization problem
      // V.    Verify that the solve succeeded (publish stats) TODO: failure action
      // VI.   Updates solution in nlp problem data structure
      // VII.  Evaluates solution trajectory  (prepare for publish)
      // VIII. Publish solution trajectory and desired now

      using casadi::DM, casadi::DMDict;
      namespace sc = std::chrono;
      using namespace fkin;

      try
      {
        BOOST_LOG_TRIVIAL(trace) << "Solving " << name();

        // I. == Read DDS data =====================================================
        BOOST_LOG_TRIVIAL(debug) << "  I. read DDS data";
        // Data are put into data structures that are used further below

        read_inputs();      // Fetch inputs, sample and hold
        read_parameters();  // Fetch user-configurable parameters from DDS

        // Get constant parameters that are needed in output of BatchKinematics2D (fish course, speeds)
        double U_v = double(model()->parameters(model()->param_idx.at("setting_speed_U_v")));
        double fish_sog = double(model()->parameters(model()->param_idx.at("fish_speed_NE_V_s")));
        double W = double(model()->parameters(model()->param_idx.at("current_speed_surface_W")));
        double alpha = double(model()->parameters(model()->param_idx.at("current_dir_surface_alpha")));
        double D_s = double(model()->parameters(model()->param_idx.at("aim_distance_D_s")));
        double R_x = double(model()->parameters(model()->param_idx.at("setting_Rx")));
        double R_y = double(model()->parameters(model()->param_idx.at("setting_Ry")));
        double t_d = 0.; // set properly later

        // II. == Warm start ==================================================
        BOOST_LOG_TRIVIAL(debug) << "  II. warm start";
        auto nlp_input = DMDict{{"w", DM(model()->formulation->nlp_problem.x_init)}};

        auto d_param = model()->formulation->mpc.at("v").at("extractor")(nlp_input).at("v");
        auto sys_1 = model()->formulation->mpc.at("x_1").at("shifter")(nlp_input).at("w_out");
        auto sys_2 = model()->formulation->mpc.at("x_2").at("shifter")(nlp_input).at("w_out");

        model()->formulation->nlp_problem.x_init = vertcat(d_param, sys_1, sys_2).get_elements();

        // III. == Set variable guesses in NLP ==================================
        BOOST_LOG_TRIVIAL(debug) << "  III. set variable guesses in NLP";
        model()->formulation->set_variable("v", d_param);
        model()->formulation->set_variable("x_1_0", casadi::DM::zeros(model()->x1_dim, 1));
        model()->formulation->set_variable("x_2_0", model()->x_k);
        model()->t_k = sc::duration_cast<sc::seconds>(m_next_step - m_t0).count();

        DM t0_opt = DM(model()->t_k); // if formulation has explicit time else, 0 is ok
        //t0_opt(0) = 0; // Need to be tested more.

        // IV. == Solve NLP =====================================================
        BOOST_LOG_TRIVIAL(debug) << "  IV. solve NLP";
        model()->formulation->set_abort(cancel_token);

        BOOST_LOG_TRIVIAL(debug) << "Skip new solution: " << m_keep_solution;

        if (!m_keep_solution){

          const auto& prob = model()->formulation->nlp_problem;
          auto result = model()->formulation->nlp_solver(
              DMDict({{"x0", prob.x_init},
                      {"lbx", prob.x_lb},
                      {"ubx", prob.x_ub},
                      {"lbg", prob.g_lb},
                      {"ubg", prob.g_ub},
                      {"p", vertcat(model()->parameters, t0_opt, t0_opt)},
                      {"lam_x0", prob.lambda_init},
                      {"lam_g0", model()->lamb_g}}));

          // TODO: may need clever multi-evaluation
          // Maybe parallel evaluation with futures?

          // V. == Check status =================================================
          BOOST_LOG_TRIVIAL(debug) << "  V. check status";
          fkin::OptiStats opti_stats(model()->formulation->stats());
          opti_stats.obj() = double(result["f"]);
          opti_stats.id() = model()->outputId;
          opti_stats.p() = vertcat(model()->parameters, model()->t_k).get_elements();
          opti_stats.x0() = model()->x_k.get_elements();

          static_cast<dds::pub::DataWriter<fkin::OptiStats>>(
              model()->output_writers.at("nlp_stats"))->write(opti_stats);

          if (cancel_token)
          {
            BOOST_LOG_TRIVIAL(info) << "Solve was interrupted by user";
            event(new mimir::EvInterrupt());
            return;
          }

          if(!model()->formulation->nlp_solver.stats().at("success").as_bool())
          {
            BOOST_LOG_TRIVIAL(debug) << model()->formulation->nlp_solver.stats();

            if(--m_retries > 0) {
              // reset initial guess of Lagrange multipliers and decision parameters
              auto v_guess = casadi::DM::zeros(model()->v_dim, 1);
              model()->formulation->set_variable("v", v_guess);
              model()->lamb_g = std::vector<double>(model()->formulation->nlp_problem.g_lb.size(), 0);
              step_time();
              event(new mimir::EvReady());
              return;
            }

            // Max number of failed attempts in a row, giving up.
            event(new mimir::EvInterrupt());
            return;
          }
          else{
            m_retries = 10; // Reset retries to magic number
          }

          // VI. == Update nlp problem  =========================================
          BOOST_LOG_TRIVIAL(debug) << "  VI. update NLP problem";
          model()->formulation->nlp_problem.x_init = result["x"].get_elements();
          model()->formulation->nlp_problem.lambda_init = result["lam_x"].get_elements();
          model()->lamb_g = result["lam_g"].get_elements();
          const auto& nlp_output = model()->formulation->nlp_problem.x_init;

          // VII. == Set solution data  =========================================
          // (maybe unpacker + timegrid as optional for collocation)
          BOOST_LOG_TRIVIAL(debug) << "  VII. set solution data";

          auto decide_param =
           model()->formulation->mpc.at("v").at("extractor")(DMDict({{"w", nlp_output}})).at("v");

          auto unpacked_x1 =
           model()->formulation->mpc.at("x_1").at("unpacker")(DMDict({{"w", nlp_output}}));

          auto solT_x1 =
           model()->formulation->mpc.at("x_1").at("timegrid")(DMDict({{"t0", DM(model()->t_k)},
                                                                      {"v", decide_param}}));
          auto unpacked_x2 =
           model()->formulation->mpc.at("x_2").at("unpacker")(DMDict({{"w", nlp_output}}));

          auto solT_x2 =
           model()->formulation->mpc.at("x_2").at("timegrid")(DMDict({{"t0", DM(model()->t_k)},
                                                                      {"v", decide_param}}));

          BOOST_LOG_TRIVIAL(info) << "Decided parameters: " << decide_param;

          t_d = double(model()->formulation->mpc.at("x_2").at("Tf")(DMDict{{"w", nlp_output}}).at("Tf"));

          double tmp_h = std::pow(R_x - R_y, 2) / std::pow(R_x + R_y, 2);
          double Pi = 2*std::acos(0.0);
          // Ramanujan, second approximation:
          double ellipse_circum = Pi*(R_x + R_y)*(1 + 3*tmp_h/(10 + std::pow(4 - 3*tmp_h, 0.5)));

          double s_pd = 0.5*ellipse_circum; // We use half ellipse circumference approximation..

          // Time interval of interest to provide trajectories
          double t_f = ceil(t_d + (D_s + s_pd)/U_v); // t0 = 0, tf = t_pd = t_d + (D_s + s_pd)/Uv
          // We can use the trajectory function, since x_2 has variable step and no control input
          double deltaT = t_f/model()->formulation->N2;

          if (unpacked_x2["U"].nnz() > 0)
            throw std::runtime_error("PursePlanner assumes that x_2 subsystem has no input u");

          auto decide_param_traj = decide_param;
          decide_param_traj(model()->decide_idx.at("x_2_DT")) = deltaT;

          // Unused:
          /*auto trajx1 = model()->formulation->mpc.at("x_1").at("trajectory")(
            DMDict({{"U", unpacked_x1["U"]},
            {"X0", unpacked_x1["X0"]},
            {"p", model()->parameters},
            {"v", decide_param},
            {"t0", t0_opt}}));*/

          if(!(model()->formulation->nlp_config().technique() == "single-shooting"))
          {
            model()->x_k = unpacked_x2["Xc"](casadi::Slice(), 0); // TO BE REMOVED
            model()->T = solT_x2["T"];
            model()->X = horzcat(unpacked_x2["X0"], unpacked_x2["Xc"]);
            throw std::runtime_error("PursePlanner assumes the technique is single-shooting");
          }
          else
          {
            auto trajectory =
             model()->formulation->mpc.at("x_2").at("trajectory")(
                 DMDict({{"U", unpacked_x2["U"]},
                         {"X0", unpacked_x2["X0"]},
                         {"p", model()->parameters},
                         {"v", decide_param_traj},
                         {"t0", t0_opt}}));

            model()->x_k = trajectory["X"](casadi::Slice(), 0); // TO BE REMOVED
            model()->T = trajectory["T"];
            model()->X = trajectory["X"];

            BOOST_LOG_TRIVIAL(trace) << "T_traj: " << trajectory["T"];
            BOOST_LOG_TRIVIAL(trace) << "X_traj: " << trajectory["X"];
            BOOST_LOG_TRIVIAL(trace) << "Trajectory elements: " << trajectory["T"].nnz();

            // Update output
            model()->T1 = trajectory["T"];
            model()->X1 = trajectory["X"];

          }

          // Control input solution
          model()->U = unpacked_x2["U"];
          model()->Tu = solT_x2["Tu"];

          BOOST_LOG_TRIVIAL(trace) << "T: " << model()->T;
          BOOST_LOG_TRIVIAL(trace) << "Xc: " << unpacked_x2["Xc"];
          BOOST_LOG_TRIVIAL(trace) << "U: " << model()->U;
          BOOST_LOG_TRIVIAL(trace) << "Tu: " << model()->Tu;

        }

        // VIII. == Publish solution trajectory and desired now ================
        BOOST_LOG_TRIVIAL(debug) << "  VIII. publish solution trajectory and desired now";

        if(model()->T.nnz() > 0)
        {
          auto Tx = model()->T.get_elements();
          auto Tu = model()->Tu.get_elements();

          //auto vessel_rot_idx_u = casadi::Slice(0,1);
          fkin::BatchKinematics2D vessel_traj, fish_traj;
          fkin::BatchIdVec1d vessel_rot;//, vessel_acc;
          std::string id = model()->outputId;
          vessel_traj.id() = id;
          fish_traj.id() = id;
          vessel_rot.id() = id;
          vessel_traj.timestamps().reserve(Tx.size());
          vessel_rot.timestamps().reserve(Tx.size());
          fish_traj.timestamps().reserve(Tx.size());

          // Find the time point in solution closest to now
          // Use {Tx, Tu} resp. if a {state, input} variable is to be found
          auto t_expected = m_next_step - m_t0; // Expected time now in optimization
          auto t_now_sim_epoch = std::chrono::steady_clock::now() - m_t0; // Actual time now

          // First time in solution vector that are not less than t_now
          auto time_it = std::lower_bound(
              Tx.begin(), Tx.end(),
              sc::duration_cast<sc::seconds>(t_now_sim_epoch).count());

          // If it is found, publish vessel commands
          if(time_it != Tx.end()){

            // Index of the solution
            auto idx = std::distance(Tx.begin(), time_it);

            // Forward diff (f(idx+1) - f(idx))/deltaT
            auto idx_1 = idx+1;
            auto idx_2 = idx;
            if (static_cast<size_t>(idx + 1) == Tx.size()) {
              // Backward diff (f(idx) - f(idx-1))/deltaT
              idx_1 = idx;
              idx_2 = idx-1;
            }

            double rad_2_deg = 180./(2*std::acos(0.0)); // (180/pi)
            auto course_1 = double(model()->X(model()->state2_idx.at("vessel_course"), idx_1));
            auto course_2 = double(model()->X(model()->state2_idx.at("vessel_course"), idx_2));
            auto time_1 = Tx[idx_1];
            auto time_2 = Tx[idx_2];
            double vessel_course_rate = (course_1 - course_2)/(time_1 - time_2);
            double capped_turn_rate = std::fmin(
                std::fmax(double(vessel_course_rate), -model()->formulation->omega_max),
                model()->formulation->omega_max);
            capped_turn_rate *= rad_2_deg;

            double chi_v = double(model()->X(model()->state2_idx.at("vessel_course"), idx));
            double beta = asin((W/U_v)*sin(alpha - chi_v)); // Side slip.
            // speed over ground
            double vessel_sog = sqrt(U_v*U_v + W*W + 2*U_v*W*cos(chi_v - beta - alpha));

            BOOST_LOG_TRIVIAL(trace) << "Simulation time for commanded output: " << (*time_it);

            static_cast<dds::pub::DataWriter<ratatosk::types::DoubleVal>>(
                model()->output_writers.at("vessel_speed"))->write(
                    ratatosk::types::DoubleVal(vessel_sog));

            static_cast<dds::pub::DataWriter<ratatosk::types::DoubleVal>>(
                model()->output_writers.at("vessel_course_rate"))->write(
                    ratatosk::types::DoubleVal(capped_turn_rate));

            BOOST_LOG_TRIVIAL(debug) << "Commands are: " << vessel_sog << ", " << capped_turn_rate;

          }

          if(m_keep_solution)
          {
            step_time();
            event(new mimir::EvReady());
            return;
          }

          auto t_overtime = t_now_sim_epoch - t_expected; // Is the optimization late?
          // Find deployment position
          auto t_deploy = t_now_sim_epoch - t_overtime + sc::milliseconds(static_cast<int64_t>(t_d*1000));

          double t_late =  sc::duration_cast<sc::milliseconds>(t_overtime).count();
          BOOST_LOG_TRIVIAL(warning) << " Overtime: " << t_late;

          static_cast<dds::pub::DataWriter<ratatosk::types::DoubleVal>>(
              model()->output_writers.at("deploy_time"))->write(
                  ratatosk::types::DoubleVal(double(t_d - t_late/1000)));

          BOOST_LOG_TRIVIAL(debug) << "Deploy in: " << t_d - t_late/1000 << " sec";

          time_it = std::lower_bound(
              Tx.begin(), Tx.end(),
              sc::duration_cast<sc::seconds>(t_deploy).count());

          if(time_it != Tx.end()) {

            auto idx = std::distance(Tx.begin(), time_it);
            auto deploy_pos = model()->X(model()->state2_idx.at("vessel_NE"), idx);

            BOOST_LOG_TRIVIAL(debug) << "Deploy pos is: " << deploy_pos;

            static_cast<dds::pub::DataWriter<ratatosk::types::Double2>>(
                model()->output_writers.at("deploy_position"))->write(
                    ratatosk::types::Double2(double(deploy_pos(0)), double(deploy_pos(1))));

          }

          // Find collision point
          auto t_collide = t_deploy + sc::seconds(static_cast<int64_t>(D_s/U_v));

          time_it = std::lower_bound(
              Tx.begin(), Tx.end(),
              sc::duration_cast<sc::seconds>(t_collide).count());

          if(time_it != Tx.end()) {

            auto idx = std::distance(Tx.begin(), time_it);
            auto collide_pos = model()->X(model()->state2_idx.at("vessel_NE"), idx);

            static_cast<dds::pub::DataWriter<ratatosk::types::Double2>>(
                model()->output_writers.at("collide_position"))->write(
                    ratatosk::types::Double2(double(collide_pos(0)), double(collide_pos(1))));

          }

          auto end_idx = model()->X.size2();

          // Convert simulation time to wall clock time
          for (casadi_int i = 0; i < end_idx; ++ i) //auto& t : Tx)
          {
            auto steady_time = m_t0 + sc::milliseconds(static_cast<int64_t>(double(Tx[i])*1000));
            auto wall_time = (steady_time - m_t0 + m_t0_wall); // drifts if system_clock changes
            auto epoch_time = sc::duration_cast<sc::milliseconds>(wall_time.time_since_epoch()).count();
            vessel_traj.timestamps().push_back(fkin::Timestamp(epoch_time));
            fish_traj.timestamps().push_back(fkin::Timestamp(epoch_time));
            vessel_rot.timestamps().push_back(fkin::Timestamp(epoch_time));
          }

          // Calculate rate of turn with finite difference
          casadi_int n_elem = model()->X.size2();
          std::vector<casadi_int> last_row{n_elem - 1};
          std::vector<casadi_int> secnd_last_col{n_elem - 2};
          // Shifts one and keeps last element (band(n,1) is sub diagonal, so we transpose)
          casadi::DM shift_operator = casadi::DM(casadi::Sparsity::band(n_elem,1).T()
           + casadi::Sparsity::triplet(n_elem, n_elem, last_row, secnd_last_col));

          casadi::DM vessel_courses = model()->X(model()->state2_idx.at("vessel_course"),
           casadi::Slice()).T();
          casadi::DM courses_shifted = mtimes(shift_operator, vessel_courses);
          casadi::DM delta_courses = courses_shifted - vessel_courses;
          casadi::DM delta_times = mtimes(shift_operator, Tx) - Tx;
          casadi::DM vessel_turn_rate = delta_courses/delta_times;

          BOOST_LOG_TRIVIAL(trace) << "calc vessel_turn_rate are: " << vessel_turn_rate;

          // Loop over columns of X to fetch solution trajectory
          for(casadi_int i = 0; i < end_idx /* model()->X.size2() */; ++i)
          {
            casadi::DM stateX = model()->X(casadi::Slice(), i);

            casadi::DM vessel_NE(stateX(model()->state2_idx.at("vessel_NE")));
            casadi::DM fish_NE(stateX(model()->state2_idx.at("fish_NE")));

            casadi::DM vessel_course(stateX(model()->state2_idx.at("vessel_course")));

            double capped_turn_rate = std::fmin(
                std::fmax(double(vessel_turn_rate(i)), -model()->formulation->omega_max),
                model()->formulation->omega_max);

            double chi_v = double(vessel_course);
            double beta = asin((W/U_v)*sin(alpha - chi_v)); // Side slip.
            // speed over ground
            double vessel_sog = sqrt(U_v*U_v + W*W + 2*U_v*W*cos(chi_v - beta - alpha));

            vessel_traj.batch().push_back( Kinematics2D(
                 id,
                 Vector2d(double(vessel_NE(0)), double(vessel_NE(1))),
                 Vector1d(double(stateX(model()->state2_idx.at("vessel_course")))),
                 Vector1d(vessel_sog)));

            fish_traj.batch().push_back( Kinematics2D(
                 id,
                 Vector2d(double(fish_NE(0)), double(fish_NE(1))),
                 Vector1d(0.),
                 Vector1d(double(fish_sog))));

            vessel_rot.batch().push_back( IdVec1d(id, Vector1d(double(capped_turn_rate))) );

          }

          static_cast<dds::pub::DataWriter<fkin::BatchKinematics2D>>(
              model()->output_writers.at("trajectory_vessel"))->write(vessel_traj);

          static_cast<dds::pub::DataWriter<fkin::BatchKinematics2D>>(
              model()->output_writers.at("trajectory_fish"))->write(fish_traj);

          static_cast<dds::pub::DataWriter<fkin::BatchIdVec1d>>(
              model()->output_writers.at("trajectory_vessel_rot"))->write(vessel_rot);

        }

        plot(model()->plot_on);
        step_time();
        event(new mimir::EvReady());
      }
      catch (casadi::CasadiException &e)
      {
        BOOST_LOG_TRIVIAL(fatal) << "Casadi threw exception in " << name() << ": " << e.what();
        event(new mimir::EvError());
        throw;
      }
      catch (std::out_of_range &e)
      {
        BOOST_LOG_TRIVIAL(fatal) << "Out of bounds: " << name() << ": " << e.what();
        event(new mimir::EvError());
        throw;
      }
      catch (std::exception &e)
      {
        BOOST_LOG_TRIVIAL(fatal) << name() << ": " << e.what();
        event(new mimir::EvError());
        throw;
      }
      catch (...)
      {
        BOOST_LOG_TRIVIAL(fatal) << name() << " exception thrown";
        event(new mimir::EvError());
        throw;
      }
    }

    void PursePlanner::read_inputs()
    {
      using casadi::DM;
      using casadi::DMDict;
      typedef std::vector<double> vdob;

      auto readBit =
       [=](const std::string& key) -> std::pair<bool, bool>
       {
        auto samples = static_cast<dds::sub::DataReader<fkin::Bit>>(
            model()->input_readers.at(key))->read();
        if (samples.length() > 0){
          auto sample = (--samples.end());
          if(sample->info().valid()){
            return std::make_pair(
                true,
                sample->data().value());
          }
        }
        return std::make_pair(false, false);
       };

      auto readDouble2 =
       [=](const std::string& key) -> std::pair<bool, casadi::DM>
       {
         auto samples = static_cast<dds::sub::DataReader<ratatosk::types::Double2>>(
             model()->input_readers.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             return std::make_pair(
                 true,
                 casadi::DM(std::vector<double>{sample->data().x(), sample->data().y()}));
           }
         }
         return std::make_pair(false, casadi::DM());
       };

      /*
      auto readDouble3 =
       [=](const std::string& key) -> std::pair<bool, casadi::DM>
       {
        auto samples = static_cast<dds::sub::DataReader<ratatosk::types::Double3>>(
            model()->input_readers.at(key))->read();
        if (samples.length() > 0){
          auto sample = (--samples.end());
          if(sample->info().valid()){
            return std::make_pair(
                true,
                casadi::DM(std::vector<double>{
                   sample->data().x(), sample->data().y(), sample->data().z()}));
          }
        }
        return std::make_pair(false, casadi::DM());
       }; */

      auto readPosInfo =
       [=](const std::string& key) -> std::pair<bool, ratatosk::types::PosInfo>
       {
        auto samples = static_cast<dds::sub::DataReader<ratatosk::types::PosInfo>>(
            model()->input_readers.at(key))->read();
        if (samples.length() > 0){
          auto sample = (--samples.end());
          if(sample->info().valid()){

            return std::make_pair(true, sample->data());
          }
        }
        return std::make_pair(false, ratatosk::types::PosInfo());
       };

      auto readGyroInfo =
       [=](const std::string& key) -> std::pair<bool, ratatosk::types::GyroInfo>
       {
        auto samples = static_cast<dds::sub::DataReader<ratatosk::types::GyroInfo>>(
            model()->input_readers.at(key))->read();
        if (samples.length() > 0){
          auto sample = (--samples.end());
          if(sample->info().valid()){

            return std::make_pair(true, sample->data());
          }
        }
        return std::make_pair(false, ratatosk::types::GyroInfo());
       };

      auto resD2 = readDouble2("GPS_origin");
      if(resD2.first)
        model()->gps_origin = resD2.second;

      auto vesselPosInfo = readPosInfo("vessel_pos_info");
      auto fishPosInfo = readPosInfo("fish_pos_info");
      auto vesselGyroInfo = readGyroInfo("vessel_gyro_info");

      // States of path planner are defined in ::formulate_dynamics:

      casadi::DM xk = model()->x_k;
      BOOST_LOG_TRIVIAL(debug) << "STATE before: " << xk;

      double PI180 = 2.*std::acos(0.0)/180.; //std::atan(1)*4./180.;

      if(vesselPosInfo.first)
      {
        auto res = model()->toNED(
            DMDict{{"GPS", DM(vdob{vesselPosInfo.second.lat(), vesselPosInfo.second.lon()})},
                   {"GPS0", model()->gps_origin}}).at("NED");

        xk(model()->state2_idx.at("vessel_NE")) = res(casadi::Slice(0,2));
      }

      if(vesselGyroInfo.first)
      {
        // This may be wrong. Is true heading the same as course?
        xk(model()->state2_idx.at("vessel_course")) = vesselGyroInfo.second.hdt()*PI180;
      }

      if(fishPosInfo.first)
      {
        auto resPI = model()->toNED(
            DMDict{{"GPS", DM(vdob{fishPosInfo.second.lat(), fishPosInfo.second.lon()})},
                   {"GPS0", model()->gps_origin}}).at("NED");

        BOOST_LOG_TRIVIAL(trace) << "NED fish: " << resPI;
        xk(model()->state2_idx.at("fish_NE")) = resPI(casadi::Slice(0,2));
      }

      // Set water frame to origin
      xk(model()->state2_idx.at("water_NE")) = casadi::DM::zeros(2,1);

      BOOST_LOG_TRIVIAL(debug) << "STATE after: " << xk;

      model()->x_k = xk;

      // Parameters, which in reality are initial conditions..
      model()->parameters(model()->param_idx.at("vessel_NE_q0")) =
       xk(model()->state2_idx.at("vessel_NE"));

      model()->parameters(model()->param_idx.at("fish_NE_p_s0")) =
       xk(model()->state2_idx.at("fish_NE"));

      // Whether the user requests to keep the suggested trajectory or not.
      auto keep_solution = readBit("keep_solution");
      if (keep_solution.first)
        m_keep_solution = keep_solution.second;

    }

    void PursePlanner::read_parameters()
    {

      auto readIdVec1 =
       [=](const std::string& key){
         auto samples = static_cast<dds::sub::DataReader<fkin::IdVec1d>>(
             model()->parameter_inputs.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             model()->parameters(model()->param_idx.at(key)) = sample->data().vec().x();
           }
         }
       };

      auto readIdVec2 =
       [=](const std::string& key){
         auto samples = static_cast<dds::sub::DataReader<fkin::IdVec2d>>(
             model()->parameter_inputs.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             model()->parameters(model()->param_idx.at(key)) =
              casadi::DM(std::vector<double>{sample->data().vec().x(), sample->data().vec().y()});
           }
         }
       };

      /*
      auto readDouble1 =
       [=](const std::string& key){
         auto samples = static_cast<dds::sub::DataReader<ratatosk::types::DoubleVal>>(
             model()->parameter_inputs.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             model()->parameters(model()->param_idx.at(key)) = sample->data().val();
           }
         }
       };

      auto readDouble3 =
       [=](const std::string& key){
         auto samples = static_cast<dds::sub::DataReader<ratatosk::types::Double3>>(
             model()->parameter_inputs.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             model()->parameters(model()->param_idx.at(key)) =
              casadi::DM(std::vector<double>{
                   sample->data().x(), sample->data().y(), sample->data().z()});
           }
         }
       };
      */

      auto readDouble2 =
       [=](const std::string& key) -> std::pair<bool, casadi::DM>
       {
         auto samples = static_cast<dds::sub::DataReader<ratatosk::types::Double2>>(
             model()->parameter_inputs.at(key))->read();
         if (samples.length() > 0){
           auto sample = (--samples.end());
           if(sample->info().valid()){
             //model()->parameters(model()->param_idx.at(key)) =
              return std::make_pair(
                  true,
                  casadi::DM(std::vector<double>{sample->data().x(), sample->data().y()}));
           }
         }
         return std::make_pair(false, casadi::DM());
       };

      readIdVec1("setting_speed_U_v");
      readIdVec2("setting_radii");
      readIdVec1("aim_distance_D_s");
      readIdVec1("fish_margin_d_f");
      readIdVec1("fish_depth_z_s");
      readIdVec2("leadline_tau_ll_z_d");
      readIdVec1("sink_margin_z_min");

      auto surf_current = readDouble2("current_surface");
      auto fish_vog = readDouble2("fish_velocity_over_ground");
      auto fish_current = readDouble2("current_fish");

      if(surf_current.first)
      {
        double W = double(norm_2(surf_current.second));
        double alpha = std::atan2(
            double(surf_current.second(1)),
            double(surf_current.second(0))); // atan2(v_E, v_N)

        model()->parameters(model()->param_idx.at("current_speed_surface_W")) = W;
        model()->parameters(model()->param_idx.at("current_dir_surface_alpha")) = alpha;

      }

      if(fish_vog.first && fish_current.first)
      {
        auto current_fish = fish_current.second;
        auto fish_NED_velocity = fish_vog.second;

        double V_s = double(norm_2(fish_NED_velocity));
        double chi_s = std::atan2(
            double(fish_NED_velocity(1)),
            double(fish_NED_velocity(0))); // atan2(v_sE, v_sN)

        double W = double(model()->parameters(model()->param_idx.at("current_speed_surface_W")));
        double alpha = double(model()->parameters(model()->param_idx.at("current_dir_surface_alpha")));
        // fish course in water frame (surface current)
        double psi_s = atan2(V_s*sin(chi_s)-W*sin(alpha),V_s*cos(chi_s)-W*cos(alpha));

        model()->parameters(model()->param_idx.at("fish_speed_NE_V_s")) = V_s;
        model()->parameters(model()->param_idx.at("fish_course_NED_chi_s")) = chi_s;
        model()->parameters(model()->param_idx.at("fish_course_WF_psi_s")) = psi_s;

      }

      // Note: some parameters are set in set_input, because they depend on xk (initial conditions).

      BOOST_LOG_TRIVIAL(debug) << "Parameter vector is: " << model()->parameters;

    }

    void PursePlanner::initialize(const std::atomic<bool>&)
    {
      try
      {
        using namespace std::chrono_literals;
        BOOST_LOG_TRIVIAL(debug) << name() << " is initializing";
        std::this_thread::sleep_for(2s); // use waitset instead for inputs

        m_retries = 10; // Magic number of retries before giving up;
        model()->x_k = model()->x_0;
        model()->t_k = 0.;

        // TODO: Need check to confirm that required signals are available
        read_inputs(); // sets state vector x_k for planner formulation
        read_parameters(); // sets parameter vector for planner formulation

        m_t0 = std::chrono::steady_clock::now(); // https://stackoverflow.com/a/35282833
        m_t0_wall = std::chrono::system_clock::now();

        // Cold start parameter guesses
        auto v_guess = casadi::DM::zeros(model()->v_dim, 1);
        model()->formulation->set_variable("v", v_guess);
        model()->lamb_g = std::vector<double>(model()->formulation->nlp_problem.g_lb.size(), 0);
        model()->formulation->set_variable("x_1_0", casadi::DM::zeros(model()->x1_dim, 1));
        model()->formulation->set_variable("x_2_0", model()->x_k);

        // DDS Publish configured nlp settings
        static_cast<dds::pub::DataWriter<fkin::NlpConfig>>(
            model()->output_writers.at("nlp_config"))->write(model()->formulation->nlp_config());

        m_next_step = std::chrono::steady_clock::now();
        event(new mimir::EvReady());
      }
      catch (...)
      {
        event(new mimir::EvError());
        throw;
      }
    }

    void PursePlanner::timer(const std::atomic<bool>& cancel_token)
    {

      // There is no sanity checks in terms of real-timeliness of execution.
      // probably want external time point to wait until.
      typedef std::chrono::milliseconds scm;
      auto now = std::chrono::steady_clock::now();
      auto time_p = m_next_step;
      auto timeleft = std::chrono::duration_cast<scm>(time_p - now);
      BOOST_LOG_TRIVIAL(debug) << "Time left until next step: " << timeleft.count() << " ms";

      auto fraction = (m_time_step/100 > scm(0) ? m_time_step/100 :
       (m_time_step/50 > scm(0) ? m_time_step/50 :
        (m_time_step/25 > scm(0) ? m_time_step/25 :
         (m_time_step/10 > scm(0) ? m_time_step/10 :
          (m_time_step/2 > scm(0) ?  m_time_step/2  : scm(1))))));

      while(now < time_p)
      {
        auto diff = time_p - now;
        std::this_thread::sleep_for(diff < fraction ? diff : fraction);
        if(cancel_token)
          break;
        now = std::chrono::steady_clock::now();
      }

      if(cancel_token)
      {
        BOOST_LOG_TRIVIAL(trace) << "Timer Canceled";
        event(new mimir::EvInterrupt());
      }
      else
      {
        BOOST_LOG_TRIVIAL(trace) << "Timeout";
        event(new mimir::EvTimeout());
      }
    }

    void PursePlanner::event(boost::statechart::event_base * const event)
    {
      // who is responsible for the pointer?
      m_scheduler.queue_event(
          m_stateMachine,
          make_intrusive(event));
    }

    PursePlanner::~PursePlanner() = default;

    void PursePlanner::plot(bool do_plot)
    {
      if(!do_plot)
        return;

#ifdef MIMIR_WITH_GNUPLOT
      using casadi::Slice;
      model()->gnuplot << "plot '-' with lines ls 1 title 'chi'\n"//, "
                       //<< " '-' with steps ls 2 title 'psi_{dot}'\n";//, "
                       //<< " '-' with lines ls 3 title 'slack'\n";
      model()->gnuplot.send1d(std::make_tuple(
           model()->T.get_elements(),
           model()->X(model()->state2_idx.at("vessel_course"), Slice()).get_elements()));
      /*model()->gnuplot.send1d(std::make_tuple(
           model()->Tu.get_elements(),
           model()->U(model()->input2_idx.at("vessel_acc"), Slice()).get_elements()));*/
      /*model()->gnuplot.send1d(std::make_tuple(
           model()->Tu.get_elements(),
           model()->U(model()->input2_idx.at("slack_dist"), Slice()).get_elements()));*/

      model()->gnuplot.flush();

      model()->ne_plot << "plot '-' with lines ls 1 title 'vessel', "
                       << " '-' with lines ls 2 title 'fish'\n";
      model()->ne_plot.send1d(std::make_tuple(
           model()->X(model()->state2_idx.at("vessel_NE").start + 1, Slice()).get_elements(),
           model()->X(model()->state2_idx.at("vessel_NE").start, Slice()).get_elements()));
      model()->ne_plot.send1d(std::make_tuple(
           model()->X(model()->state2_idx.at("fish_NE").start + 1, Slice()).get_elements(),
           model()->X(model()->state2_idx.at("fish_NE").start, Slice()).get_elements()));

      model()->ne_plot.flush();

#else
      BOOST_LOG_TRIVIAL(trace) << "Gnuplot not available";
#endif
    }
  }
}


// Copyright 2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_
#define OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_

#include <common/types.hpp>
#include <optimization/visibility_control.hpp>
#include <optimization/utils.hpp>
#include <helper_functions/crtp.hpp>
#include <Eigen/Core>
#include <vector>
#include <cstddef>
#include <memory>
#include <tuple>
#include <utility>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace common
{
namespace optimization
{
/// CRTP base class for a mathematical expression.
/// \tparam Derived Implementation class. Must implement `score_(..)`, `jacobian_()` and
/// `hessian(..)`.
/// \tparam DomainValueT Parameter type.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
class OPTIMIZATION_PUBLIC Expression : public common::helper_functions::crtp<Derived>
{
public:
  static constexpr auto NumJacobianCols = NumJacobianColsT;
  static constexpr auto NumVars = NumVarsT;
  using DomainValue = DomainValueT;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
  using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
  using JacobianRef = Eigen::Ref<Jacobian>;
  using HessianRef = Eigen::Ref<Hessian>;

  /// Get the result of an expression for a given parameter value.
  /// \param x Parameter value
  /// \return Evaluated score
  Value operator()(const DomainValue & x)
  {
    return this->impl().score_(x);
  }

  /// Get the jacobian at a given parameter value.
  /// \param x Parameter value.
  /// \param out Evaluated jacobian matrix.
  void jacobian(const DomainValue & x, JacobianRef out)
  {
    this->impl().jacobian_(x, out);
  }

  /// Get the hessian at a given parameter value.
  /// \param x Parameter value.{
  /// \param out Evaluated hessian matrix.
  void hessian(const DomainValue & x, HessianRef out)
  {
    this->impl().hessian_(x, out);
  }

  /// Pre-compute and cache the score/jacobian/hessian values. Which terms are to be computed is
  /// specified with the `ComputeMode` argument. By default, this function does nothing. You can
  /// implement and hide this function in your implementation to allow for some caching behavior.
  /// Though it is encouraged to use `CachedExpression` which already handles a big portion of the
  /// cache state management.
  /// \param x Parameter value.
  /// \param mode Computation mode. Which terms (score, jacobian, hessian) are to be computed
  /// can be set within this element.
  void evaluate(const DomainValue & x, const ComputeMode & mode)
  {
    (void)x;
    (void)mode;
  }
};

/// Expression class for cases when pre-evaluating elements or computing elements together
/// may be more efficient. This class implements the necessary boilerplate to manage the cache
/// state book-keeping.
/// This class implements `score_(..)`, `jacobian_(..)` and `hessian_(..)`; hence the
/// implementation must only implement `evaluate_(..)`
/// \tparam Derived CRTP implementation class. This class should implement `evaluate_()`.
/// \tparam DomainValueT Parameter type.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
/// \tparam ComparatorT Comparator function for `DomainValueT`
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
  typename ComparatorT>
class CachedExpression : public
  Expression<CachedExpression<Derived, DomainValueT,
    NumJacobianColsT, NumVarsT, ComparatorT>,
    DomainValueT, NumJacobianColsT, NumVarsT>
{
public:
  static constexpr auto NumJacobianCols = NumJacobianColsT;
  static constexpr auto NumVars = NumVarsT;
  using DomainValue = DomainValueT;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
  using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
  using JacobianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumJacobianCols>>;
  using HessianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumVars>>;

  explicit CachedExpression(const ComparatorT & comparator = ComparatorT())
  : m_score{0.0}, m_jacobian(Jacobian::Zero()), m_hessian{Hessian::Zero()},
    m_cache_state(comparator) {}

  Value score_(const DomainValue & x)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
      evaluate(x, ComputeMode{}.set_score());
    }
    return m_score;
  }

  void jacobian_(const DomainValue & x, JacobianRef out)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::JACOBIAN)) {
      evaluate(x, ComputeMode{}.set_jacobian());
    }
    out = m_jacobian;
  }

  void hessian_(const DomainValue & x, HessianRef out)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
      evaluate(x, ComputeMode{}.set_hessian());
    }
    out = m_hessian;
  }

  void evaluate(const DomainValue & x, const ComputeMode & mode)
  {
    if (!mode.score() && !mode.jacobian() && !mode.hessian()) {
      return;
    }
    m_cache_state.update(x, mode);
    // Call the implementation method.
    static_cast<Derived *>(this)->evaluate_(x, mode);
  }

protected:
  void set_score(Value score)
  {
    m_score = score;
  }

  void set_jacobian(const JacobianRef & jacobian)
  {
    m_jacobian = jacobian;
  }

  void set_hessian(const HessianRef & hessian)
  {
    m_hessian = hessian;
  }

private:
  Value m_score;
  Jacobian m_jacobian;
  Hessian m_hessian;
  CacheStateMachine<DomainValue, ComparatorT> m_cache_state;
};

/// A generalized representation of an optimization problem. Constrained
/// implementation classes must implement the following methods:
/// score_opt_(x);
/// jacobian_opt_(x, out);
/// hessian_opt_(x, out);
/// \tparam Derived Implementation class
/// \tparam DomainValueT The type for parameter to be optimized for.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
/// \tparam ObjectiveT Objective implementation class.
/// \tparam EqualityConstraintsT A tuple of equality constraint expressions.
/// \tparam InequalityConstraintsT A tuple of inequality constraint expressions.
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
  typename ObjectiveT, typename EqualityConstraintsT, typename InequalityConstraintsT>
class OPTIMIZATION_PUBLIC OptimizationProblem;

// Definition of OptimizationProblem.
// Template specialization is used for type deduction and forwarding constraint parameter packs
// to the tuples. This way "std::tuple" is enforced to be used on the constrained
// OptimizationProblem implementations.
template<typename Derived,
  typename ... EqualityConstraints,
  typename ... InequalityConstraints,
  typename ObjectiveT, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
class OPTIMIZATION_PUBLIC OptimizationProblem<Derived, DomainValueT, NumJacobianColsT, NumVarsT,
    ObjectiveT,
    std::tuple<EqualityConstraints...>,
    std::tuple<InequalityConstraints...>>
  : public Expression<Derived, DomainValueT, NumJacobianColsT, NumVarsT>
{
public:
  using EqualityConstraintsT = std::tuple<EqualityConstraints...>;
  using InequalityConstraintsT = std::tuple<InequalityConstraints...>;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVarsT, NumJacobianColsT>;
  using Hessian = Eigen::Matrix<Value, NumVarsT, NumVarsT>;
  using JacobianRef = Eigen::Ref<Jacobian>;
  using HessianRef = Eigen::Ref<Hessian>;
  static constexpr bool8_t is_unconstrained{(std::tuple_size<EqualityConstraintsT>::value == 0U) &&
    (std::tuple_size<InequalityConstraintsT>::value == 0U)};

  OptimizationProblem(
    ObjectiveT && objective,
    EqualityConstraintsT && eq_constraints,
    InequalityConstraintsT && ineq_constraints
  )
  : m_objective(std::forward<ObjectiveT>(objective)),
    m_equality_constraints(std::forward<EqualityConstraintsT>(eq_constraints)),
    m_inequality_constraints(std::forward<InequalityConstraintsT>(ineq_constraints))
  {
  }

  template<typename ... Args, typename Dummy = void, typename = std::enable_if_t<
      is_unconstrained, Dummy>>
  OptimizationProblem(
    Args && ... args
  )
  : m_objective(std::forward<Args>(args)...)
  {
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, Value> score_(const DomainValueT & x)
  {
    return m_objective(x);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, Value> score_(const DomainValueT & x)
  {
    return opt_impl()->score_opt_(x);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
  {
    m_objective.jacobian(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
  {
    opt_impl()->jacobian_opt_(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> hessian_(const DomainValueT & x, HessianRef out)
  {
    m_objective.hessian(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> hessian_(const DomainValueT & x, HessianRef out)
  {
    opt_impl()->hessian_opt_(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> evaluate(
    const DomainValueT & x,
    const ComputeMode & mode)
  {
    m_objective.evaluate(x, mode);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> evaluate(
    const DomainValueT & x,
    const ComputeMode & mode)
  {
    opt_impl()->evaluate_(x, mode);
  }

protected:
  ObjectiveT & objective()
  {
    return m_objective;
  }

  EqualityConstraintsT & equality_constraints()
  {
    return m_equality_constraints;
  }

  InequalityConstraintsT & inequality_constraints()
  {
    return m_inequality_constraints;
  }

private:
  Derived * opt_impl()
  {
    return static_cast<Derived *>(this);
  }

  ObjectiveT m_objective;
  EqualityConstraintsT m_equality_constraints;
  InequalityConstraintsT m_inequality_constraints;
};

/// Convenience class for unconstrained optimization problems. This is a thin wrapper
/// around the objective expression.
/// \tparam ObjectiveT Objective of the optimization problem.
/// \tparam DomainValueT Parameter type.
/// \tparam NumVarsT Number of variables.
template<typename ObjectiveT, typename DomainValueT, int NumVarsT>
class UnconstrainedOptimizationProblem : public
  OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
    DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>
{
  using OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
    DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>::OptimizationProblem;
};

}  // namespace optimization
}  // namespace common
}  // namespace autoware

#endif  // OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_


#include "include/MMVII_all.h"

// #include <Eigen/Dense>

namespace MMVII
{

/* ========================== */
/*          ::                */
/* ========================== */

/// Computation to get the point we have at end of iterating a rectangle
template <const int Dim> cPtxd<int,Dim> CalPEnd(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1)
{
    cPtxd<int,Dim> aRes = aP0;
    aRes[Dim-1] = aP1[Dim-1] ;
    return aRes;
}

/* ========================== */
/*   cBorderPixBoxIterator    */
/* ========================== */

template <const int Dim>
  cBorderPixBoxIterator<Dim>::cBorderPixBoxIterator(tBPB & aBPB,const  tPt & aP0) :
      cPixBoxIterator<Dim>  (aBPB.PB(),aP0),
      mBPB                  (&aBPB)
{
}

template <const int Dim>
  cBorderPixBoxIterator<Dim>  & cBorderPixBoxIterator<Dim>::operator ++(int)
{
   return ++(*this);
}

template <const int Dim>
  cBorderPixBoxIterator<Dim>  & cBorderPixBoxIterator<Dim>::operator ++()
{
    //cPixBoxIterator<Dim>::operator ++ ();
    tPBI::operator ++ ();

    mBPB->IncrPt(tPBI::mPCur);
    return *this;
}

/* ========================== */
/*      cBorderPixBox         */
/* ========================== */

template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const tPB & aPB,const tPt & aSz) :
    mPB     (aPB),
    mSz     (aSz),
    mBoxInt (mPB.Dilate(-mSz)),
    mX0     (mBoxInt.P0().x()),
    mX1     (mBoxInt.P1().x()),
    mBegin  (*this,mPB.P0()),
    mEnd    (*this,CalPEnd(mPB.P0(),mPB.P1()))
{
}
template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const tPB & aPB,int aSz) :
      cBorderPixBox<Dim>(aPB,tPt::PCste(aSz))
{
}

template <const int Dim>
  cBorderPixBox<Dim>::cBorderPixBox(const cBorderPixBox<Dim> & aBPB) :
    cBorderPixBox<Dim>(mPB,mSz)
{
}

template <const int Dim>  cPixBox<Dim> &   cBorderPixBox<Dim>::PB() {return mPB;}

template <const int Dim>  void cBorderPixBox<Dim>::IncrPt(tPt & aP)
{
   if  ((aP.x()==mX0) && (mBoxInt.Inside(aP)))
      aP.x() = mX1;

}

/* ========================== */
/*          cPtxd             */
/* ========================== */

int NbPixVign(const int & aVign){return 1+2*aVign;}

template <const int Dim> int NbPixVign(const cPtxd<int,Dim> & aVign)
{
   int aRes = NbPixVign(aVign[0]);
   for (int aD=1 ;aD<Dim ; aD++)
       aRes *= NbPixVign(aVign[aD]);
   return aRes;
}

// static const int VeryBig = 1e9;
// const cPt2di  ThePSupImage( VeryBig, VeryBig);
// const cPt2di  ThePInfImage(-VeryBig,-VeryBig);

cPt2di  TAB4Corner[4] = {{1,1},{-1,1},{-1,-1},{1,-1}};

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PCste(const Type & aVal)
{
   cPtxd<Type,Dim> aRes;
   for (int aK=0 ; aK<Dim; aK++)
       aRes.mCoords[aK]= aVal;
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::FromPtInt(const cPtxd<int,Dim> & aPInt)
{
   cPtxd<Type,Dim> aRes;
   for (int aK=0 ; aK<Dim; aK++)
       aRes.mCoords[aK]= aPInt[aK];
   return aRes;
}

/*
void ff()
{
    cPtxd<double,3>::FromPtInt(cPt3di(0,0,0));
}
*/

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PRand()
{
   cPtxd<Type,Dim> aRes;
   for (int aK=0 ; aK<Dim; aK++)
       aRes.mCoords[aK]= tNumTrait<Type>::RandomValue();
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PRandC()
{
   cPtxd<Type,Dim> aRes;
   for (int aK=0 ; aK<Dim; aK++)
       aRes.mCoords[aK]= RandUnif_C();
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PRandUnit()
{
   cPtxd<Type,Dim> aRes = PRandC();
   while (NormInf(aRes)<1e-2)
        aRes = PRandC();
   return VUnit(aRes);
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cPtxd<Type,Dim>::PRandInSphere()
{
   cPtxd<Type,Dim> aRes = PRandC();
   while (Norm2(aRes)>1.0)
        aRes = PRandC();
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>
      cPtxd<Type,Dim>::PRandUnitDiff(const cPtxd<Type,Dim>& aP0,const Type & aDist)
{
   cPtxd<Type,Dim> aRes = PRandUnit();
   while (NormInf(aRes-aP0)<aDist)
        aRes = PRandUnit();
   return aRes;
}

template <class Type,const int Dim>
   typename cPtxd<Type,Dim>::tBigNum cPtxd<Type,Dim>::MinSqN2(const std::vector<tPt> & aVecPts,bool SVP) const
{
   if (aVecPts.empty())
   {
       MMVII_INTERNAL_ASSERT_medium(SVP,"MinSqN2 on empty vect, no def");
       return -1;
   }
   tBigNum aRes = SqN2(aVecPts[0]-*this);
   for (size_t  aKPts=1 ; aKPts<aVecPts.size() ; aKPts++)
       aRes = std::min(aRes, SqN2(aVecPts[aKPts]-*this));
   return aRes;
}

template <class Type,const int Dim> double NormK(const cPtxd<Type,Dim> & aPt,double anExp)
{
   double aRes = pow(std::abs(aPt[0]),anExp);
   for (int aD=1 ; aD<Dim; aD++)
      aRes += pow(std::abs(aPt[aD]),anExp);
   return pow(aRes,1/anExp);
}

template <class Type,const int Dim> double Norm2(const cPtxd<Type,Dim> & aPt)
{
   double aRes = Square(aPt[0]);
   for (int aD=1 ; aD<Dim; aD++)
      aRes += Square(aPt[aD]);
   return sqrt(aRes);
}

template <class Type,const int Dim> Type Norm1(const cPtxd<Type,Dim> & aPt)
{
   Type aRes = std::abs(aPt[0]);
   for (int aD=1 ; aD<Dim; aD++)
      aRes += std::abs(aPt[aD]);
   return aRes;
}

template <class Type,const int Dim> Type NormInf(const cPtxd<Type,Dim> & aPt)
{
   Type aRes = std::abs(aPt[0]);
   for (int aD=1 ; aD<Dim; aD++)
      aRes = std::max(aRes,std::abs(aPt[aD]));
   return aRes;
}

template <class Type,const int Dim> Type MinAbsCoord(const cPtxd<Type,Dim> & aPt)
{
   Type aRes = std::abs(aPt[0]);
   for (int aD=1 ; aD<Dim; aD++)
      aRes = std::min(aRes,std::abs(aPt[aD]));
   return aRes;
}

template <class T,const int Dim>
   typename  tNumTrait<T>::tBig Scal(const cPtxd<T,Dim> &aP1,const cPtxd<T,Dim> & aP2)
{
   typename tNumTrait<T>::tBig  aRes = aP1[0]*aP2[0];
   for (int aD=1 ; aD<Dim; aD++)
      aRes +=  aP1[aD]*aP2[aD];
   return aRes;
}

template <class T,const int Dim>  T Cos(const cPtxd<T,Dim> &aP1,const cPtxd<T,Dim> & aP2)
{
   return T(Scal(aP1,aP2)) / (Norm2(aP1)*Norm2(aP2));
}

template <class Type,const int Dim> std::ostream & operator << (std::ostream & OS,const cPtxd<Type,Dim> &aP)
{
    OS << "[" << aP.x();
    for (int aD=1; aD<Dim; aD++)
       OS << "," << aP[aD];
    OS << "]" ;
    return OS;
}

template<class T,const int Dim> cPtxd<T,Dim>  VUnit(const cPtxd<T,Dim> & aP)
{
   return aP / T(Norm2(aP));  // Check by 0 is made in operator
}

template <class Type,const int DimOut,const int DimIn> cPtxd<Type,DimOut> CastDim(const cPtxd<Type,DimIn> & aPt)
{
    MMVII_INTERNAL_ASSERT_tiny(DimIn==DimOut,"CastDim : different dim");

    return  cPtxd<Type,DimOut>(aPt.PtRawData());

}

template <const int Dim>  class cAllocNeighourhood
{

   public :
      typedef  cPtxd<int,Dim>    tPt;
      typedef  std::vector<tPt>  tVecPt;
      typedef  std::vector<tVecPt>  tVVPt;

      static  const tVVPt & AllocTabGrowNeigh(int aDMax)
      {
           static  tVVPt aRes;;
           if (int(aRes.size()) > aDMax) return aRes;

           aRes = tVVPt(1+aDMax,tVecPt());

           for (const auto & aP :  cPixBox(tPt::PCste(-aDMax),tPt::PCste(aDMax+1)))
               aRes.at(NormInf(aP)).push_back(aP);

           return aRes;
      }

      static const tVecPt &  Alloc(int aNbPix)
      {
// StdOut() <<  "----------------======================\n";
            static  std::vector<tVecPt> aBufRes(Dim);
            MMVII_INTERNAL_ASSERT_tiny((aNbPix>0)&&(aNbPix<=Dim),"Bad Nb in neighbourhood");

            // If alreay computed all fine
            tVecPt & aRes = aBufRes[aNbPix-1];
            if (! aRes.empty()) return aRes;

            // Usee full neighboor
            cPixBox<Dim> aPB(tPt::PCste(-1),tPt::PCste(2));
            for (const auto & aP : aPB)
            {
                int aN = Norm1(aP);
                if ((aN>0) && (aN<=aNbPix))
                {
                    aRes.push_back(aP);
                    // StdOut() << aP << "\n";
                }
            }

            return aRes;
      }
};

template <const int Dim>  const std::vector<cPtxd<int,Dim>> & AllocNeighbourhood(int aNbVois)
{
   return  cAllocNeighourhood<Dim>::Alloc(aNbVois);
}

template <const int Dim>  const std::vector<std::vector<cPtxd<int,Dim>>> & TabGrowNeigh(int aDistMax)
{
   return  cAllocNeighourhood<Dim>::AllocTabGrowNeigh(aDistMax);
}

/*
template <class Type> std::ostream & operator << (std::ostream & OS,const cPtxd<Type,1> &aP)
{ return  OS << "[" << aP.x() << "]"; }
template <class Type> std::ostream & operator << (std::ostream & OS,const cPtxd<Type,2> &aP)
{ return  OS << "[" << aP.x() << "," << aP.y() << "]"; }
template <class Type> std::ostream & operator << (std::ostream & OS,const cPtxd<Type,3> &aP)
{ return  OS << "[" << aP.x() << "," << aP.y() << "," << aP.z()<< "]"; }
template <class Type> std::ostream & operator << (std::ostream & OS,const cPtxd<Type,4> &aP)
{ return  OS << "[" << aP.x() << "," << aP.y() << "," << aP.z() << "," << aP.t() << "]"; }
*/

/* ========================== */
/*          ::                */
/* ========================== */
    //  To test Error_Handler mecanism

static std::string MesNegSz="Negative size in rect object";
/*
static std::string  TestErHandler;
void TestBenchRectObjError(const std::string & aType,const std::string &  aMes,const char * aFile,int aLine)
{
   TestErHandler = aMes;
}
*/

/* ========================== */
/*          cPixBox           */
/* ========================== */

template <const int Dim>   cPixBox<Dim>::cPixBox(const cPtxd<int,Dim> & aP0,const cPtxd<int,Dim> & aP1,bool AllowEmpty) :
     cTplBox<int,Dim>(aP0,aP1,AllowEmpty),
     mBegin  (*this,aP0),
     mEnd    (*this,CalPEnd(aP0,aP1))
{
}

template <const int Dim>   cPixBox<Dim>::cPixBox(const cPixBox<Dim> & aR) :
   cPixBox<Dim>(aR.mP0,aR.mP1,true)
{
}

template <const int Dim> cPixBox<Dim>  cPixBox<Dim>::BoxWindow(const tPt & aC,int aSz)
{
    return cPixBox<Dim>(aC-cPtxd<int,Dim>::PCste(aSz),aC+cPtxd<int,Dim>::PCste(aSz+1));
}

template <const int Dim> cPixBox<Dim>  cPixBox<Dim>::BoxWindow(int aSz)
{
    return BoxWindow(cPtxd<int,Dim>::PCste(0),aSz);
}

template <const int Dim>   cPixBox<Dim>::cPixBox(const cTplBox<int,Dim> & aR) :
   cPixBox<Dim>(aR.P0(),aR.P1(),true)
{
}

template <const int Dim> tINT8  cPixBox<Dim>::IndexeLinear(const tPt & aP) const
{
   tINT8 aRes = 0;
   for (int aK=0 ; aK<Dim ; aK++)
      aRes += tINT8(aP[aK]-tBox::mP0[aK]) * tINT8(tBox::mSzCum[aK]);
   return aRes;
}

template <const int Dim> bool  cPixBox<Dim>::SignalAtFrequence(const tPt & anIndex,double aFreq) const
{
   return MMVII::SignalAtFrequence(IndexeLinear(anIndex),aFreq,this->mNbElem-1);
}

template <const int Dim> cPtxd<int,Dim>  cPixBox<Dim>::FromIndexeLinear(tINT8  anIndexe) const
{
   tPt aRes;
   for (int aK=0 ; aK<Dim ; aK++)
   {
      aRes[aK]  = tBox::mP0[aK] + anIndexe % tBox::mSz[aK];
      anIndexe /= tBox::mSz[aK];
   }
   return aRes;
}

template <const int Dim> int cPixBox<Dim>::Interiority(const int  aCoord,int aD) const
{
   return std::min(aCoord-tBox::mP0[aD],tBox::mP1[aD]-1-aCoord);
}

template <const int Dim> int cPixBox<Dim>::Interiority(const tPt &  aP,int aD) const
{
   return Interiority(aP[aD],aD);
}

template <const int Dim> int cPixBox<Dim>::Interiority(const tPt& aP  ) const
{
   int aRes = Interiority(aP,0);
   for (int aD=1 ; aD<Dim ; aD++)
       aRes = std::min(aRes,Interiority(aP,aD));

   return aRes;
}

template <const int Dim> int cPixBox<Dim>::WinInteriority(const tPt& aP,const tPt& aWin,int aD) const
{
   return Interiority(aP,aD)-aWin[aD];
}

template <const int Dim> cBorderPixBox<Dim>  cPixBox<Dim>::Border(int aSz) const
{
   return  cBorderPixBox<Dim>(*this,aSz);
}

template <const int Dim> cPtxd<int,Dim>  cPixBox<Dim>::CircNormProj(const tPt & aPt) const
{
    cPtxd<int,Dim>  aRes;
    for (int aD=0 ; aD<Dim ; aD++)
       aRes[aD] = tBox::mP0[aD] + mod(aPt[aD]-tBox::mP0[aD],tBox::mSz[aD]);
    return aRes;
}

/* ========================== */
/*       cParseBoxInOut       */
/* ========================== */

template <const int Dim> cParseBoxInOut<Dim>::cParseBoxInOut(const tBox & aBoxGlob,const tBox & aBoxIndexe) :
    mBoxGlob (aBoxGlob),
    mBoxIndex (aBoxIndexe)
{
}
        // static tThis  CreateFromMem(const tBox&, double AvalaibleMem);

template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSize(const tBox & aBox,const tPt & aSzTile)
{
   return  tThis(aBox,tBox(tPt::PCste(0),CByC2P(aBox.Sz(),aSzTile,DivSup<int>)));
}
template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSizeCste(const tBox & aBox,int aSz)
{
    return CreateFromSize(aBox,tPt::PCste(aSz));
}

template <const int Dim> cParseBoxInOut<Dim> cParseBoxInOut<Dim>::CreateFromSzMem(const tBox & aBox,double aSzMem)
{
    return CreateFromSizeCste(aBox,round_ni(pow(aSzMem,1/double(Dim))));
}

template <const int Dim>  const cPixBox<Dim> &  cParseBoxInOut<Dim>::BoxIndex() const
{
      return  mBoxIndex;
}

template <const int Dim>  cPtxd<int,Dim>  cParseBoxInOut<Dim>::Index2Glob(const tPt & anIndex) const
{
      return  CByC1P(mBoxGlob.FromNormaliseCoord(mBoxIndex.ToNormaliseCoord(anIndex)),round_ni);
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxOut(const tPt & anIndex) const
{
      return  cPixBox<Dim>
              (
                   Index2Glob(anIndex),
                   Index2Glob(anIndex+tPt::PCste(1))
              );
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxIn(const tPt & anIndex,const tPt& aDil) const
{
   return mBoxGlob.Inter(BoxOut(anIndex).Dilate(aDil));
}

template <const int Dim>  cPixBox<Dim> cParseBoxInOut<Dim>::BoxIn(const tPt & anIndex,int aDil) const
{
   return mBoxGlob.Inter(BoxOut(anIndex).Dilate(tPt::PCste(aDil)));
}

/* ========================== */
/*          cTplBox           */
/* ========================== */

template <class Type,const int Dim>
   cTplBox<Type,Dim>::cTplBox
   (
       const cPtxd<Type,Dim> & aP0,
       const cPtxd<Type,Dim> & aP1,
       bool AllowEmpty
   ) :
       mP0  (aP0),
       mP1  (aP1),
       mSz  (aP0),
       mNbElem (1)
{
    //for (int aK=Dim-1 ; aK>=0 ; aK--)
    for (int aK=0 ; aK<Dim ; aK++)
    {
       mSz[aK] = mP1[aK] - mP0[aK];
       if (AllowEmpty)
       {
          // make coherent but empty
          if (mSz[aK] <0)
          {
             mP1[aK] = mP0[aK];
             mSz[aK] = 0;
          }
       }
       else
       {
          MMVII_INTERNAL_ASSERT_strong(mSz[aK]>0,MesNegSz);
       }
       mSzCum[aK] = mNbElem;
       mNbElem *= mSz[aK];
    }
}

template <class Type,const int Dim>
   cTplBox<Type,Dim>::cTplBox
   (
       const cPtxd<Type,Dim> & aSz,
       bool AllowEmpty
   ) :
     cTplBox<Type,Dim>(tPt::PCste(0),aSz,AllowEmpty)
{
}

template <class Type,const int Dim> bool  cTplBox<Type,Dim>::IsEmpty() const
{
   return mNbElem == 0;
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Empty()
{
   return  cTplBox<Type,Dim>(tPt::PCste(0),true);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Inter(const tBox & aBox)const
{
  return tBox(PtSupEq(mP0,aBox.mP0),PtInfEq(mP1,aBox.mP1),true);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Sup(const tBox & aB2)const
{
  if (IsEmpty() && aB2.IsEmpty())
     return  Empty();
  if (IsEmpty())     return aB2;
  if (aB2.IsEmpty()) return *this;

  return tBox(PtInfEq(mP0,aB2.mP0),PtSupEq(mP1,aB2.mP1),true);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Dilate(const tPt & aPt) const
{
   return tBox(mP0-aPt,mP1+aPt);
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::Dilate(const Type & aVal) const
{
   return Dilate(tPt::PCste(aVal));
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::ScaleCentered(const Type & aVal) const
{
   tPt aMil = (mP0+mP1)/Type(2.0);
   tPt anAmpl = (mP1-mP0)* Type(aVal/2.0);
   return tBox(aMil-anAmpl,aMil+anAmpl);
}

template <class Type,const int Dim> void cTplBox<Type,Dim>::AssertSameArea(const cTplBox<Type,Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong((*this)==aR2,"Rect obj were expected to have identic area");
}
template <class Type,const int Dim> void cTplBox<Type,Dim>::AssertSameSz(const cTplBox<Type,Dim> & aR2) const
{
    MMVII_INTERNAL_ASSERT_strong(Sz()==aR2.Sz(),"Rect obj were expected to have identic size");
}

template <class Type,const int Dim> bool cTplBox<Type,Dim>::operator == (const tBox & aR2) const
{
    return (mP0==aR2.mP0) && (mP1==aR2.mP1);
}

template <class Type,const int Dim> bool cTplBox<Type,Dim>::IncludedIn(const tBox & aR2) const
{
    return SupEq(mP0,aR2.mP0) && InfEq(mP1,aR2.mP1) ;
}

template <class Type,const int Dim> cTplBox<Type,Dim> cTplBox<Type,Dim>::Translate(const cPtxd<Type,Dim> & aTr) const
{
   return cTplBox<Type,Dim>(mP0+aTr,mP1+aTr);
}

template <class Type,const int Dim> cPtxd<Type,Dim>  cTplBox<Type,Dim>::FromNormaliseCoord(const cPtxd<double,Dim> & aPN) const
{
    // MMVII_INTERNAL_ASSERT_strong(false,"To Change
    cPtxd<Type,Dim> aRes;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        // aRes[aK] = mP0[aK] + round_down(mSz[aK]*aPN[aK]);
        aRes[aK] = mP0[aK] + tBaseNumTrait<Type>::RoundNearestToType(mSz[aK]*aPN[aK]);
    }
    return aRes;
    // return Proj(aRes);
}

template <class Type,const int Dim> void  cTplBox<Type,Dim>::Corners(tCorner & aRes) const
{
    for (size_t aKpt=0; aKpt<(1<<Dim)  ; aKpt++)
    {
        for (size_t aD=0 ; aD<Dim ; aD++)
        {
            aRes[aKpt][aD] = (aKpt & (1<<aD)) ? mP0[aD] : mP1[aD];
        }
    }
}

template <class Type,const int Dim> cPtxd<double,Dim>  cTplBox<Type,Dim>::ToNormaliseCoord(const cPtxd<Type,Dim> & aP) const
{
    cPtxd<double,Dim> aRes;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        aRes[aK] = (aP[aK]-mP0[aK]) / double(mSz[aK]);
    }
    return aRes;
}

template <class Type,const int Dim>  cPtxd<double,Dim>  cTplBox<Type,Dim>::RandomNormalised()
{
   cPtxd<double,Dim>  aRes;
   for (int aK=0 ; aK<Dim ; aK++)
   {
        aRes[aK] = RandUnif_0_1();
   }
   return aRes;
}

template <class Type,const int Dim> cPtxd<Type,Dim>   cTplBox<Type,Dim>::GeneratePointInside() const
{
/*
   cPtxd<double,Dim> aP0 = RandomNormalised();
   cPtxd<Type,Dim>  aP1 = FromNormaliseCoord(aP0);
   cPtxd<Type,Dim> aP2 = Proj(aP1);
StdOut() << "BOX " << mP0 << " " << mP1 << "\n";
StdOut() <<  aP0 << aP1 << aP2 << "\n"; getchar();
   return aP2;
*/
   return Proj(FromNormaliseCoord(RandomNormalised()));
}

template <class Type,const int Dim> cTplBox<Type,Dim>  cTplBox<Type,Dim>::GenerateRectInside(double aPowSize) const
{
    cPtxd<Type,Dim> aP0;
    cPtxd<Type,Dim> aP1;
    for (int aK=0 ; aK<Dim ; aK++)
    {
        double aSzRed = pow(RandUnif_0_1(),aPowSize);
        double aX0 = (1-aSzRed) * RandUnif_0_1();
        double aX1 = aX0 + aSzRed;
        // int aI0 = round_down(aX0*mSz[aK]);
        // int aI1 = round_down(aX1*mSz[aK]);
        Type aI0 =  tBaseNumTrait<Type>::RoundNearestToType(aX0*mSz[aK]);
        Type aI1 =  tBaseNumTrait<Type>::RoundNearestToType(aX1*mSz[aK]);
        aI1 = std::min((mP1[aK]-1),std::max(aI1,aI0+1));
        aI0  = std::max((mP0[aK]),std::min(aI0,aI1-1));
        aP0[aK] = aI0;
        aP1[aK] = aI1;

    }
    return cTplBox<Type,Dim>(aP0,aP1);
}

cBox2dr ToR(const cBox2di & aBox)
{
   return cBox2dr(ToR(aBox.P0()),ToR(aBox.P1()));
}

cBox2di ToI(const cBox2dr & aBox)
{
    return cBox2di(Pt_round_down(aBox.P0()),Pt_round_up(aBox.P1()));
}

cBox2dr operator * (const cBox2dr & aBox,double aScale)
{
    return cBox2dr(aBox.P0()*aScale,aBox.P1()*aScale);
}

//        x0,y1        x1,y1
//        x0,y0        x1,y0

template <class Type>
    void CornersTrigo(typename cTplBox<Type,2>::tCorner & aRes,const  cTplBox<Type,2>& aBox)
{
   aRes[0] = cPtxd<Type,2>(aBox.P1().x(),aBox.P1().y());
   aRes[1] = cPtxd<Type,2>(aBox.P0().x(),aBox.P1().y());
   aRes[2] = cPtxd<Type,2>(aBox.P0().x(),aBox.P0().y());
   aRes[3] = cPtxd<Type,2>(aBox.P1().x(),aBox.P0().y());
}

/* ========================== */
/*       cTpxBoxOfPts         */
/* ========================== */

template <class Type,const int Dim>   cTplBoxOfPts<Type,Dim>::cTplBoxOfPts() :
   mNbPts (0),
   mP0    (tPt::PCste(0)),
   mP1    (tPt::PCste(0))
{
}

template <class Type,const int Dim>  int  cTplBoxOfPts<Type,Dim>::NbPts() const {return mNbPts;}

template <class Type,const int Dim>  const cPtxd<Type,Dim> &  cTplBoxOfPts<Type,Dim>::P0() const
{
   MMVII_INTERNAL_ASSERT_medium(mNbPts,"cTplBoxOfPts<Type,Dim>::P0()")
   return  mP0;
}

template <class Type,const int Dim>  const cPtxd<Type,Dim> &  cTplBoxOfPts<Type,Dim>::P1() const
{
   MMVII_INTERNAL_ASSERT_medium(mNbPts,"cTplBoxOfPts<Type,Dim>::P1()")
   return  mP1;
}

template <class Type,const int Dim>  cTplBox<Type,Dim>  cTplBoxOfPts<Type,Dim>::CurBox() const
{
    return  cTplBox<Type,Dim>(mP0,mP1);
}

template <class Type,const int Dim>  void  cTplBoxOfPts<Type,Dim>::Add(const tPt & aP)
{
   if (mNbPts==0)
   {
      mP0 = aP;
      mP1 = aP;
   }
   else
   {
      SetInfEq(mP0,aP);
      SetSupEq(mP1,aP);
   }
   mNbPts++;
}

template <class Type,const int Dim>  cPtxd<int,Dim> Pt_round_down(const cPtxd<Type,Dim>  aP)
{
   return ICByC1P(aP,round_down);
}
template <class Type,const int Dim>  cPtxd<int,Dim> Pt_round_up(const cPtxd<Type,Dim>  aP)
{
   return ICByC1P(aP,round_up);
}
template <class Type,const int Dim>  cPtxd<int,Dim> Pt_round_ni(const cPtxd<Type,Dim>  aP)
{
   return ICByC1P(aP,round_ni);
}

template <class Type> bool WindInside4BL(const cBox2di & aBox,const cPtxd<Type,2> & aPt,const  cPt2di & aSzW)
{
   return
	   (aPt.x() >= aBox.P0().x() + aSzW.x())
       &&  (aPt.y() >= aBox.P0().y() + aSzW.y())
       &&  (aPt.x() <  aBox.P1().x() - aSzW.x()-1)
       &&  (aPt.y() <  aBox.P1().y() - aSzW.y()-1) ;
}

/* ========================== */
/*       INSTANTIATION        */
/* ========================== */

template void CornersTrigo(typename cTplBox<tREAL8,2>::tCorner & aRes,const  cTplBox<tREAL8,2>&);
template void CornersTrigo(typename cTplBox<tINT4,2>::tCorner & aRes,const  cTplBox<tINT4,2>&);

template  bool WindInside4BL(const cBox2di & aBox,const cPtxd<tINT4,2> & aPt,const  cPt2di & aSzW);
template  bool WindInside4BL(const cBox2di & aBox,const cPtxd<tREAL8,2> & aPt,const  cPt2di & aSzW);

#define MACRO_INSTATIATE_PTXD_2DIM(TYPE,DIMIN,DIMOUT)\
template  cPtxd<TYPE,DIMOUT> CastDim<TYPE,DIMOUT,DIMIN>(const cPtxd<TYPE,DIMIN> & aPt);

#define MACRO_INSTATIATE_PTXD(TYPE,DIM)\
MACRO_INSTATIATE_PTXD_2DIM(TYPE,DIM,1);\
MACRO_INSTATIATE_PTXD_2DIM(TYPE,DIM,2);\
MACRO_INSTATIATE_PTXD_2DIM(TYPE,DIM,3);\
MACRO_INSTATIATE_PTXD_2DIM(TYPE,DIM,4);\
template  std::ostream & operator << (std::ostream & OS,const cPtxd<TYPE,DIM> &aP);\
template  cPtxd<TYPE,DIM> cPtxd<TYPE,DIM>::PCste(const TYPE&);\
template  cPtxd<TYPE,DIM> cPtxd<TYPE,DIM>::PRand();\
template  cPtxd<TYPE,DIM> cPtxd<TYPE,DIM>::PRandC();\
template  cPtxd<TYPE,DIM> cPtxd<TYPE,DIM>::PRandUnit();\
template  cPtxd<TYPE,DIM> cPtxd<TYPE,DIM>::PRandInSphere();\
template typename cPtxd<TYPE,DIM>::tBigNum cPtxd<TYPE,DIM>::MinSqN2(const std::vector<tPt> &,bool SVP) const;\
template  cPtxd<TYPE,DIM>  cPtxd<TYPE,DIM>::PRandUnitDiff(const cPtxd<TYPE,DIM>& ,const TYPE&);\
template  double NormK(const cPtxd<TYPE,DIM> & aPt,double anExp);\
template  double Norm2(const cPtxd<TYPE,DIM> & aPt);\
template  TYPE Norm1(const cPtxd<TYPE,DIM> & aPt);\
template  TYPE NormInf(const cPtxd<TYPE,DIM> & aPt);\
template  TYPE MinAbsCoord(const cPtxd<TYPE,DIM> & aPt);\
template  typename  tNumTrait<TYPE>::tBig Scal(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &);\
template  TYPE Cos(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &);\
template  cPtxd<TYPE,DIM>  VUnit(const cPtxd<TYPE,DIM> & aP);\
template  cPtxd<TYPE,DIM>  cPtxd<TYPE,DIM>::FromPtInt(const cPtxd<int,DIM> & aPInt);

// template  cPtxd<TYPE,DIM>  PCste(const DIM & aVal);

#define MACRO_INSTATIATE_POINT(DIM)\
MACRO_INSTATIATE_PTXD(tINT4,DIM)\
MACRO_INSTATIATE_PTXD(tREAL4,DIM)\
MACRO_INSTATIATE_PTXD(tREAL8,DIM)\
MACRO_INSTATIATE_PTXD(tREAL16,DIM)

#define MACRO_INSTATIATE_PRECT_DIM(DIM)\
MACRO_INSTATIATE_POINT(DIM)\
template const std::vector<std::vector<cPtxd<int,DIM>>> & TabGrowNeigh(int);\
template const std::vector<cPtxd<int,DIM>> & AllocNeighbourhood(int);\
template cPtxd<int,DIM> Pt_round_down(const cPtxd<double,DIM>  aP);\
template cPtxd<int,DIM> Pt_round_up(const cPtxd<double,DIM>  aP);\
template cPtxd<int,DIM> Pt_round_ni(const cPtxd<double,DIM>  aP);\
template class cBorderPixBoxIterator<DIM>;\
template class cBorderPixBox<DIM>;\
template class cTplBox<tINT4,DIM>;\
template class cTplBoxOfPts<tINT4,DIM>;\
template  class cParseBoxInOut<DIM>;\
template class cTplBox<tREAL8,DIM>;\
template class cTplBoxOfPts<tREAL8,DIM>;\
template class cPixBox<DIM>;\
template  int NbPixVign(const cPtxd<int,DIM> & aVign);\
template class cDataGenUnTypedIm<DIM>;\
template <> const cPixBox<DIM> cPixBox<DIM>::TheEmptyBox(cPtxd<int,DIM>::PCste(0),cPtxd<int,DIM>::PCste(0),true);

/*
void F()
{
   cPtxd<tINT4,2>::T_PCste(2);
}
*/

MACRO_INSTATIATE_PRECT_DIM(1)
MACRO_INSTATIATE_PRECT_DIM(2)
MACRO_INSTATIATE_PRECT_DIM(3)
MACRO_INSTATIATE_POINT(4)

};