#ifndef OFFVSETACFITTED_HPP
#define OFFVSETACFITTED_HPP

#include <vector>
#include <string>
#include <boost/serialization/list.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/filesystem.hpp>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>

#include "arch/AACAgent.hpp"
#include "bib/Seed.hpp"
#include "bib/Utils.hpp"
#include <bib/MetropolisHasting.hpp>
#include <bib/XMLEngine.hpp>
#include "MLP.hpp"
#include "kde.hpp"
#include "kdtree++/kdtree.hpp"

#define DOUBLE_COMPARE_PRECISION 1e-9

typedef struct _sample {
  std::vector<double> s;
  std::vector<double> pure_a;
  std::vector<double> a;
  std::vector<double> next_s;
  double r;
  bool goal_reached;

  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar& BOOST_SERIALIZATION_NVP(s);
    ar& BOOST_SERIALIZATION_NVP(pure_a);
    ar& BOOST_SERIALIZATION_NVP(a);
    ar& BOOST_SERIALIZATION_NVP(next_s);
    ar& BOOST_SERIALIZATION_NVP(r);
    ar& BOOST_SERIALIZATION_NVP(goal_reached);
  }

  //Used to store all sample into a tree, might be stochastic
  //only pure_a is negligate
  bool operator< (const _sample& b) const {
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return s[i] < b.s[i];
    }

    for (uint i = 0; i < a.size(); i++) {
      if(fabs(a[i] - b.a[i])>=DOUBLE_COMPARE_PRECISION)
        return a[i] < b.a[i];
    }

    for (uint i = 0; i < next_s.size(); i++) {
      if(fabs(next_s[i] - b.next_s[i])>=DOUBLE_COMPARE_PRECISION)
        return next_s[i] < b.next_s[i];
    }

    if(fabs(r - b.r)>=DOUBLE_COMPARE_PRECISION)
        return r < b.r;

    return goal_reached < b.goal_reached;
  }

  typedef double value_type;

  inline double operator[](size_t const N) const{
        return s[N];
  }

  bool same_state(const _sample& b) const{
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return false;
    }

    return true;
  }

} sample;

class OffVSetACFitted : public arch::AACAgent<MLP, arch::AgentProgOptions> {
 public:
  typedef MLP PolicyImpl;

  OffVSetACFitted(unsigned int _nb_motors, unsigned int _nb_sensors)
    : arch::AACAgent<MLP, arch::AgentProgOptions>(_nb_motors), nb_sensors(_nb_sensors) {

  }

  virtual ~OffVSetACFitted() {
    delete kdtree_s;

    delete vnn;
    delete ann;
  }

  const std::vector<double>& _run(double reward, const std::vector<double>& sensors,
                                 bool learning, bool goal_reached, bool) {

    vector<double>* next_action = ann->computeOut(sensors);

    if (last_action.get() != nullptr && learning){
      trajectory.insert({last_state, *last_pure_action, *last_action, sensors, reward, goal_reached});
      last_trajectory.insert( {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached});
      proba_s.add_data(last_state);

      sample sa = {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached};
      kdtree_s->insert(sa);
    }

    last_pure_action.reset(new vector<double>(*next_action));
    if(learning) {
      if(gaussian_policy){
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalGaussianWReject(*next_action, noise);
        delete next_action;
        next_action = randomized_action;
      } else if(bib::Utils::rand01() < noise){ //e-greedy
        for (uint i = 0; i < next_action->size(); i++)
          next_action->at(i) = bib::Utils::randin(-1.f, 1.f);
      }
    }
    last_action.reset(next_action);

    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    return *next_action;
  }

  void _unique_invoke(boost::property_tree::ptree* pt, boost::program_options::variables_map*) override {
    hidden_unit_v           = pt->get<int>("agent.hidden_unit_v");
    hidden_unit_a           = pt->get<int>("agent.hidden_unit_a");
    noise                   = pt->get<double>("agent.noise");
    gaussian_policy         = pt->get<bool>("agent.gaussian_policy");
    lecun_activation        = pt->get<bool>("agent.lecun_activation");
    change_v_each           = pt->get<uint>("agent.change_v_each");
    strategy_u              = pt->get<uint>("agent.strategy_u");
    current_loaded_v        = pt->get<uint>("agent.index_starting_loaded_v");

    if(hidden_unit_v == 0)
      vnn = new LinMLP(nb_sensors + nb_motors , 1, 0.0, lecun_activation);
    else
      vnn = new MLP(nb_sensors + nb_motors, hidden_unit_v, nb_sensors, 0.0, lecun_activation);

    if(hidden_unit_a == 0)
      ann = new LinMLP(nb_sensors , nb_motors, 0.0, lecun_activation);
    else
      ann = new MLP(nb_sensors, hidden_unit_a, nb_motors, lecun_activation);

    kdtree_s = new kdtree_sample(nb_sensors);
  }

  void _start_episode(const std::vector<double>& sensors, bool) override {
    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    last_action = nullptr;
    last_pure_action = nullptr;

    //trajectory.clear();
    last_trajectory.clear();

    fann_reset_MSE(vnn->getNeuralNet());

    if(episode == 0 || episode % change_v_each == 0){
      if (! boost::filesystem::exists( "vset."+std::to_string(current_loaded_v) ) ){
        LOG_ERROR("file doesn't exists "<< "vset."+std::to_string(current_loaded_v));
        exit(1);
      }

      vnn->load("vset."+std::to_string(current_loaded_v));
      current_loaded_v++;
      end_episode();
    }
  }

  void update_actor_old_lw(){
    if (last_trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(last_trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      std::vector<double> deltas;
      for(auto it = last_trajectory.begin(); it != last_trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          for (uint i = 0; i < nb_motors; i++)
            data->output[n][i] = sm.a[i];

          deltas.push_back(target - mine);
          n++;
        }
      }

      if(n > 0) {
        double* importance = new double[n];
        double norm = *std::max_element(deltas.begin(), deltas.end());
        for (uint i =0 ; i < n; i++){
            importance[i] = deltas[i] / norm;
        }

        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch_lw(subdata, importance, 5000, 0, 0.0001);

        fann_destroy_train(subdata);

        delete[] importance;
      }
      fann_destroy_train(data);

    }
  }

  void update_actor_old_lw_all(){
    if (trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      std::vector<double> deltas;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          for (uint i = 0; i < nb_motors; i++)
            data->output[n][i] = sm.a[i];

          deltas.push_back(target - mine);
          n++;
        }
      }

      if(n > 0) {
        double* importance = new double[n];
        double norm = *std::max_element(deltas.begin(), deltas.end());
        for (uint i =0 ; i < n; i++){
            importance[i] = deltas[i] / norm;
        }

        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch_lw(subdata, importance, 5000, 0, 0.0001);

        fann_destroy_train(subdata);

        delete[] importance;
      }
      fann_destroy_train(data);

    }
  }

  void update_actor_old(){
     if (last_trajectory.size() > 0) {
      bool update_delta_neg;
      bool update_pure_ac;

      switch(strategy_u){
        case 0:
          update_pure_ac=false;
          update_delta_neg=false;
          break;
        case 1:
          update_pure_ac=true;
          update_delta_neg=false;
          break;
        case 2:
          update_pure_ac=false;
          update_delta_neg=true;
          break;
      }

      struct fann_train_data* data = fann_create_train(last_trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = last_trajectory.begin(); it != last_trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          if(update_pure_ac){
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
          } else {
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.a[i];
          }

          n++;
        } else if(update_delta_neg && !update_pure_ac){
            for (uint i = 0; i < nb_sensors ; i++)
              data->input[n][i] = sm.s[i];
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
            n++;
        }
      }

      if(n > 0) {
        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch(subdata, 5000, 0, 0.0001);

        fann_destroy_train(subdata);
      }
      fann_destroy_train(data);
    }
  }

  void update_actor_old_all(){
     if (trajectory.size() > 0) {
      bool update_pure_ac=false;
      bool update_delta_neg=false;

      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          if(update_pure_ac){
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
          } else {
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.a[i];
          }

          n++;
        } else if(update_delta_neg && !update_pure_ac){
            for (uint i = 0; i < nb_sensors ; i++)
              data->input[n][i] = sm.s[i];
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
            n++;
        }
      }

      if(n > 0) {
        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch(subdata, 5000, 0, 0.0001);

        fann_destroy_train(subdata);
      }
      fann_destroy_train(data);
    }
  }

   void update_actor_nfqca(){
    if(trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        for (uint i = 0; i < nb_sensors ; i++)
          data->input[n][i] = sm.s[i];

        for (uint i = 0; i < nb_motors; i++)
          data->output[n][i] = 0.f;//sm.a[i]; //don't care

        n++;
      }

      datann_derivative d = {vnn, (int)nb_sensors, (int)nb_motors};

//       auto iter = [&]() {
//         fann_train_epoch_irpropm_gradient(ann->getNeuralNet(), data, derivative_nn, &d);
//       };
//
//       auto eval = [&]() {
//         //compute weight sum
//         return ann->weight_l1_norm();
//       };
//
//       bib::Converger::determinist<>(iter, eval, 1, 0.0001, 25, "actor");
      fann_train_epoch_irpropm_gradient(ann->getNeuralNet(), data, derivative_nn, &d);

      fann_destroy_train(data);
    }
  }

  void end_episode() override {
    if(strategy_u <= 2)
      update_actor_old();
    else if(strategy_u <= 3)
      update_actor_old_lw();
    else if(strategy_u == 10)
      update_actor_nfqca();
    else if(strategy_u == 15)
      update_actor_old_lw_all();
    else if(strategy_u == 20)
      update_actor_old_all();
    else {
      LOG_ERROR("not implemented");
      exit(1);
    }

  }

  void end_instance(bool) override {
    episode++;
  }

  double criticEval(const std::vector<double>& perceptions, const std::vector<double>& actions) override {
    return vnn->computeOutVF(perceptions, actions);
  }

  arch::Policy<MLP>* getCopyCurrentPolicy() override {
        return new arch::Policy<MLP>(new MLP(*ann) , gaussian_policy ? arch::policy_type::GAUSSIAN : arch::policy_type::GREEDY, noise, decision_each);
  }

  void save(const std::string& path) override {
    ann->save(path+".actor");
    vnn->save(path+".critic");
    bib::XMLEngine::save<>(trajectory, "trajectory", "trajectory.data");
  }

  void load(const std::string& path) override {
    ann->load(path+".actor");
    vnn->load(path+".critic");
  }

 protected:
  void _display(std::ostream& out) const override {
    out << std::setw(12) << std::fixed << std::setprecision(10) << sum_weighted_reward << " " << std::setw(
          8) << std::fixed << std::setprecision(5) << vnn->error() << " " << noise << " " << trajectory.size() ;
  }

  void _dump(std::ostream& out) const override {
    out <<" " << std::setw(25) << std::fixed << std::setprecision(22) <<
        sum_weighted_reward << " " << std::setw(8) << std::fixed <<
        std::setprecision(5) << vnn->error() << " " << trajectory.size() ;
  }

 private:
  uint nb_sensors;

  uint episode = 0;
  uint current_loaded_v = 0;
  uint change_v_each;
  uint strategy_u;

  double noise;
  bool gaussian_policy, lecun_activation;
  uint hidden_unit_v;
  uint hidden_unit_a;

  std::shared_ptr<std::vector<double>> last_action;
  std::shared_ptr<std::vector<double>> last_pure_action;
  std::vector<double> last_state;

  std::set<sample> trajectory;
  std::set<sample> last_trajectory;
//     std::list<sample> trajectory;
  KDE proba_s;

  struct L1_distance
  {
    typedef double distance_type;

    double operator() (const double& __a, const double& __b, const size_t) const
    {
      double d = fabs(__a - __b);
      return d;
    }
  };
  typedef KDTree::KDTree<sample, KDTree::_Bracket_accessor<sample>, L1_distance> kdtree_sample;
  kdtree_sample* kdtree_s;

  MLP* ann;
  MLP* vnn;
};

#endif


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/fullPlanetaryRotationModel.h"

#if USE_CSPICE
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"
#endif

#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h"

#if USE_SOFA
#include "Tudat/Astrodynamics/Ephemerides/itrsToGcrsRotationModel.h"
#include "Tudat/Astrodynamics/Ephemerides/synchronousRotationalEphemeris.h"
#include "Tudat/Astrodynamics/EarthOrientation/earthOrientationCalculator.h"
#include "Tudat/Astrodynamics/EarthOrientation/shortPeriodEarthOrientationCorrectionCalculator.h"
#include "Tudat/Mathematics/Interpolators/jumpDataLinearInterpolator.h"
#endif

namespace tudat
{

namespace simulation_setup
{

//! Function to retrieve a state from one of two functions
Eigen::Vector6d getStateFromSelectedStateFunction(
        const double currentTime,
        const bool useFirstFunction,
        const std::function< Eigen::Vector6d( const double ) > stateFunction1,
        const std::function< Eigen::Vector6d( const double ) > stateFunction2 )
{
    return ( useFirstFunction ) ? ( stateFunction1( currentTime ) ) : ( stateFunction2( currentTime ) );
}

//! Function to create a state function for a body, valid both during propagation, and outside propagation
std::function< Eigen::Vector6d( const double, bool ) > createRelativeStateFunction(
        const NamedBodyMap& bodyMap,
        const std::string orbitingBody,
        const std::string centralBody )
{
    // Retrieve state functions for relevant bodies (obtained from current state of body objects)
    std::function< Eigen::Vector6d( const double ) > bodyInertialStateFunction =
            std::bind( &Body::getState, bodyMap.at( orbitingBody ) );
    std::function< Eigen::Vector6d( const double ) > centralBodyInertialStateFunction =
            std::bind( &Body::getState, bodyMap.at(  centralBody ) );

    // Define relative state function from body object
    std::function< Eigen::Vector6d( const double ) > fromBodyStateFunction =
            std::bind(
                &ephemerides::getDifferenceBetweenStates, bodyInertialStateFunction,
                centralBodyInertialStateFunction, std::placeholders::_1 );

    // Define state function from ephemeris
    std::function< Eigen::Vector6d( const double ) > fromEphemerisStateFunction;

    if( bodyMap.at( orbitingBody )->getEphemeris( )->getReferenceFrameOrigin( ) == centralBody )
    {
        fromEphemerisStateFunction = std::bind( &ephemerides::Ephemeris::getCartesianState,
                                                bodyMap.at( orbitingBody )->getEphemeris( ), std::placeholders::_1 );

    }
    else
    {
        std::function< Eigen::Vector6d( const double ) > ephemerisInertialStateFunction =
                std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, bodyMap.at( orbitingBody ),
                           std::placeholders::_1 );
        std::function< Eigen::Vector6d( const double ) > ephemerisCentralBodyInertialStateFunction =
                std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, bodyMap.at( centralBody ),
                           std::placeholders::_1 );
        fromEphemerisStateFunction = std::bind(
                    &ephemerides::getDifferenceBetweenStates,
                    ephemerisInertialStateFunction,
                    ephemerisCentralBodyInertialStateFunction, std::placeholders::_1 );
    }

    return std::bind( &getStateFromSelectedStateFunction, std::placeholders::_1, std::placeholders::_2,
                      fromBodyStateFunction, fromEphemerisStateFunction );
}

//! Function to create a rotation model.
std::shared_ptr< ephemerides::RotationalEphemeris > createRotationModel(
        const std::shared_ptr< RotationModelSettings > rotationModelSettings,
        const std::string& body,
        const NamedBodyMap& bodyMap )
{
    using namespace tudat::ephemerides;

    // Declare return object.
    std::shared_ptr< RotationalEphemeris > rotationalEphemeris;

    // Check which type of rotation model is to be created.
    switch( rotationModelSettings->getRotationType( ) )
    {
    case simple_rotation_model:
    {
        // Check whether settings for simple rotation model are consistent with its type.
        std::shared_ptr< SimpleRotationModelSettings > simpleRotationSettings =
                std::dynamic_pointer_cast< SimpleRotationModelSettings >( rotationModelSettings );
        if( simpleRotationSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected simple rotation model settings for " + body );
        }
        else
        {
            // Create and initialize simple rotation model.
            rotationalEphemeris = std::make_shared< SimpleRotationalEphemeris >(
                        simpleRotationSettings->getInitialOrientation( ),
                        simpleRotationSettings->getRotationRate( ),
                        simpleRotationSettings->getInitialTime( ),
                        simpleRotationSettings->getOriginalFrame( ),
                        simpleRotationSettings->getTargetFrame( ) );
        }
        break;
    }
#if USE_SOFA
    case gcrs_to_itrs_rotation_model:
    {

        // Check whether settings for simple rotation model are consistent with its type.
        std::shared_ptr< GcrsToItrsRotationModelSettings > gcrsToItrsRotationSettings =
                std::dynamic_pointer_cast< GcrsToItrsRotationModelSettings >( rotationModelSettings );
        if( gcrsToItrsRotationSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected GCRS to ITRS rotation model settings for " + body );
        }
        else
        {
            std::shared_ptr< earth_orientation::EOPReader > eopReader = std::make_shared< earth_orientation::EOPReader >(
                        gcrsToItrsRotationSettings->getEopFile( ),
                        gcrsToItrsRotationSettings->getEopFileFormat( ),
                        gcrsToItrsRotationSettings->getNutationTheory( ) );

            // Load polar motion corrections
            std::shared_ptr< interpolators::LinearInterpolator< double, Eigen::Vector2d > > cipInItrsInterpolator =
                    std::make_shared< interpolators::LinearInterpolator< double, Eigen::Vector2d > >(
                        eopReader->getCipInItrsMapInSecondsSinceJ2000( ) );

            // Load nutation corrections
            std::shared_ptr< interpolators::LinearInterpolator< double, Eigen::Vector2d > > cipInGcrsCorrectionInterpolator =
                    std::make_shared< interpolators::LinearInterpolator< double, Eigen::Vector2d > >(
                        eopReader->getCipInGcrsCorrectionMapInSecondsSinceJ2000( ) );

            // Create polar motion correction (sub-diural frequencies) object
            std::shared_ptr< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > >
                    shortPeriodPolarMotionCalculator =
                    std::make_shared< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > >(
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->conversionFactor_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->minimumAmplitude_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->amplitudesFiles_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->argumentMultipliersFile_ );

            // Create full polar motion calculator
            std::shared_ptr< earth_orientation::PolarMotionCalculator > polarMotionCalculator =
                    std::make_shared< earth_orientation::PolarMotionCalculator >
                    ( cipInItrsInterpolator, shortPeriodPolarMotionCalculator );

            // Create IAU 2006 precession/nutation calculator
            std::shared_ptr< earth_orientation::PrecessionNutationCalculator > precessionNutationCalculator =
                    std::make_shared< earth_orientation::PrecessionNutationCalculator >(
                        gcrsToItrsRotationSettings->getNutationTheory( ), cipInGcrsCorrectionInterpolator );

            // Create UT1 correction (sub-diural frequencies) object
            std::shared_ptr< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< double > >
                    ut1CorrectionSettings =
                    std::make_shared< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< double > >(
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->conversionFactor_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->minimumAmplitude_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->amplitudesFiles_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->argumentMultipliersFile_ );

            std::shared_ptr< interpolators::OneDimensionalInterpolator < double, double > > dailyUtcUt1CorrectionInterpolator =
                    std::make_shared< interpolators::JumpDataLinearInterpolator< double, double > >(
                        eopReader->getUt1MinusUtcMapInSecondsSinceJ2000( ), 0.5, 1.0 );

            // Create default time scale converter
            std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > terrestrialTimeScaleConverter =
                    std::make_shared< earth_orientation::TerrestrialTimeScaleConverter >
                    (  dailyUtcUt1CorrectionInterpolator, ut1CorrectionSettings );

            // Create rotation model
            std::shared_ptr< earth_orientation::EarthOrientationAnglesCalculator > earthOrientationCalculator =
                    std::make_shared< earth_orientation::EarthOrientationAnglesCalculator >(
                        polarMotionCalculator, precessionNutationCalculator, terrestrialTimeScaleConverter );
            rotationalEphemeris = std::make_shared< ephemerides::GcrsToItrsRotationModel >(
                        earthOrientationCalculator, gcrsToItrsRotationSettings->getInputTimeScale( ),
                        gcrsToItrsRotationSettings->getOriginalFrame( ) );

            break;
        }

    }
#endif

#if USE_CSPICE
    case spice_rotation_model:
    {
        // Create rotational ephemeris directly from Spice.
        rotationalEphemeris = std::make_shared< SpiceRotationalEphemeris >(
                    rotationModelSettings->getOriginalFrame( ),
                    rotationModelSettings->getTargetFrame( ) );
        break;
    }
#endif
<<<<<<< HEAD
=======
    case planetary_rotation_model:
    {
        std::shared_ptr< PlanetaryRotationModelSettings > planetaryRotationModelSettings =
                std::dynamic_pointer_cast< PlanetaryRotationModelSettings >( rotationModelSettings );
        if( planetaryRotationModelSettings == nullptr )
        {
            std::cerr<<"Error, expected planetary rotation model settings for "<<body<<std::endl;
        }
        else
        {

            Eigen::VectorXd bodyKeplerElements = orbital_element_conversions::convertCartesianToKeplerianElements(
                        spice_interface::getBodyCartesianStateAtEpoch(
                            body, planetaryRotationModelSettings->getCentralBody( ), "ECLIPJ2000", "NONE", 0.0 ),
                        spice_interface::getBodyGravitationalParameter( planetaryRotationModelSettings->getCentralBody( ) ) );

            double meanAnomalyAtJ2000 = orbital_element_conversions::convertEccentricAnomalyToMeanAnomaly(
                        orbital_element_conversions::convertTrueAnomalyToEccentricAnomaly(
                            bodyKeplerElements( 5 ), bodyKeplerElements( 1 ) ), bodyKeplerElements( 1 ) );

            double meanMotion = orbital_element_conversions::convertSemiMajorAxisToEllipticalMeanMotion(
                        bodyKeplerElements( 0 ), spice_interface::getBodyGravitationalParameter(
                            planetaryRotationModelSettings->getCentralBody( ) ) + spice_interface::getBodyGravitationalParameter( body ) );

            std::shared_ptr< PlanetaryOrientationAngleCalculator > planetaryOrientationAnglesCalculator
                    = std::make_shared< PlanetaryOrientationAngleCalculator >(
                        planetaryRotationModelSettings->getAnglePsiAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePsiRateAtEpoch( ),
                        planetaryRotationModelSettings->getAngleIAtEpoch( ),
                        planetaryRotationModelSettings->getAngleIRateAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePhiAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePhiRateAtEpoch( ),
                        planetaryRotationModelSettings->getCoreFactor( ),
                        planetaryRotationModelSettings->getFreeCoreNutationRate( ),
                        meanMotion, meanAnomalyAtJ2000,
                        planetaryRotationModelSettings->getOriginalFrame( ),
                        planetaryRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                        planetaryRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                        planetaryRotationModelSettings->getTimeDependentPhaseCorrectionFunctions( ),
                        planetaryRotationModelSettings->getRotationRateCorrections( ),
                        planetaryRotationModelSettings->getxPolarMotionCoefficients( ),
                        planetaryRotationModelSettings->getyPolarMotionCoefficients( ) );

            rotationalEphemeris = std::make_shared< PlanetaryRotationModel >(
                        planetaryRotationModelSettings->getAngleN( ),
                        planetaryRotationModelSettings->getAngleJ( ),
                        planetaryOrientationAnglesCalculator,
                        planetaryRotationModelSettings->getOriginalFrame( ), planetaryRotationModelSettings->getTargetFrame( ) );
        }

        break;
    }
>>>>>>> origin/master
    case synchronous_rotation_model:
    {
        std::shared_ptr< SynchronousRotationModelSettings > synchronousRotationSettings =
                std::dynamic_pointer_cast< SynchronousRotationModelSettings >( rotationModelSettings );
        if( synchronousRotationSettings == NULL )
        {
            throw std::runtime_error( "Error, expected synchronous rotation model settings for " + body );
        }
        else
        {
            if( bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrigin( ) == synchronousRotationSettings->getCentralBodyName( ) )
            {
                if( bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrientation( ) !=
                        synchronousRotationSettings->getOriginalFrame( ) )
                {
                    throw std::runtime_error( "Error, ephemeris of body " + body + " is in " +
                                              bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrientation( ) +
                                              " frame when making synchronous rotation model, expected " +
                                              synchronousRotationSettings->getOriginalFrame( ) + " frame." );
                }
            }
            std::shared_ptr< SynchronousRotationalEphemeris > synchronousRotationalEphemeris = std::make_shared< SynchronousRotationalEphemeris >(
                        createRelativeStateFunction( bodyMap, body, synchronousRotationSettings->getCentralBodyName( ) ),
                        synchronousRotationSettings->getCentralBodyName( ),
                        synchronousRotationSettings->getOriginalFrame( ),
                        synchronousRotationSettings->getTargetFrame( ) );

            rotationalEphemeris = synchronousRotationalEphemeris;
        }
        break;
    }
    default:
        throw std::runtime_error(
                    "Error, did not recognize rotation model settings type " +
                    std::to_string( rotationModelSettings->getRotationType( ) ) );
    }

    return rotationalEphemeris;
}

} // namespace simulation_setup

} // namespace tudat


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP

#include <boost/geometry/algorithms/distance.hpp>

namespace boost { namespace geometry
{

//----------------------------------------------------------------------
// Function     : point_on_linestring -> rename to alongLine NO, different
//----------------------------------------------------------------------
// Purpose      : Calculates coordinates of a point along a given line
//                on a specified distance
// Parameters   : const L& : line,
//                float position: position to calculate point
//                P& point: point to calculate
// Return       : true if point lies on line
//----------------------------------------------------------------------
// Author       : Barend, Geodan BV Amsterdam
// Date         : spring 1996
//----------------------------------------------------------------------
template <typename P, typename L>
bool point_on_linestring(L const& line, double const& position, P& point)
{
    double current_distance = 0.0;
    if (line.size() < 2)
    {
        return false;
    }

    typename L::const_iterator vertex = line.begin();
    typename L::const_iterator previous = vertex++;

    while (vertex != line.end())
    {
        double const dist = distance(*previous, *vertex);
        current_distance += dist;

        if (current_distance > position)
        {
            // It is not possible that dist == 0 here because otherwise
            // the current_distance > position would not become true (current_distance is increased by dist)
            double const fraction = 1.0 - ((current_distance - position) / dist);

            // point i is too far, point i-1 to near, add fraction of
            // distance in each direction
            point.x ( previous->x() + (vertex->x() - previous->x()) * fraction);
            point.y ( previous->y() + (vertex->y() - previous->y()) * fraction);

            return true;
        }
        previous = vertex++;
    }

    // point at specified position does not lie on line
    return false;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP


#pragma once

#include <cstdint>
#include <istream>
#include <ostream>
#include <stdexcept>
#include <vector>
#include <iterator>
#include <algorithm>
#include <type_traits>

#include <boost/config.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/detail/workaround.hpp>
#include "xoroshiro.hpp"
#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)

template<typename IntType, std::size_t w, std::size_t CMWC_CYCLE, std::uint32_t CMWC_C_MAX, std::uint64_t A, bool is_complementary>
class complementary_multiply_with_carry_engine {
	typedef typename std::conditional<is_complementary, std::true_type, std::false_type>::type complementary;
public:
	typedef IntType result_type;

	// Required for old Boost.Random concept.
	static const bool has_fixed_range = true;
	static const IntType default_seed = 1;

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, using the default seed.
	*/
	complementary_multiply_with_carry_engine()
	{
		seed();
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, seeding it with @c value.
	*/
	template<typename IT>
	BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(complementary_multiply_with_carry_engine,
		IT, value)
	{
		seed(value);
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, seeding it with values
	* produced by a call to @c seq.generate().
	*/
	BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(complementary_multiply_with_carry_engine,
		SeedSeq, seq)
	{
		seed(seq);
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine and seeds it with values
	* taken from the iterator range [first, last) and adjusts
	* first to point to the element after the last one used.
	* If there are not enough elements, throws @c std::invalid_argument.
	*
	* first and last must be input iterators.
	*/
	template<class It>
	complementary_multiply_with_carry_engine(It& first, It last)
	{
		seed(first, last);
	}

	// compiler-generated copy constructor and assignment operator are fine.

	/**
	* Calls seed(default_seed)
	*/
	void seed()
	{
		seed(default_seed);
	}

	/**
	* seeds a @c complementary_multiply_with_carry_engine with splitmix64.
	*/
	template<typename IT>
	BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(complementary_multiply_with_carry_engine, IT, value)
	{
		BOOST_STATIC_ASSERT(std::numeric_limits<IT>::is_integer);

		boost::random::splitmix64 intgen(static_cast<std::uint64_t>(value));
		detail::generator_seed_seq<boost::random::splitmix64> gen(intgen);
		seed(gen);
	}

	/**
	* Seeds a @c complementary_multiply_with_carry_engine using values from a SeedSeq.
	*/
	BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(complementary_multiply_with_carry_engine, SeedSeq, seq)
	{
		detail::seed_array_int<w>(seq, _Q);
		_carry = detail::seed_one_int<IntType, CMWC_C_MAX>(seq);
		_i = CMWC_CYCLE - 1;
		warmup();
	}

	/**
	* Seeds a @c complementary_multiply_with_carry_engine with values taken from the
	* iterator range [first, last) and adjusts @c first to
	* point to the element after the last one used. If there are
	* not enough elements or all the whole input range is zero,
	* throws @c std::invalid_argument.
	*
	* @c first and @c last must be input iterators.
	*/
	template<class It>
	void seed(It& first, It last)
	{
		detail::fill_array_int<w>(first, last, _Q );
		_carry = detail::seed_one_int<IntType, CMWC_C_MAX>(first, last);
		_i = CMWC_CYCLE - 1;
		warmup();
	}

	/**
	* Returns the smallest value that the @c complementary_multiply_with_carry_engine
	* can produce.
	*/
	static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ( )
	{ return 0; }

	/**
	* Returns the largest value that the @c complementary_multiply_with_carry_engine
	* can produce.
	*/
	static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
	{ return boost::low_bits_mask_t<w>::sig_bits; }

	/**
	* Returns the next value of the @c
	* complementary_multiply_with_carry_engine,
	* complemented.
	*/
	template<typename U = complementary>
		typename std::enable_if<
			std::is_same<U, std::true_type>::value, result_type>::type
	operator()( )
	{
		_i = (_i + 1) & (CMWC_CYCLE - 1);
		const std::uint64_t t = A * _Q[_i] + _carry;
		_carry = t >> 32;
		std::uint32_t x = t + _carry;
		if (x < _carry) {
			++x, ++_carry;
		}
		return (_Q[_i] = std::uint32_t { 0xFFFFFFFE } - x); // m - x
	}

	/**
	 * Returns the next value of the @c
	 * complementary_multiply_with_carry_engine,
	 * not-complemented.
	 */
	template<typename U = complementary>
		typename std::enable_if<
			std::is_same<U, std::false_type>::value, result_type>::type
	operator()( )
	{
		_i = (_i + 1) & (CMWC_CYCLE - 1);
		const std::uint64_t t = A * _Q[_i] + _carry;
		_carry = t >> 32;
		std::uint32_t x = t + _carry;
		if (x < _carry) {
			++x, ++_carry;
		}
		return (_Q[_i] = x);
	}

	/** Fills a range with random values. */
	template<class Iter>
	void generate(Iter first, Iter last)
	{
		detail::generate_from_int(*this, first, last);
	}

	/** Advances the state of the generator by @c z. */
	void discard(std::uintmax_t z)
	{
		while (z--) {
			_i = (_i + 1) & (CMWC_CYCLE - 1);
			const std::uint64_t t = A * _Q[_i] + _carry;
			_carry = t >> 32;
			_carry += ((t + _carry) < _carry);
		}
	}

	/** Writes a @c complementary_multiply_with_carry_engine to a @c std::ostream. */
	template<class CharT, class Traits>
	friend std::basic_ostream<CharT, Traits>&
		operator<<( std::basic_ostream<CharT, Traits>& os,
			const complementary_multiply_with_carry_engine& cmwc)
	{
		std::size_t i = cmwc._i;
		for (; i < CMWC_CYCLE; ++i) {
			os << cmwc._Q[i] << ' ';
		}
		i = 0;
		for (; i < cmwc._i; ++i) {
			os << cmwc._Q[i] << ' ';
		}
		os << cmwc._carry;
		return os;
	}

	/** Reads a @c complementary_multiply_with_carry_engine from a @c std::istream. */
	template<class CharT, class Traits>
	friend std::basic_istream<CharT, Traits>&
		operator >> ( std::basic_istream<CharT, Traits>& is,
			complementary_multiply_with_carry_engine& cmwc)
	{
		for (std::size_t i = 0; i < CMWC_CYCLE; ++i) {
			is >> cmwc._Q [ i ] >> std::ws;
		}
		is >> cmwc._carry;
		cmwc._i = 0;
		return is;
	}

private:

	// As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
	inline void warmup()
	{
		discard(4 * CMWC_CYCLE);
	}

	IntType _Q[CMWC_CYCLE];
	std::size_t _i;
	IntType _carry;
};

using mwc256 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 256, 809430660, 1540315826, false>;

/*
 * Generators below from:
 *
 * Marsaglia, George (2003) "Random Number Generators,"Journal of Modern Applied Statistical Methods: Vol. 2 : Iss. 1 , Article 2, Page 9.
 * DOI: 10.22237/jmasm/1051747320
 * Available at: http://digitalcommons.wayne.edu/jmasm/vol2/iss1/2
 */

using cmwc4 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 4, 123, 987651670, true>;
using cmwc8 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 8, 123, 987651386, true>;
using cmwc16 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 16, 123, 987651178, true>;
using cmwc32 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 32, 123, 987655670, true>;
using cmwc64 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 64, 123, 987651206, true>;
using cmwc128 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 128, 123, 987688302, true>;
using cmwc256 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 256, 123, 987662290, true>;
using cmwc512 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 512, 123, 123462658, true>;
using cmwc1024 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 1024, 123, 5555698, true>;
using cmwc2048 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 2048, 123, 1030770, true>;

using cmwc4096 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 4096, 809430660, 18782, true>;

#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace random
} // namespace boost


//! 2D形状の定義
#pragma once
#include "geom.hpp"
#include "spinner/resmgr.hpp"
#include "spinner/assoc.hpp"
#include <boost/pool/object_pool.hpp>
#include <memory>
#include "spinner/structure/treenode.hpp"
#include "spinner/rflag.hpp"
#include "boundary.hpp"

namespace boom {
	namespace geo2d {
		using Int_OP = spn::Optional<int>;
		struct Convex;
		struct Poly;
		struct Capsule;
		struct Circle;
		struct AABB;
		struct Segment;
		struct Ray;
		struct Line;
		struct Point;
		using CTGeo = spn::CType<Convex, Poly, Capsule, Circle, AABB, Segment, Ray, Line, Point>;
		template <class T>
		using ITagP = ITagP_base<T, CTGeo>;

		using PointL = std::vector<Vec2>;
		using SegL = std::vector<Segment>;
		using Convex2 = std::pair<Convex, Convex>;
		struct Line;
		struct Ray;
		struct IModel;
		struct Circle : ITagP<Circle> {
			Vec2	vCenter;
			float	fRadius;

			Circle() = default;
			Circle(const Vec2& c, float r);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;
			bool hit(const Circle& c, float t=NEAR_THRESHOLD) const;

			void getArcPoints(PointL& dst, float ang0, float ang1, float deep) const;
			Circle operator * (const AMat32& m) const;
			Circle operator * (float s) const;
			Circle& operator += (const Vec2& ofs);
			void distend(float width, float mindist);

			// ---- for MakeBoundary ----
			void setBoundary(const IModel* p);
			void appendBoundary(const IModel* p);

			friend std::ostream& operator << (std::ostream& os, const Circle& c);
		};
		//! AxisAlignedBox
		struct AABB : ITagP<AABB> {
			Vec2	minV, maxV;

			int _getAreaNumX(float p) const;
			int _getAreaNumY(float p) const;
			void _makeSegmentX(Segment& s, int num) const;
			void _makeSegmentY(Segment& s, int num) const;
			bool _checkHitX(const Segment& s0, int from, int to) const;
			bool _checkHitY(const Segment& s0, int from, int to) const;
			std::pair<int,int> _getAreaNum(const Vec2& p) const;

			AABB() = default;
			AABB(const Vec2& min_v, const Vec2& max_v);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;
			Vec2 nearest(const Vec2& pos) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& l, float t=NEAR_THRESHOLD) const;
			bool hit(const AABB& ab, float t=NEAR_THRESHOLD) const;
			AABB operator * (const AMat32& m) const;
			AABB& operator += (const Vec2& ofs);
			void distend(float width, float mindist);

			// ---- for MakeBoundary ----
			void setBoundary(const IModel* p);
			void appendBoundary(const IModel* p);

			friend std::ostream& operator << (std::ostream& os, const AABB& a);
		};
		//! IModelインタフェースの子ノードイテレータ
		struct MdlItr {
			TfBase_SP	_sp;

			MdlItr() = default;
			MdlItr(const TfBase_SP& sp);

			MdlItr& operator ++ ();
			bool operator == (const MdlItr& m) const;
			bool operator != (const MdlItr& m) const;
			explicit operator bool () const;
			const TfBase* get() const;
		};
		struct IModel : ::boom::IModelNode {
			// -----------------------------
			virtual Vec2 im_getCenter() const = 0;
			virtual float im_getArea() const = 0;
			virtual float im_getInertia() const = 0;
			virtual void im_getBVolume(Circle& c) const = 0;
			virtual void im_getBVolume(AABB& a) const = 0;
			// -----------------------------
			virtual void im_transform[[noreturn]](void* dst, const AMat32& m) const = 0;
			//! サポート射像
			/*! 均等でないスケーリングは対応しない、移動は後でオフセット、回転はdirを逆にすれば代用可
				・・との理由で行列変換後の物体に対する射像は無し */
			virtual Vec2 im_support(const Vec2& dir) const = 0;
			//! 図形と点の判定
			virtual bool im_hitPoint(const Vec2& p, float t=NEAR_THRESHOLD) const = 0;
			virtual uint32_t getCID() const = 0;
			virtual MdlItr getInner() const;
			virtual Model_SP im_clone() const = 0;

			virtual Vec2 toLocal(const Vec2& v) const { return v; }
			virtual Vec2 toLocalDir(const Vec2& v) const { return v; }
			virtual Vec2 toWorld(const Vec2& v) const { return v; }
			virtual Vec2 toWorldDir(const Vec2& v) const { return v; }
			virtual spn::Optional<const AMat32&> getToLocal() const { return spn::none; }
			virtual spn::Optional<const AMat32&> getToWorld() const { return spn::none; }
			const static AMat32 cs_idMat;
			const AMat32& getToLocalI() const;
			const AMat32& getToWorldI() const;
		};

		//! TにIModelインタフェースを付加
		template <class T>
		struct Model : IModelP_base<T, IModel> {
			using base_t = IModelP_base<T, IModel>;
			using base_t::base_t;

			Model() = default;
			Model(const T& t): base_t(t) {}
			Model(T&& t): base_t(std::move(t)) {}
			// 各種ルーチンの中継
			Model_SP im_clone() const override {
				return std::make_shared<Model<T>>(static_cast<const T&>(*this));
			}
			void im_transform(void* dst, const AMat32& m) const override { *reinterpret_cast<T*>(dst) = *this * m; }
			void im_getBVolume(Circle& c) const override { c = T::bs_getBCircle(); }
			void im_getBVolume(AABB& a) const override { a = T::bs_getBBox(); }
			float im_getInertia() const override { return T::bs_getInertia(); }
			float im_getArea() const override { return T::bs_getArea(); }
			Vec2 im_getCenter() const override { return T::bs_getCenter(); }
			Vec2 im_support(const Vec2& dir) const override { return T::support(dir); }
			bool im_hitPoint(const Vec2& pos, float t=NEAR_THRESHOLD) const override { return T::hit(pos, t); }
			std::ostream& print(std::ostream& os) const override { return os << static_cast<const T&>(*this); }
		};
		template <class T>
		std::ostream& operator << (std::ostream& os, const Model<T>& m) {
			return m.print(os);
		}
		using CircleM = Model<Circle>;

		class TfBase : public spn::TreeNode<TfBase>,
						virtual public IModel
		{
			private:
				friend class spn::TreeNode<TfBase>;
				using base_t = spn::TreeNode<TfBase>;
			protected:
				void* _getUserData(void*, std::true_type);
				void* _getUserData(void* udata, std::false_type);
				virtual void onChildAdded(const SP& node);
				virtual void onChildRemove(const SP& node);
				virtual void onParentChange(const SP& from, const SP& to);
				virtual void _setAsChanged();
			public:
				//! 子ノードの取得
				MdlItr getInner() const override;
				bool hasInner() const override;
				//! 下層オブジェクトの形状を変更した時に手動で呼ぶ
				virtual void setAsChanged();
				virtual bool isLeaf() const { return false; }
				virtual spn::Pose2D& tf_refPose[[noreturn]]();
				virtual const spn::Pose2D& tf_getPose() const;
				Model_SP im_clone() const override;
				virtual TfBase_SP clone() const = 0;
		};
		template <class Boundary, class Ud>
		class TfNode_base : public TfBase,
							public Model<Boundary>
		{
			protected:
				using model_t = Model<Boundary>;
				Ud					_udata;
			public:
				using model_t::model_t;
				Model_SP im_clone() const override {
					return TfBase::im_clone();
				}
				/*! ユーザーデータがvoidの時は親ノードのデータを返す */
				void* getUserData() override {
					return _getUserData(&_udata, std::is_same<spn::none_t, Ud>());
				}
		};
		template <class Boundary, class Ud=spn::none_t>
		class TfNode_Static : public TfNode_base<Boundary, Ud> {
			private:
				using base_t = TfNode_base<Boundary, Ud>;
				using SP = typename base_t::SP;
				using Time_t = typename base_t::Time_t;
				mutable bool	_bChanged = true,
								_bValid;
			protected:
				void onChildAdded(const TfBase::SP& node) override {
					_bChanged = true;
					base_t::onChildAdded(node);
				}
				void onChildRemove(const TfBase::SP& node) override {
					_bChanged = true;
					base_t::onChildRemove(node);
				}
				void _setAsChanged() override {
					_bChanged = true;
				}
			public:
				SP clone() const override {
					return std::make_shared<TfNode_Static>(*this);
				}
				bool imn_refresh(Time_t t) const override {
					// 更新条件はフラグ
					if(_bChanged) {
						_bChanged = false;
						std::vector<const IModel*> pm;
						// 直下の子ノードをリストアップ
						this->template iterateDepthFirst<false>([&pm](auto& node, int depth){
							if(depth == 0)
								return spn::Iterate::StepIn;
							pm.push_back(&node);
							return spn::Iterate::Next;
						});
						if((_bValid = !pm.empty())) {
							// 境界ボリュームの更新
							auto* core = const_cast<Boundary*>(reinterpret_cast<const Boundary*>(base_t::getCore()));
							auto op = MakeBoundary<Boundary>(&pm[0], pm.size(), t);
							if((_bValid = op))
								*core = *op;
						}
						// 子ノードが無い場合は常にヒットしない
					}
					return _bValid;
				}
		};
		template <class Boundary, class Ud=spn::none_t>
		class TfNode_Dynamic : public TfNode_base<Boundary, Ud> {
			private:
				using base_t = TfNode_base<Boundary, Ud>;
				using Time_t = typename base_t::Time_t;
				using SP = typename base_t::SP;
				using Time_OP = spn::Optional<Time_t>;
				mutable Time_OP		_opTime = 0;
				mutable bool		_bValid;
			protected:
				void onChildAdded(const SP& node) override {
					_opTime = spn::none;
					base_t::onChildAdded(node);
				}
				void onChildRemove(const SP& node) override {
					_opTime = spn::none;
					base_t::onChildRemove(node);
				}
			public:
				SP clone() const override {
					return std::make_shared<TfNode_Dynamic>(*this);
				}
				bool imn_refresh(Time_t t) const override {
					// 更新時刻が古い時のみ処理を行う
					if(!_opTime || *_opTime < t) {
						_opTime = t;

						std::vector<const IModel*> pm;
						// 子ノードをリストアップ
						this->template iterateDepthFirst<false>([&pm](auto& node, int depth){
							if(depth == 0)
								return spn::Iterate::StepIn;
							pm.push_back(&node);
							return spn::Iterate::Next;
						});
						if((_bValid = !pm.empty())) {
							// 境界ボリュームの更新
							auto* core = const_cast<Boundary*>(reinterpret_cast<const Boundary*>(base_t::getCore()));
							auto op = MakeBoundary<Boundary>(&pm[0], pm.size(), t);
							if((_bValid = op))
								*core = *op;
						}
						// 子ノードが無い場合は常にヒットしない
					}
					return _bValid;
				}
		};

		#define mgr_tf2d (::boom::geo2d::TfMgr::_ref())
		class TfMgr : public spn::ResMgrA<TfBase_SP, TfMgr> {};

		using LNear = std::pair<Vec2, LinePos>;
		struct Point : Vec2, ITagP<Point> {
			// -----------------------------
			const float& bs_getArea() const;
			const float& bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			using Vec2::Vec2;
			using Vec2::distance;
			float distance(const Segment& s) const;
			LNear nearest(const Segment& s) const;
			const Vec2& support(const Vec2& dir) const;
			Point operator * (const AMat32& m) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Point& p);
		};
		using PointM = Model<Point>;

		//! 直線
		struct Line : ITagP<Line> {
			Vec2	pos, dir;

			Line() = default;
			Line(const Vec2& p, const Vec2& d);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------

			Vec2x2 nearest(const Line& st) const;
			Vec2 nearest(const Vec2& p) const;
			float distance(const Vec2& p) const;
			//! 点を線分上に置く
			Vec2 placeOnLine(const Vec2& p) const;
			//! 基準位置に対する方向ベクトルとの内積
			float posDot(const Vec2& p) const;
			Line operator * (const AMat32& m) const;
			LineDivision checkSide(const Vec2& p, float t=DOT_THRESHOLD) const;

			Vec2 support(const Vec2& dir) const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Line& l);
		};
		using LineM = Model<Line>;
		//! 半直線
		struct Ray : ITagP<Ray> {
			Vec2	pos, dir;

			Ray() = default;
			Ray(const Vec2& p, const Vec2& d);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------

			const Line& asLine() const;
			Vec2x2 nearest(const Ray& r) const;
			Vec2 nearest(const Vec2& p) const;
			Ray operator * (const AMat32& m) const;

			Vec2 support(const Vec2& dir) const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Ray& r);
		};
		using RayM = Model<Ray>;
		//! 線分
		struct Segment : ITagP<Segment> {
			Vec2	from, to;

			Segment() = default;
			Segment(const Vec2& v0, const Vec2& v1);
			// -----------------------------
			const float& bs_getArea() const {INVOKE_ERROR}
			const float& bs_getInertia() const {INVOKE_ERROR}
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			float distance(const Segment& l) const;
			float length() const;
			float len_sq() const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;

			/*! \return Vec2(最寄り座標),LINEPOS(最寄り線分位置) */
			LNear nearest(const Vec2& p) const;
			//! 線分が交差する位置を調べる
			/*! \return first: 交差座標
						second: 交差しているライン位置 (ONLINE or NOHIT) */
			Vec2_OP crossPoint(const Segment& l) const;
			Vec2_OP crossPoint(const Line& l) const;
			float ratio(const Vec2& p) const;
			Vec2 getDir() const;
			Line asLine() const;
			bool online(const Vec2& p) const;

			Segment operator * (const AMat32& m) const;
			friend std::ostream& operator << (std::ostream& os, const Segment& s);
		};
		using SegmentM = Model<Segment>;
		using AABBM = Model<AABB>;

		struct Capsule : ITagP<Capsule> {
			Vec2	from, to;
			float	radius;

			Capsule() = default;
			Capsule(const Vec2& f, const Vec2& t, float r);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			Capsule operator * (const AMat32& m) const;
			Capsule& operator += (const Vec2& ofs);

			spn::none_t hit(...) const;
			bool hit(const Vec2& p , float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;
			bool hit(const Capsule& c, float t=NEAR_THRESHOLD) const;

			friend std::ostream& operator << (std::ostream& os, const Capsule& c);
		};
		using CapsuleM = Model<Capsule>;

		struct Poly : ITagP<Poly> {
			Vec2		point[3];
			bool _isInTriangle(const Vec2& p, float threshold) const;

			Poly() = default;
			Poly(const Vec2& p0, const Vec2& p1, const Vec2& p2);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			//! 座標が三角形の内部に含まれるかを判定
			/*! ポリゴンは時計回りを想定
				辺上は含まない */
			bool isInTriangle(const Vec2& p) const;
			std::pair<Vec2,int> nearest(const Vec2& p) const;

			Vec2 support(const Vec2& dir) const;
			void addOffset(const Vec2& ofs);
			static float CalcArea(const Vec2& p0, const Vec2& p1, const Vec2& p2);
			static float CalcArea(const Vec2& p0, const Vec2& p1);
			//! 鈍角を探す
			/*! \return 鈍角の番号 (負数は該当なし) */
			int getObtuseCorner() const;
			//! 頂点の並びが時計回りかを判定
			bool isCW() const;
			//! 頂点の並びを反転
			void invert();
			LineDivision checkSide(const Line& l, float t=DOT_THRESHOLD) const;

			spn::none_t hit(...) const;
			//! 座標が三角形と衝突するか判定
			/*! isInTriangleとは異なり辺上もHitとみなす */
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Poly& p, float t=NEAR_THRESHOLD) const;
			Poly operator * (const AMat32& m) const;
			Poly& operator += (const Vec2& ofs);
			void distend(float width, float mindist);
			friend std::ostream& operator << (std::ostream& os, const Poly& p);
		};
		using PolyM = Model<Poly>;
		//! 多角形基本クラス
		struct Convex : ITagP<Convex> {
			using AreaL = std::vector<float>;
			struct AreaSum {
				float result;

				AreaSum(): result(0) {}
				void operator()(int /*n*/, const Vec2& p0, const Vec2& p1) {
					result += Poly::CalcArea(p0,p1);
				}
			};
			struct AreaList {
				AreaL	areaL;
				float	sum;

				AreaList(int n): areaL(n), sum(0) {}
				void operator()(int n, const Vec2& p0, const Vec2& p1) {
					float a = Poly::CalcArea(p0,p1);
					areaL[n] = a;
					sum += a;
				}
			};
			PointL	point;
			const static uint8_t cs_index[1<<8];

			Convex() = default;
			/*! \param[in] v 凸包と分かっている頂点 */
			Convex(std::initializer_list<Vec2> v);
			Convex(const PointL& pl);
			Convex(PointL&& pl);

			using CBPoints = std::function<void (PointL&&)>;
			using CBConvex = std::function<void (Convex&&)>;
			static void MonotoneToConvex(const PointL& pts, const Vec2& dir, const CBConvex& cb);
			static void ConcaveToMonotone(const PointL& pts, const Vec2& dir, const CBPoints& cb);
			static bool IsMonotone(const PointL& pts, const Vec2& dir);
			static bool IsConvex(const PointL& pts);
			//! 凹ポリゴンを内包するような凸形状を求める
			static Convex FromConcave(const PointL& src);
			bool addPoint(const Vec2& p);
			// -----------------------------
			float bs_getArea() const;
			Circle bs_getBCircle() const;
			Vec2 bs_getCenter() const;
			float bs_getInertia() const;
			AABB bs_getBBox() const;
			// -----------------------------
			/*! 同時に求めると少し効率が良い為 */
			std::tuple<float,float,Vec2> area_inertia_center() const;
			Vec2 support(const Vec2& dir) const;
			//! 2つに分割
			/*! \param[out] c0 線分の進行方向左側
				\param[out] c1 線分の進行方向右側 */
			Convex2 splitTwo(const Line& l) const;
			//! 2つに分割して左側を自身に適用
			Convex split(const Line& l);
			//! 2つに分割して右側は捨てる
			void splitThis(const Line& l);

			template <class CB>
			void iterate(CB cb) const {
				// 頂点数は3つ以上
				int nL = point.size();
				AssertP(Trap, nL > 2);

				// 先にブリッジの箇所を処理
				cb(nL-1, point.back(), point.front());
				for(int i=0 ; i<nL-1 ; i++)
					cb(i, point[i], point[i+1]);
			}
			void addOffset(const Vec2& ofs);

			//! 指定ポイントの内部的な領域IDと内外位置を取得
			/*! \return first=内外判定
						second=領域ID */
			std::pair<ConvexPos, int> checkPosition(const Vec2& pos, float threshold=DOT_THRESHOLD) const;
			//! 内部的な通し番号における外郭ライン
			Segment getOuterSegment(int n) const;
			Line getOuterLine(int n) const;
			std::pair<bool,PointL> getOverlappingPoints(const Convex& mdl, const Vec2& inner) const;
			static Convex GetOverlappingConvex(const Convex& m0, const Convex& m1, const Vec2& inner);
			//! 凸包が直線と交差している箇所を2点計算
			std::tuple<bool,Vec2,Vec2> checkCrossingLine(const Line& l) const;
			Convex operator * (const AMat32& m) const;
			Convex& operator *= (const AMat32& m);
			Convex& operator += (const Vec2& ofs);
			void distend(float width, float mindist);
			//! 頂点が時計回りになっているか
			bool checkCW() const;
			//! 頂点の並びを時計回りに修正
			void adjustLoop();
			int getNPoints() const;
			Vec2 getPoint(int n) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Convex& c);
		};
		using ConvexM = Model<Convex>;
		//! GJK法による衝突判定(2D)
		/*! ヒットチェックのみ。衝突時は内部点を出力可 */
		class GSimplex {
			protected:
				const IModel	&_m0, &_m1;
				Poly	_poly;			//!< 凸包を構成するポリゴン
				Vec2	_posA[3],		//!< vtx(A-B)を求める時に使ったA側の座標
						_inner;			//!< 内部点
				bool	_bHit;			//!< 衝突の有無
				int		_nVtx;			//!< 使用された頂点の数(min=1, max=3)
			private:
				void _minkowskiSub(const Vec2& dir, int n);
				void _gjkMethod();
				void _setAsHit(int nv, const Vec2& inner);
				void _setAsNotHit(int nv);
			public:
				//! 初期化 = GJKによる判定(ヒットチェックだけ)
				GSimplex(const IModel& m0, const IModel& m1);
				bool getResult() const;
				//! 衝突時: 内部点を取得
				const Vec2& getInner() const;
		};
		//! GJKで最近傍対を求める
		/*! 常に頂点リストを時計回りに保つ */
		class GEpa : public GSimplex {
			/*! ミンコフスキー差の凸形状を構成する最大頂点数(VList)
				必要ならvectorでやっても良いがとりあえず決め打ち */
			constexpr static int MAX_VERT = 0x100;
			using VPool = boost::object_pool<Vec2x2>;
			static thread_local VPool tls_vPool;
			using VList = std::array<Vec2x2*, MAX_VERT>;

			VList	_vl;
			size_t	_szVl;
			//! 新しく頂点メモリを確保
			/*! \param n 格納先のインデックス (負数なら末尾) */
			Vec2x2* _allocVert(int n=-1);
			//! 頂点ポインタからのインデックス検索
			/*! \param vp 検索する頂点ポインタ */
			Int_OP _getIndex(const Vec2x2* vp) const;

			//! 点と辺の両対応
			struct LmLen {
				float			dist;
				Vec2			dir;
				const Vec2x2	*vtx[2];	//!< index[1]==nullptrの時は単一の頂点を表す

				bool operator < (const LmLen& len) const {
					return dist < len.dist;
				}
			};
			//! 最短距離リスト
			spn::AssocVec<LmLen>	_asv;
			//! デバッグ用
			void _printASV(std::ostream& os) const;

			union {
				Vec2x2	_pvec;
				Vec2x2	_nvec;
			};
			//! v0.firstとv1.firstからなる線分候補をリストに追加
			/*!	\return 最近傍点が原点と重なっていれば0x02, 線分候補が追加されれば0x01, それ以外は0x00 */
			int _addAsv(const Vec2x2& v0, const Vec2x2& v1);

			//! 指定方向へのミンコフスキー差
			/*! \param[in] n 計算した頂点の挿入先インデックス */
			const Vec2x2& _minkowskiSub(const Vec2& dir, int n=-1);
			//! Hit時の脱出ベクトル
			/*! 最低でも3頂点以上持っている前提 */
			void _epaMethodOnHit(float threshold);
			//! NoHit時の最短ベクトル
			void _epaMethodNoHit(float threshold);

			//! NoHit: 頂点が2つしか無い時の補正
			void _recover2NoHit();
			//! Hit: 頂点が2つしか無い時の補正
			void _recover2OnHit();
			//! 頂点の並び順を時計回りに修正
			void _adjustLoop3();
			//! 頂点リスト(3つ以上)から最短距離リストを生成
			void _geneASV();
			void _clear();

			public:
				GEpa(const IModel& m0, const IModel& m1, float threshold=NEAR_THRESHOLD);
				~GEpa();
				/*! 非衝突時に有効
					\return A側の最近傍点, B側の最近傍点 */
				const Vec2x2& getNearestPair() const;
				/*! 衝突時にそれを回避するための最短移動ベクトル(A側)
					\return first=A側の最深点 second=A側の回避ベクトル */
				const Vec2x2& getPVector() const;
		};
		//! ミンコフスキー差を求める
		Vec2 MinkowskiSub(const IModel& m0, const IModel& m1, const Vec2& dir);
		//! DualTransform (point2D -> line2D)
		Line Dual(const Vec2& v);
		//! DualTransform (line2D -> point2D)
		Vec2 Dual(const Line& ls);
		Vec2 Dual2(const Vec2& v0, const Vec2& v1);
		//! DualTransform (point3D -> plane)
		Plane Dual(const Vec3& v);
		//! DualTransform (plane -> point3D)
		Vec3 Dual(const Plane& plane);

		template <class CLIP>
		inline Vec2 NearestPoint(const Line& ls, const Vec2& p, CLIP clip) {
			Vec2 toP = p - ls.pos;
			float d = ls.dir.dot(toP);
			return ls.pos + ls.dir * clip(d);
		}
		bool IsCW(const PointL& pts);
		bool IsCrossing(const Line& ls0, const Line& ls1, float len0, float len1, float t=NEAR_THRESHOLD);
		template <class CLIP0, class CLIP1>
		inline Vec2x2_OP NearestPoint(const Line& ls0, const Line& ls1, CLIP0 clip0, CLIP1 clip1) {
			float st0d = ls0.dir.len_sq(),
					st1d = ls1.dir.len_sq(),
					st01d = ls0.dir.dot(ls1.dir);
			float d = st0d * st1d - spn::Square(st01d);
			if(std::fabs(d) < 1e-5f) {
				// 2つの直線は平行
				return spn::none;
			}
			Mat22 m0(st1d, st01d,
					st01d, st0d);
			Vec2	m1((ls1.pos - ls0.pos).dot(ls0.dir),
						(ls0.pos - ls1.pos).dot(ls1.dir));
			m1 = m0 * m1;
			m1 *= spn::Rcp22Bit(d);
			return Vec2x2(ls0.pos + ls0.dir * clip0(m1.x),
							ls1.pos + ls1.dir * clip1(m1.y));
		}

		struct Types {
			using CTGeo = ::boom::geo2d::CTGeo;
			using IModel = ::boom::geo2d::IModel;
			using GJK = ::boom::geo2d::GSimplex;
			using Narrow = ::boom::Narrow<Types>;
		};
	}
}