#ifndef STAN_MATH_TORSTEN_EVENT_SOLVER_HPP
#define STAN_MATH_TORSTEN_EVENT_SOLVER_HPP

#include <stan/math/prim/fun/multiply.hpp>
#include <stan/math/rev/fun/multiply.hpp>
#include <stan/math/torsten/dsolve/pk_vars.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_integrator.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/model_factory.hpp>
#include <stan/math/torsten/event.hpp>
#include <stan/math/torsten/mpi/session.hpp>
#include <stan/math/torsten/mpi/my_worker.hpp>
#include <stan/math/torsten/mpi/precomputed_gradients.hpp>
#include <Eigen/Dense>
#include <vector>

namespace torsten{

  template<typename T_model, typename T_em>
  struct EventSolver;

  /*
   * the solver wrapper is aware of @c T_model so it build model
   * accordingly.
   */
  template<typename T_model, typename T_event_record,
           typename T0, typename T4, template<typename...> class theta_container,
           typename... event_ctrl_type,
           typename... ode_data_type>
  struct EventSolver<T_model,
                     EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                      std::tuple<event_ctrl_type...>,
                                                                      ode_data_type...>> > {
    using EM = EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                std::tuple<event_ctrl_type...>,
                                                                ode_data_type...>>;
    using T_param = NonEventParameters<T0, T4, theta_container,
                                       std::tuple<event_ctrl_type...>,
                                       ode_data_type...>;
    /*
     * Data used to fill the results when computation throws exception.
     */
    static constexpr double invalid_res_d = std::numeric_limits<double>::quiet_NaN();

    /*
     * calculate the number of parameters according to the
     * model type.
     *
     * @tparam T_er type of events record
     * @param id subject id among a population
     * @param rec events record for the population
     */
    static int system_size(int id, const T_event_record& rec,
                           const std::vector<theta_container<T4>>& theta) {
      const int ncmt = rec.ncmt;
      const int npar = theta[0].size();
      const int nvar = pmx_model_nvars<T_model,
                                       typename EM::T_time,
                                       typename EM::T_scalar,
                                       typename EM::T_rate>::nvars(ncmt, npar);
      const int nvar_ss = pmx_model_nvars_ss<T_model,
                                             typename EM::T_amt,
                                             typename EM::T_par_rate,
                                             typename EM::T_par_ii>::nvars(npar);
      return rec.has_ss_dosing(id) ? pk_nsys(ncmt, nvar, nvar_ss) : pk_nsys(ncmt, nvar);
    }

    /**
     * Every Torsten function calls Pred.
     *
     * Predicts the amount in each compartment for each event,
     * given the event schedule and the parameters of the model.
     *
     * Proceeds in two steps. First, computes all the events that
     * are not included in the original data set but during which
     * amounts in the system get updated. Secondly, predicts
     * the amounts in each compartment sequentially by going
     * through the augmented schedule of events. The returned pred
     * Matrix only contains the amounts in the event originally
     * specified by the users.
     *
     * This function is valid for all models. What changes from one
     * model to the other are the Pred1 and PredSS functions, which
     * calculate the amount at an individual event.
     *
     * @tparam T_em type the @c EventsManager
     * @param[in] time times of events
     * @param[in] amt amount at each event
     * @param[in] rate rate at each event
     * @param[in] ii inter-dose interval at each event
     * @param[in] evid event identity:
     *                    (0) observation
     *                    (1) dosing
     *                    (2) other
     *                    (3) reset
     *                    (4) reset AND dosing
     * @param[in] cmt compartment number at each event (starts at 1)
     * @param[in] addl additional dosing at each event
     * @param[in] ss steady state approximation at each event
     * (0: no, 1: yes)
     * @param[in] pMatrix parameters at each event
     * @param[in] addParm additional parameters at each event
     * @parem[in] model basic info for ODE model and evolution operators
     * @param[in] SystemODE matrix describing linear ODE system that
     * defines compartment model. Used for matrix exponential solutions.
     * Included because it may get updated in modelParameters.
     * @return a matrix with predicted amount in each compartment
     * at each event.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(int id,
              const T_event_record& events_rec,
              Eigen::Matrix<typename EM::T_scalar, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::Dynamic;
      using std::vector;
      using torsten::PKRec;

      int nCmt = EM::nCmt(events_rec);

      res.resize(nCmt, events_rec.num_event_times(id));
      PKRec<typename EM::T_scalar> init(nCmt);
      init.setZero();

      try {
        EM em(id, events_rec, theta, event_ctrl..., ode_data...);
        int ikeep = 0, iev = 0;

        while(ikeep < em.nKeep) {
          stepper(iev, init, em, integrator, scalar_pars...);
          if(em.events().keep(iev)) {
            res.col(ikeep) = init;
            ikeep++;
          }
          iev++;
        }
      } catch (const std::exception& e) {
        throw;
      }
    }

    /*
     * Step through a range of events.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void stepper(int i, PKRec<typename EM::T_scalar>& init, const EM& em,
                 const integrator_type& integrator,
                 const scalar_pars_type... scalar_pars) {
      auto& events = em.events();
      using scalar = typename EM::T_scalar;
      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                              par_index_seq<ode_data_type...>{},
                                                                              scalar_pars...)};
      auto ev = em.event(i);
      ev(init, pkmodel, integrator);
    }

    /**
     * Solve an event from event manager and store the result in both
     * data format(solution & gradient) and var format.
     *
     * @param i id of the event to be solved
     * @param init var solution
     * @param sol_d data solution
     * @param em event manager
     * @param integrator numerical integrator for ODE
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void stepper_solve(int i, torsten::PKRec<typename EM::T_scalar>& init,
                       torsten::PKRec<double>& sol_d,
                       const EM& em,
                       const integrator_type& integrator,
                       const scalar_pars_type... scalar_pars) {
      using std::vector;
      using stan::math::var;

      auto& events = em.events();

      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                              par_index_seq<ode_data_type...>{},
                                                                              scalar_pars...)};
      auto ev = em.event(i);
      ev(sol_d, init, pkmodel, integrator, scalar_pars...);
    }

    template<typename integrator_type, typename... scalar_pars_type>
    /**
     * For MPI solutions, after each rank solves its corresponding
     * subjects, all ranks sync their results among each other.
     *
     * @param i id of the event to be synced
     * @param init var solution
     * @param sol_d data solution
     * @param em event manager
     * @param integrator numerical integrator for ODE
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    void stepper_sync(int i, torsten::PKRec<typename EM::T_scalar>& init,
                      torsten::PKRec<double>& sol_d,
                      const EM& em,
                      const integrator_type& integrator,
                      const scalar_pars_type... scalar_pars) {
      using std::vector;
      using stan::math::var;

      auto& events = em.events();

      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      if (events.is_reset(i)) {
        init.setZero();
      } else if (events.is_ss_dosing(i)) {  // steady state event
        T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                                par_index_seq<ode_data_type...>{},
                                                                                scalar_pars...)};
        auto curr_amt = em.fractioned_amt(i);
        vector<var> v_i(dsolve::pk_vars(curr_amt, events.rate(i), events.ii(i), pkmodel.par()));
        int nsys = torsten::pk_nsys(em.ncmt, v_i.size());
        if (events.ss(i) == 2)
          init += torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);  // steady state without reset
        else
          init = torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);  // steady state with reset (ss = 1)
      } else if (events.time(i) > tprev) {
        // auto curr_rates = stan::math::value_of(em.fractioned_rates(i));
        auto curr_rates = em.fractioned_rates(i);
          T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                                par_index_seq<ode_data_type...>{},
                                                                                scalar_pars...)};
          vector<var> v_i =
            pmx_model_vars<T_model>::vars(events.time(i), init, curr_rates, pkmodel.par());
          int nsys = torsten::pk_nsys(em.ncmt, v_i.size());
          init = torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);
      }

      if (events.is_bolus_dosing(i)) {
        init(events.cmt(i) - 1) += em.fractioned_amt(i);
      }
    }

#ifdef TORSTEN_MPI

    /*
     * MPI solution when the population
     * information passed in as ragged arrays. The overloading occurs
     * on res arg for results. Here the result is var.
     *
     * @param events_rec event record
     * @param res solution
     * @param integrator ODE integrator
     * @param theta PMX parameters passed into ODE function
     * @param event_ctrl optional PMX parameters: bioavailability & tlag
     * @param ode_data optional ODE parameters: real & integer data
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(const T_event_record& events_rec,
              Eigen::Matrix<stan::math::var, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::MatrixXd;
      using Eigen::VectorXd;
      using Eigen::Dynamic;
      using std::vector;
      using::stan::math::var;
      using torsten::PKRec;

      using scalar = typename EM::T_scalar;

      const int nCmt = EM::nCmt(events_rec);
      const int np = events_rec.num_subjects();
      bool is_invalid = false;
      std::ostringstream rank_fail_msg;

      const MPI_Comm& comm = torsten::mpi::Session::pmx_parm_comm().comm();
      int rank = torsten::mpi::Session::pmx_parm_comm().rank();
      int size = torsten::mpi::Session::pmx_parm_comm().size();

      std::vector<MPI_Request> req(np);
      vector<MatrixXd> res_d(np);

      res.resize(nCmt, events_rec.total_num_event_times);

      PKRec<scalar> init(nCmt);
      PKRec<double> pred1;
      for (int id = 0; id < np; ++id) {

        /* For every rank */

        const int nKeep = events_rec.num_event_times(id);

        int nev = EM::num_events(id, events_rec, theta, event_ctrl..., ode_data...);
        res_d[id].resize(system_size(id, events_rec, theta), nev);
        res_d[id].setConstant(0.0);

        int my_worker_id = torsten::mpi::my_worker(id, np, size);

        /* only solver rank */

        if (rank == my_worker_id) {
          if (is_invalid) {
            res_d[id].setConstant(invalid_res_d);
          } else {
            try {
              EM em(id, events_rec, theta, event_ctrl..., ode_data...);
              auto& events = em.events();
              assert(nev == events.size());
              assert(nKeep == em.nKeep);
              init.setZero();

              int ikeep = 0, iev = 0;
              while(ikeep < em.nKeep) {
                stepper_solve(iev, init, pred1, em, integrator, scalar_pars...);
                res_d[id].col(iev).segment(0, pred1.size()) = pred1;
                if(events.keep(iev)) {
                  res.col(EM::begin(id, events_rec) + ikeep) = init;
                  ikeep++;
                }
                iev++;
              }
            } catch (const std::exception& e) {
              is_invalid = true;
              res_d[id].setConstant(invalid_res_d);
              rank_fail_msg << "Rank " << rank << " failed to solve id " << id << ": " << e.what();
            }
          }
        }
        MPI_Ibcast(res_d[id].data(), res_d[id].size(), MPI_DOUBLE, my_worker_id, comm, &req[id]);
      }

      // int finished = 0;
      // int index;
      // int flag = 0;
      // for(int id = 0; id < np; ++id) {
      //   MPI_Wait(&req[id], MPI_STATUS_IGNORE);

      //   if (is_invalid) continue;
      //   if (std::isnan(res_d[id](0))) {
      //     assert(rank != torsten::mpi::my_worker(id, np, size));
      //     is_invalid = true;
      //     rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
      //   } else {
      //     EM em(id, events_rec);
      //     auto& events = em.events();
      //     PKRec<scalar> init(nCmt); init.setZero();
      //     PKRec<double> pred1 = VectorXd::Zero(res_d[id].rows());

      //     int ikeep = 0, iev = 0;
      //     while(ikeep < em.nKeep) {
      //       pred1 = res_d[id].col(iev);
      //       stepper_sync(iev, init, pred1, em, integrator, scalar_pars...);
      //       if(events.keep(iev)) {
      //         res.col(EM::begin(id, events_rec) + ikeep) = init;
      //         ikeep++;
      //       }
      //       iev++;
      //     }
      //   }
      // }

      int finished = 0;
      int index = 0;
      int flag = 0;
      while (finished < np) {
        MPI_Test(&req[index], &flag, MPI_STATUS_IGNORE);
        if (flag) {
          int id = index;
          index++;
          finished++;
          if (is_invalid) continue;
          if (std::isnan(res_d[id](0))) {
            // assert(rank != torsten::mpi::my_worker(id, np, size));
            is_invalid = true;
            rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
          } else {
            EM em(id, events_rec, theta, event_ctrl..., ode_data...);
            auto& events = em.events();
            PKRec<scalar> init(nCmt); init.setZero();
            PKRec<double> pred1 = VectorXd::Zero(res_d[id].rows());

            int ikeep = 0, iev = 0;
            while(ikeep < em.nKeep) {
              pred1 = res_d[id].col(iev);
              stepper_sync(iev, init, pred1, em, integrator, scalar_pars...);
              if(events.keep(iev)) {
                res.col(EM::begin(id, events_rec) + ikeep) = init;
                ikeep++;
              }
              iev++;
            }
          }
        }
      }

      MPI_Barrier(comm);

      if(is_invalid) {
        throw std::runtime_error(rank_fail_msg.str());
      }
    }

    /*
     * Data-only MPI solver that takes ragged arrays as input.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(const T_event_record& events_rec, Eigen::MatrixXd& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::MatrixXd;
      using Eigen::VectorXd;
      using Eigen::Dynamic;
      using std::vector;
      using::stan::math::var;
      using torsten::PKRec;

      using ER = NONMENEventsRecord<double, double, double, double>;
      using EM_d = EventsManager<ER, NonEventParameters<T0, T4, theta_container, std::tuple<event_ctrl_type...>, ode_data_type...>>;

      const int nCmt = EM_d::nCmt(events_rec);
      const int np = events_rec.num_subjects();
      bool is_invalid = false;
      std::ostringstream rank_fail_msg;

      const MPI_Comm& comm = torsten::mpi::Session::pmx_data_comm().comm();
      int rank = torsten::mpi::Session::pmx_data_comm().rank();
      int size = torsten::mpi::Session::pmx_data_comm().size();

      std::vector<MPI_Request> req(np);

      res.resize(nCmt, events_rec.total_num_event_times);

      PKRec<double> init(nCmt);
      for (int id = 0; id < np; ++id) {

        /* For every rank */
        int nKeep = events_rec.num_event_times(id);
        int my_worker_id = torsten::mpi::my_worker(id, np, size);
        int begin_id = EM_d::begin(id, events_rec) * nCmt;
        int size_id = nKeep * nCmt;

        /* only solver rank */
        if (rank == my_worker_id) {
          try {
            EM em(id, events_rec, theta, event_ctrl..., ode_data...);
            auto& events = em.events();
            init.setZero();
            int ikeep = 0, iev = 0;
            while(ikeep < em.nKeep) {
              stepper(iev, init, em, integrator, scalar_pars...);
              if(events.keep(iev)) {
                res.col(EM_d::begin(id, events_rec) + ikeep) = init;
                ikeep++;
              }
              iev++;
            }
          } catch (const std::exception& e) {
            is_invalid = true;
            res(begin_id) = invalid_res_d;
            rank_fail_msg << "Rank " << rank << " failed to solve id " << id << ": " << e.what();
          }
        }

        MPI_Ibcast(res.data() + begin_id, size_id, MPI_DOUBLE, my_worker_id, comm, &req[id]);
      }

      // make sure every rank throws in case any rank fails
      int finished = 0;
      int index = 0;
      int flag = 0;
      while (finished < np && size > 1) {
        MPI_Testany(np, req.data(), &index, &flag, MPI_STATUS_IGNORE);
        if (flag) {
          finished++;
          if(is_invalid) continue;
          int id = index;
          int begin_id = EM_d::begin(id, events_rec) * nCmt;
          if (std::isnan(res(begin_id))) {
            is_invalid = true;
            rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
          }
        }
      }

      MPI_Barrier(comm);

      if(is_invalid) {
        MPI_Barrier(comm);
        throw std::runtime_error(rank_fail_msg.str());
      }
    }
#else

    /*
     * For population input in the form of ragged arrays,
     * addional information of the size of each individual
     * is required to locate the data in a single array for population.
     */
    template<typename integrator_type, typename... scalar_pars_type> //NOLINT
    void pred(const T_event_record& events_rec,
              Eigen::Matrix<typename EM::T_scalar, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using ER = T_event_record;

      const int nCmt = EM::nCmt(events_rec);
      const int np = events_rec.num_subjects();

      res.resize(nCmt, events_rec.total_num_event_times);

      static bool has_warning = false;
      if (!has_warning) {
        std::cout << "Torsten Population PK solver " << "running sequentially" << "\n";
        has_warning = true;
      }

      for (int id = 0; id < np; ++id) {
        const int nKeep = events_rec.num_event_times(id);
        Eigen::Matrix<typename EM::T_scalar, -1, -1> res_id(nCmt, nKeep);
        pred(id, events_rec, res_id, integrator, theta,
             event_ctrl..., ode_data..., scalar_pars...);
        for (int j = 0; j < nKeep; ++j) {
          res.col(EM::begin(id, events_rec) + j) = res_id.col(j);
        }
      }
    }
#endif
  };

  template<typename T_model, typename T_event_record,
           typename T0, typename T4, template<typename...> class theta_container,
           typename... event_ctrl_type,
           typename... ode_data_type>
  constexpr double EventSolver<T_model,
                               EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                                std::tuple<event_ctrl_type...>,
                                                                                ode_data_type...>> >::invalid_res_d;
}
#endif


#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/OrthotropicHomogenization.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <MeshFEM/TensorProjection.hh>
#include <vector>

#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
using namespace std;
using namespace PeriodicHomogenization;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: OptimizeLinkage_cli [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m", po::value<string>(),                 "base material")
        ("degree,d",   po::value<int>()->default_value(2), "degree of finite elements")
        ("fieldOutput,o",po::value<string>(),              "Dump fluctuation stress and strain fields to specified msh file")
        ("centerFluctuationDisplacements,c",               "Shift each fluctuation displacement so that it averages to zero")
        ("fullDegreeFieldOutput,D",                        "Output full-degree nodal fields (don't do piecewise linear subsample)")
        ("distanceToIsotropy",                             "Output the distance to the closest isotropic tensor")
        ("distanceToMaterial", po::value<string>(),        "Output the distance to a particular material")
        ("ignorePeriodicMismatch",                         "Ignore mismatched nodes on the periodic faces (useful for voxel grids)")
        ("manualPeriodicVertices", po::value<string>(),    "Manually specify identified periodic vertices using a hacky file format (see PeriodicCondition constructor)")
        ("orthotropicCell,O",                              "Analyze the orthotropic symmetry base cell only")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    int d = vm["degree"].as<int>();
    if (d < 1 || d > 2) {
        cout << "Error: FEM Degree must be 1 or 2" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

// Sum the values appearing on periodically-identified vertices.
// First sum onto the reduced DoFs, then redistribute.
template<class Sim, class VField>
VField sumIdentifiedValues(const Sim &sim, VField v) {
    const auto &mesh = sim.mesh();
    if (v.domainSize() != mesh.numNodes())
        throw std::runtime_error("Expected per-node vector field");

    VField dofField(sim.numDoFs());
    dofField.clear();
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        dofField(sim.DoF(i)) +=  v(i);
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        v(i) = dofField(sim.DoF(i));

    return v;
}

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<size_t _N, size_t _FEMDegree>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    auto &mat = HMG<_N>::material;
    if (args.count("material")) mat.setFromFile(args["material"].as<string>());

    typedef LinearElasticity::Mesh<_N, _FEMDegree, HMG> Mesh;
    typedef LinearElasticity::Simulator<Mesh> Simulator;
    Simulator sim(inElements, inVertices);
    typedef typename Simulator::ETensor ETensor;
    typedef typename Simulator::VField  VField;

    BENCHMARK_START_TIMER_SECTION("Cell Problems");
    std::vector<VField> w_ij;
    std::unique_ptr<PeriodicCondition<_N>> pc;
    ETensor Eh;
    for (size_t it = 0; it < 20; ++it) {
        if (args.count("manualPeriodicVertices"))
            pc = Future::make_unique<PeriodicCondition<_N>>(sim.mesh(), args["manualPeriodicVertices"].as<string>());
        if (args.count("orthotropicCell") == 0) {
            solveCellProblems(w_ij, sim, 1e-7, args.count("ignorePeriodicMismatch"), std::move(pc));
        }
        else {
            auto systems = PeriodicHomogenization::Orthotropic::solveCellProblems(w_ij, sim, 1e-7);
            cout << systems.size() << endl;
        }

        BENCHMARK_STOP_TIMER_SECTION("Cell Problems");

        BENCHMARK_START_TIMER_SECTION("Compute Tensor");
        // ETensor Eh = homogenizedElasticityTensor(w_ij, sim);
        if (args.count("orthotropicCell") == 0)   Eh = homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        else Eh = PeriodicHomogenization::Orthotropic::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        BENCHMARK_STOP_TIMER_SECTION("Compute Tensor");

        cout << setprecision(16);
        cout << "Homogenized elasticity tensor:" << endl;
        cout << Eh << endl << endl;

        auto eigs = Eh.computeEigenstrains();
        // Make all eigenstrains positive
        if (eigs.strains(0, 0) < 0) eigs.strains.col(0) *= -1;
        if (eigs.strains(0, 1) < 0) eigs.strains.col(1) *= -1;
        if (eigs.strains(0, 2) < 0) eigs.strains.col(2) *= -1;

        cout << "Minimum Eh eigenvalue " << eigs.lambdas[0] << " for eigenstrain: "
             << eigs.strains.col(0).transpose() << endl;

        SymmetricMatrixValue<Real, _N> minEigenstrain(eigs.strains.col(0));
        SymmetricMatrixValue<Real, _N> midEigenstrain(eigs.strains.col(1));
        SymmetricMatrixValue<Real, _N> maxEigenstrain(eigs.strains.col(2));

        ETensor Eh_pinv = Eh.pseudoinverse();
        // cout << "Eh : Eh_pinv: " << endl << Eh.doubleContract(Eh_pinv) << endl << endl;
        // cout << "Eh : Eh_pinv : Eh " << endl << Eh.doubleContract(Eh_pinv).doubleContract(Eh) << endl << endl;

        // Compute discrete shape derivative of minimum eigenvalue and
        // one of the eigenstrain components
        // (to steer away from the uniform expansion mode).
        auto dEh = homogenizedElasticityTensorDiscreteDifferential(w_ij, sim);
        // Work with vector fields instead of OneForms; we probably can't compute a
        // proper Riesz representative on non-manifold meshes anyway, so don't
        // worry about distinction between one-forms and vector fields.
        VField dMinEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return minEigenstrain.doubleContract(E.doubleContract(minEigenstrain)); }, dEh).asVectorField());
        VField dMidEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return midEigenstrain.doubleContract(E.doubleContract(midEigenstrain)); }, dEh).asVectorField());
        VField dMaxEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return maxEigenstrain.doubleContract(E.doubleContract(maxEigenstrain)); }, dEh).asVectorField());

        VField dMinEigenstrainC0 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(0, 0); }, dEh).asVectorField());
        VField dMinEigenstrainC1 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(1, 1); }, dEh).asVectorField());
        VField dMinEigenstrainC2 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(0, 1); }, dEh).asVectorField());

        bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;
        MSHFieldWriter writer("vertical_linkage_it" + std::to_string(it) + ".msh", sim.mesh(), linearSubsampleFields);
        writer.addField("dE00", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(0,0); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE01", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(0,1); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE11", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(1,1); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE22", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(2,2); }, dEh).asVectorField()), DomainType::PER_NODE);

        writer.addField("dMinEigenvalue", dMinEigenvalue, DomainType::PER_NODE);
        writer.addField("dMidEigenvalue", dMidEigenvalue, DomainType::PER_NODE);
        writer.addField("dMaxEigenvalue", dMaxEigenvalue, DomainType::PER_NODE);

        writer.addField("dMinEigenstrainC0", dMinEigenstrainC0, DomainType::PER_NODE);
        writer.addField("dMinEigenstrainC1", dMinEigenstrainC1, DomainType::PER_NODE);
        writer.addField("dMinEigenstrainC2", dMinEigenstrainC2, DomainType::PER_NODE);

        VField descentStep = dMinEigenstrainC1;
        descentStep.maxColumnNormalize();
        descentStep *= 0.01;
        std::vector<MeshIO::IOVertex> pts(sim.mesh().numVertices());
        for (auto v : sim.mesh().vertices()) {
            pts[v.index()] = v.node()->p;
            pts[v.index()].point += padTo3D(PointND<_N>(descentStep(v.index())));
        }
        sim.updateMeshNodePositions(pts);
    }

    ETensor S = Eh.inverse();
    cout << "Homogenized compliance tensor:" << endl;
    cout << S << endl;
    vector<Real> moduli(flatLen(_N));

    // Shear moduli are multiplied by 4 in flattened compliance tensor...
    for (size_t i = 0; i < flatLen(_N); ++i)
        moduli[i] = ((i < _N) ? 1.0 : 0.25) / S.D(i, i);

    vector<Real> poisson;
    if (_N == 2) poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(1, 0) / S.D(0, 0) }; // v_xy
    else         poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(0, 2) / S.D(2, 2),   // v_zx
                             -S.D(1, 2) / S.D(2, 2),   // v_zy
                             -S.D(1, 0) / S.D(0, 0),   // v_xy
                             -S.D(2, 0) / S.D(0, 0),   // v_xz
                             -S.D(2, 1) / S.D(1, 1) }; // v_zy

    if (_N == 2)  {
        cout << "Approximate Young moduli:\t"  << moduli[0] << "\t" << moduli[1] << endl;
        cout << "Approximate shear modulus:\t" << moduli[2] << endl;

        cout << "v_yx, v_xy:\t" << poisson[0] << "\t" << poisson[1] << endl;
    }
    else {
        cout << "Approximate Young moduli:\t" << moduli[0] << "\t" << moduli[1] << "\t"
             << moduli[2] << endl;
        cout << "Approximate shear moduli:\t" << moduli[3] << "\t" << moduli[4] << "\t"
             << moduli[5] << endl;

        cout << "v_yx, v_zx, v_zy:\t" << poisson[0] << "\t" << poisson[1] << "\t" << poisson[2] << endl;
        cout << "v_xy, v_xz, v_yz:\t" << poisson[3] << "\t" << poisson[4] << "\t" << poisson[5] << endl;
    }

    cout << "Anisotropy:\t" << Eh.anisotropy() << endl;

    if (args.count("fieldOutput")) {
        bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;
        MSHFieldWriter writer(args["fieldOutput"].as<string>(), sim.mesh(),
                              linearSubsampleFields);
        if (args.count("centerFluctuationDisplacements")) {
            for (size_t i = 0; i < w_ij.size(); ++i) {
                auto &w = w_ij[i];
                VectorND<_N> total(VectorND<_N>::Zero());
                for (size_t ii = 0; ii < w.domainSize(); ++ii) total += w(ii);
                total *= 1.0 / w.domainSize();
                for (size_t ii = 0; ii < w.domainSize(); ++ii) w(ii) -= total;
            }
        }
        for (size_t i = 0; i < w_ij.size(); ++i) {
            writer.addField("load_ij " + to_string(i), sim.dofToNodeField(sim.constantStrainLoad(-Simulator::SMatrix::CanonicalBasis(i))), DomainType::PER_NODE);
            writer.addField("w_ij " + to_string(i), w_ij[i], DomainType::PER_NODE);
            if ((Simulator::Strain::Deg == 0) || linearSubsampleFields) {
                // Output constant (average) strain when we're outputting piecewise
                // linear solutions.
                writer.addField("strain w_ij " + to_string(i),
                        sim.averageStrainField(w_ij[i]), DomainType::PER_ELEMENT);
            }
            else {
                // Output full-degree per-element strain. (Wasteful since
                // strain fields are of degree - 1, but Gmsh/MSHFieldWriter
                // only supports full-degree ElementNodeData).
                auto strainField = sim.strainField(w_ij[i]);
                typedef SymmetricMatrixInterpolant<typename Simulator::SMatrix,
                                               _N, _FEMDegree> UpsampledStrain;
                vector<UpsampledStrain> upsampledStrainField;
                upsampledStrainField.reserve(strainField.size());
                for (const auto s: strainField)
                    upsampledStrainField.emplace_back(s);
                writer.addField("strain w_ij " + to_string(i),
                                upsampledStrainField, DomainType::PER_ELEMENT);
            }
        }
    }

    if (args.count("distanceToIsotropy")) {
        auto isoFit = closestIsotropicTensor(Eh);
        cout << endl;
        cout << "(Sq Rel Frob) Distance to Isotropy:\t" << (isoFit - Eh).frobeniusNormSq() / isoFit.frobeniusNormSq() << endl;
        cout << "Closest isotropic tensor:" << endl << isoFit << endl;
        cout << endl;
    }

    if (args.count("distanceToMaterial")) {
        Materials::Constant<_N> targetMat(args.at("distanceToMaterial").as<string>());
        auto tgtE = targetMat.getTensor();
        cout << "(Sq Rel Frob) Distance to Specified Tensor:\t" << (Eh - tgtE).frobeniusNormSq() / tgtE.frobeniusNormSq() << endl;
    }

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();
    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS,
                     MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    size_t dim;
    if      (type == MeshIO::MESH_TET) dim = 3;
    else if (type == MeshIO::MESH_TRI) dim = 2;
    else    throw std::runtime_error("Mesh must be triangle or tet.");

    // Look up and run appropriate homogenizer instantiation.
    int deg = args["degree"].as<int>();
    auto exec = (dim == 3) ? ((deg == 2) ? execute<3, 2> : execute<3, 1>)
                           : ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


//-*- mode: c++; indent-tabs-mode: nil; tab-width: 2 -*-

#include "ug_bitext.h"
#include <algorithm>
#include <boost/math/distributions/binomial.hpp>

namespace sapt
{

  float
  lbop(size_t const tries, size_t const succ, float const confidence)
  {
    return (confidence == 0
            ? float(succ)/tries
            : (boost::math::binomial_distribution<>::
               find_lower_bound_on_p(tries, succ, confidence)));
  }

  void
  snt_adder<L2R_Token<SimpleWordId> >::
  operator()()
  {
    typedef L2R_Token<SimpleWordId> tkn;
    std::vector<id_type> sids; sids.reserve(snt.size());
    BOOST_FOREACH(std::string const& foo, snt)
      {
        sids.push_back(track ? track->size() : 0);
        std::istringstream buf(foo);
        std::string w;
        std::vector<tkn> s; s.reserve(100);
        while (buf >> w) s.push_back(tkn(V[w]));
        track = append(track,s);
      }
    if (index)
      index.reset(new imTSA<tkn>(*index,track,sids,V.tsize()));
    else
      index.reset(new imTSA<tkn>(track,NULL,NULL));
  }

  snt_adder<L2R_Token<SimpleWordId> >::
  snt_adder(std::vector<std::string> const& s, TokenIndex& v,
            SPTR<imTtrack<L2R_Token<SimpleWordId> > >& t,
            SPTR<imTSA<L2R_Token<SimpleWordId> > >& i)
    : snt(s), V(v), track(t), index(i)
  { }

  bool
  expand_phrase_pair
  (std::vector<std::vector<ushort> >& a1,
   std::vector<std::vector<ushort> >& a2,
   ushort const s2, // next word on in target side
   ushort const L1, ushort const R1, // limits of previous phrase
   ushort & s1, ushort & e1, ushort& e2) // start/end src; end trg
  {
    if (a2[s2].size() == 0)
      {
        std::cout << __FILE__ << ":" << __LINE__ << std::endl;
        return false;
      }
    bitvector done1(a1.size());
    bitvector done2(a2.size());
    std::vector<std::pair<ushort,ushort> > agenda;
    // x.first:  side (1 or 2)
    // x.second: word position
    agenda.reserve(a1.size() + a2.size());
    agenda.push_back(std::pair<ushort,ushort>(2,s2));
    e2 = s2;
    s1 = e1 = a2[s2].front();
    if (s1 >= L1 && s1 < R1)
      {
        std::cout << __FILE__ << ":" << __LINE__ << std::endl;
        return false;
      }
    agenda.push_back(std::pair<ushort,ushort>(2,s2));
    while (agenda.size())
      {
        ushort side = agenda.back().first;
        ushort p    = agenda.back().second;
        agenda.pop_back();
        if (side == 1)
          {
            done1.set(p);
            BOOST_FOREACH(ushort i, a1[p])
              {
                if (i < s2)
                  {
                    // cout << __FILE__ << ":" << __LINE__ << endl;
                    return false;
                  }
                if (done2[i]) continue;
                for (;e2 <= i;++e2)
                  if (!done2[e2])
                    agenda.push_back(std::pair<ushort,ushort>(2,e2));
              }
          }
        else
          {
            done2.set(p);
            BOOST_FOREACH(ushort i, a2[p])
              {
                if ((e1 < L1 && i >= L1) ||
                    (s1 >= R1 && i < R1) ||
                    (i >= L1 && i < R1))
                  {
                    // cout << __FILE__ << ":" << __LINE__ << " "
                    // << L1 << "-" << R1 << " " << i << " "
                    // << s1 << "-" << e1<< endl;
                    return false;
                  }

                if (e1 < i)
                  {
                    for (; e1 <= i; ++e1)
                      if (!done1[e1])
                        agenda.push_back(std::pair<ushort,ushort>(1,e1));
                  }
                else if (s1 > i)
                  {
                    for (; i <= s1; ++i)
                      if (!done1[i])
                        agenda.push_back(std::pair<ushort,ushort>(1,i));
                  }
              }
          }
      }
    ++e1;
    ++e2;
    return true;
  }

  void
  print_amatrix(std::vector<std::vector<ushort> > a1, uint32_t len2,
                ushort b1, ushort e1, ushort b2, ushort e2)
  {
    using namespace std;
    std::vector<bitvector> M(a1.size(),bitvector(len2));
    for (ushort j = 0; j < a1.size(); ++j)
      {
        BOOST_FOREACH(ushort k, a1[j])
          M[j].set(k);
      }
    cout << b1 << "-" << e1 << " " << b2 << "-" << e2 << endl;
    cout << "   ";
    for (size_t c = 0; c < len2;++c)
      cout << c%10;
    cout << endl;
    for (size_t r = 0; r < M.size(); ++r)
      {
        cout << setw(3) << r << " ";
        for (size_t c = 0; c < M[r].size(); ++c)
          {
            if ((b1 <= r) && (r < e1) && b2 <= c && c < e2)
              cout << (M[r][c] ? 'x' : '-');
            else cout << (M[r][c] ? 'o' : '.');
          }
        cout << endl;
      }
    cout  << std::string(90,'-') << endl;
  }

  void
  write_bitvector(bitvector const& v, std::ostream& out)
  {
    for (size_t i = v.find_first(); i < v.size();)
      {
        out << i;
        if ((i = v.find_next(i)) < v.size()) out << ",";
      }
  }

}


//! ベクトルクラス基底ヘッダ
/*! 2-4次元のベクトルをアラインメント有りと無しでそれぞれ定義
	初期化や要素毎のアクセスなど基本的なメソッド */
#if !BOOST_PP_IS_ITERATING
	#if !defined(VECTOR_H_) || INCLUDE_VECTOR_LEVEL >= 1
		#define VECTOR_H_
		#include <boost/preprocessor.hpp>
		#include <boost/operators.hpp>
		#include <boost/serialization/access.hpp>
		#include <boost/serialization/level.hpp>
		#include <boost/serialization/nvp.hpp>
		#include <cmath>
		#include "spn_math.hpp"
		#include "tostring.hpp"
		#include "random/vector.hpp"

		#define DEF_ARGPAIR(index,data,elem)	(float BOOST_PP_CAT(f, elem))
		#define ENUM_ARGPAIR(subseq) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_FOR_EACH(DEF_ARGPAIR, NOTHING, subseq))
		#define DEF_ARGSET(index,data,elem)		elem = BOOST_PP_CAT(f, elem);
		#define SEQ_VECELEM (x)(y)(z)(w)

		// 要求された定義レベルを実体化
		#ifndef INCLUDE_VECTOR_LEVEL
			#define INCLUDE_VECTOR_LEVEL 0
		#endif
		#define BOOST_PP_ITERATION_PARAMS_1 (4, (2,4, "vector.hpp", INCLUDE_VECTOR_LEVEL))
		#include BOOST_PP_ITERATE()
		#undef INCLUDE_VECTOR_LEVEL
	#endif
#elif BOOST_PP_ITERATION_DEPTH() == 1
	// アラインメントイテレーション
	#define DIM		BOOST_PP_FRAME_ITERATION(1)
	#define BOOST_PP_ITERATION_PARAMS_2 (4, (0,1, "vector.hpp", BOOST_PP_FRAME_FLAGS(1)))
	#include BOOST_PP_ITERATE()
	#undef DIM
#else
	#define ALIGN	BOOST_PP_ITERATION()
	#define ALIGNB	BOOLNIZE(ALIGN)
	#define Vec		BOOST_PP_IF(ALIGN, alignas(16), NOTHING) VecT<DIM, ALIGNB>
	#define VT		VecT<DIM, ALIGNB>
	#include "local_macro.hpp"
	#if BOOST_PP_ITERATION_FLAGS() == 0
		// クラスのコンストラクタとメソッドのプロトタイプだけ定義
		namespace spn {
			class MTRandom;
			template <>
			struct Vec {
				constexpr static int width = DIM;
				constexpr static bool align = ALIGNB;
				using AVec = VecT<DIM,true>;
				using UVec = VecT<DIM,false>;
				union {
					struct {
						float BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM));
					};
					float m[DIM];
				};

				friend class boost::serialization::access;
				template <class Archive>
				void serialize(Archive& ar, const unsigned int /*ver*/) {
					ar & BOOST_SERIALIZATION_NVP(m);
				}

				// -------------------- ctor --------------------
				VecT() = default;
				explicit VecT(reg128 r);
				explicit VecT(float a);
				explicit VecT(ENUM_ARGPAIR(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM)));
				template <class V, class=std::enable_if_t<std::is_floating_point<V>::value>>
				explicit VecT(const V* src);
				VecT(std::initializer_list<float> il);
				reg128 loadPS() const;
				reg128 loadPSZ() const;

				//! アラインメント済ベクトルで初期化
				VecT(const AVec& v);
				//! アラインメント無しベクトルで初期化
				VecT(const UVec& v);
				VecT& operator = (const AVec& v);
				VecT& operator = (const UVec& v);
				VecT& operator = (reg128 r);
				VecT& operator = (std::initializer_list<float> il);
				// -------------------- operators --------------------
				// ベクトルとの積算や除算は同じ要素同士の演算とする
				#define DEF_PRE(op, func)	VecT& operator BOOST_PP_CAT(op,=) (float s); \
					VecT operator op (float s) const; \
					VecT& operator BOOST_PP_CAT(op,=) (const AVec& v); \
					VecT& operator BOOST_PP_CAT(op,=) (const UVec& v); \
					AVec operator op (const AVec& v) const; \
					UVec operator op (const UVec& v) const; \
					AVec&& operator op (AVec&& v) const; \
					UVec&& operator op (UVec&& v) const; \
					friend VecT operator op (float fv, const VecT& v);
				DEF_PRE(+, reg_add_ps)
				DEF_PRE(-, reg_sub_ps)
				DEF_PRE(*, reg_mul_ps)
				DEF_PRE(/, _mmDivPs)

				// -------- Luaへのエクスポート用 --------
				VecT addV(const UVec& v) const;
				VecT subV(const UVec& v) const;
				VecT mulF(float s) const;
				VecT mulM(const MatT<DIM,DIM,align>& m) const;
				VecT divF(float s) const;
				VecT invert() const;
				bool equal(const UVec& v) const;
				std::string toString() const;

				friend std::ostream& operator << (std::ostream& os, const VT& v);
				// -------------------- others --------------------
				static float _sumup(reg128 xm);
				// ロード関数呼び出しのコストが許容出来るケースではloadPS()を呼び、そうでないケースはオーバーロードで対処
				template <bool A>
				float dot(const VecT<DIM,A>& v) const;
				//! 各要素を足しあわせた合計
				float sum() const;
				//! 各要素の算術平均
				float average() const;
				template <bool A>
				float distance(const VecT<DIM,A>& v) const;
				template <bool A>
				float dist_sq(const VecT<DIM,A>& v) const;

				//! 要素ごとに最小値選択
				template <bool A>
				VecT getMin(const VecT<DIM,A>& v) const;
				template <bool A>
				void selectMin(const VecT<DIM,A>& v);

				//! 要素ごとに最大値選択
				template <bool A>
				VecT getMax(const VecT<DIM,A>& v) const;
				template <bool A>
				void selectMax(const VecT<DIM,A>& v);

				VecT operator - () const;

				/*! \return 要素が全て等しい時にtrue, それ以外はfalse */
				template <bool A>
				bool operator == (const VecT<DIM,A>& v) const;
				template <bool A>
				bool operator != (const VecT<DIM,A>& v) const;

				float normalize();
				VecT normalization() const;
				/*! \return ベクトルの長さ */
				float length() const;
				/*! \return ベクトル長の2乗 */
				float len_sq() const;
				//! 要素の何れかがNaNになっているか
				bool isNaN() const;
				//! 要素の何れかがNaN又はinfになっているか
				bool isOutstanding() const;

				void saturate(float fMin, float fMax);
				VecT saturation(float fMin, float fMax) const;
				void lerp(const UVec& v, float r);
				template <bool A>
				VecT l_intp(const VecT<DIM,A>& v, float r) const;

				//! 要素の値がランダムなベクトル
				template <class RDF>
				static VecT Random(const RDF& rdf, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVec<VecT>(rdf, r);
				}
				template <class RDF>
				static VecT RandomF(const RDF& rdf) {
					return random::GenRVecF<VecT>(rdf);
				}
				//! ランダムなベクトル（但し長さが指定値より大きい）
				template <class RDF>
				static VecT RandomWithLength(const RDF& rdf, float th, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVecLen<VecT>(rdf, th, r);
				}
				//! ランダムなベクトル (但し全ての成分の絶対値がそれぞれ基準範囲内)
				template <class RDF>
				static VecT RandomWithAbs(const RDF& rdf, const RangeF& rTh, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVecAbs<VecT>(rdf, rTh, r);
				}
				//! ランダムな方向ベクトル
				template <class RDF>
				static VecT RandomDir(const RDF& rdf) {
					return random::GenRDir<VecT>(rdf);
				}
				// -------- Luaへのエクスポート用 --------
				static VecT luaRandom(MTRandom& mt, const RangeF& r);
				static VecT luaRandomWithLength(MTRandom& mt, float th, const RangeF& r);
				static VecT luaRandomWithAbs(MTRandom& mt, const RangeF& rTh, const RangeF& r);
				#if ALIGN==1
					//! AVec -> Vec へ暗黙変換
					operator VecT<DIM,false>& ();
					operator const VecT<DIM,false>& () const;
				#endif
				#if DIM==4
					using Vec3 = VecT<3,ALIGNB>;
					/*! \return x,y,z成分 */
					const Vec3& asVec3() const;
					/*! \return x,y,zそれぞれをwで割った値 */
					Vec3 asVec3Coord() const;
				#elif DIM==3
					//! ベクトルに垂直なベクトルを適当に定める
					VecT verticalVector() const;
					/*! \return this X v の外積ベクトル */
					template <bool A>
					VecT cross(const VecT<DIM,A>& v) const;
					//! 外積計算cross()と同義
					template <bool A>
					void operator %= (const VecT<DIM,A>& v);
					//! 外積計算cross()と同義
					template <bool A>
					VecT operator % (const VecT<DIM,A>& v) const;
					using Vec4 = VecT<4,ALIGNB>;
					Vec4 asVec4(float w) const;
					//! 平面との交差点を算出
					template <bool A>
					std::tuple<VecT,bool> planeDivide(const VecT<DIM,A>& v, const PlaneT<false>& p) const;
					//! 平面にてベクトルを反転
					void flip(const PlaneT<false>& plane);
					VecT& operator *= (const QuatT<ALIGNB>& q);
					VecT operator * (const QuatT<ALIGNB>& q) const;
					VecT&& operator * (QuatT<ALIGNB>&& q) const;
					/*! \return x,y 成分 */
					const VecT<2,ALIGNB>& asVec2() const;
					// -------- Luaへのエクスポート用 --------
					VecT modV(const UVec& v) const;
					VecT mulQ(const QuatT<ALIGNB>& q) const;
					std::tuple<VecT,bool> luaPlaneDivide(const VecT<DIM,false>& v, const PlaneT<false>& p) const;
				#elif DIM==2
					float ccw(const VecT<DIM,false>& v) const;
					float cw(const VecT<DIM,false>& v) const;
					#if ALIGN==0
						static float Ccw(const UVec& v0, const UVec& v1, const UVec& v2);
						static float Cw(const UVec& v0, const UVec& v1, const UVec& v2);
					#endif
					using Vec3 = VecT<3,ALIGNB>;
					Vec3 asVec3(float z) const;
				#endif
				//! 行列との積算 (左から掛ける)
				/*! 行ベクトルとして扱う */
				template <int N, bool A>
				VecT<N,ALIGNB> operator * (const MatT<DIM,N,A>& m) const;
				template <int N, bool A>
				VecT& operator *= (const MatT<DIM,N,A>& m);

				#if DIM>=3
					//! パックされた32bit数値から色を取り出す
					static VecT FromPacked(uint32_t val);
					//! 32bit数値に値をパック (1.0が255)
					uint32_t toPacked() const;
				#endif
			};
			#undef Vec
			// 使いやすいようにクラスの別名を定義
			using BOOST_PP_CAT(BOOST_PP_IF(ALIGN,A,NOTHING), BOOST_PP_CAT(Vec,DIM)) = VT;
		}
		BOOST_CLASS_IMPLEMENTATION(spn::VT, object_serializable)
		#elif BOOST_PP_ITERATION_FLAGS() == 1
		// 同次元ベクトルとの演算を定義
		namespace spn {
			VT::VecT(reg128 r){
				STORETHIS(r);
			}
			VT::VecT(float a) {
				STORETHIS(reg_load1_ps(&a));
			}
			VT::VecT(ENUM_ARGPAIR(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM))) {
				BOOST_PP_SEQ_FOR_EACH(
					DEF_ARGSET,
					NOTHING,
					BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM)
				)
			}
			template <class V, class>
			VT::VecT(const V* src) {
				for(int i=0 ; i<DIM ; i++)
					m[i] = src[i];
			}
			template VT::VecT(const float*);
			template VT::VecT(const double*);
			VT::VecT(std::initializer_list<float> il) {
				// 要素数が満たなかったら残りは0にする
				alignas(16) float tmp[4] = {};
				auto* pTmp = tmp;
				for(auto itr=il.begin() ; itr!=il.end() ; ++itr)
					*pTmp++ = *itr;
				STORETHIS(reg_load_ps(tmp));
			}
			reg128 VT::loadPS() const {
				return LOADTHIS();
			}
			reg128 VT::loadPSZ() const {
				return LOADTHISZ();
			}
			VT& VT::operator = (reg128 r) {
				STORETHIS(r);
				return *this;
			}
			VT& VT::operator = (std::initializer_list<float> il) {
				return *this = VecT(il);
			}
			float VT::_sumup(reg128 xm) {
				SUMVEC(xm)
				float ret;
				reg_store_ss(&ret, xm);
				return ret;
			}
			VT::VecT(const AVec& v) {
				STORETHIS(LOADPS(v.m)); }
			VT::VecT(const UVec& v) {
				STORETHIS(LOADPSU(v.m)); }
			bool VT::isNaN() const {
				reg128 r0 = LOADTHISZ(),
					r_zero = reg_setzero_ps();
				reg128 res = reg_or_ps(reg_cmple_ps(r0, r_zero),
										reg_cmpgt_ps(r0, r_zero));
				res = reg_andnot_ps(res, xmm_const::fullbit());
				SUMVEC(res)
				float f;
				reg_store_ss(&f, res);
				return f != 0;
			}
			bool VT::isOutstanding() const {
				const float f = std::numeric_limits<float>::infinity();
				reg128 r_inf = reg_load1_ps(&f),
						r_zero = reg_setzero_ps(),
						r0 = reg_and_ps(LOADTHIS(), xmm_const::absmask());
				reg128 r1 = reg_or_ps(reg_cmple_ps(r0, r_zero),
									reg_cmpgt_ps(r0, r_zero));
				r0 = reg_cmpeq_ps(r0, r_inf);

				r1 = reg_andnot_ps(r1, xmm_const::fullbit());
				r0 = reg_or_ps(r0, r1);
				SUMVEC(r0)

				float fv;
				reg_store_ss(&fv, r0);
				return fv != 0;
			}

			VT& VT::operator = (const AVec& v) {
				STORETHIS(LOADPS(v.m));
				return *this;
			}
			VT& VT::operator = (const UVec& v) {
				STORETHIS(LOADPSU(v.m));
				return *this;
			}
			#define DEF_OP(op, func) \
					VT& VT::operator BOOST_PP_CAT(op,=) (float s) { \
						STORETHIS(func(LOADTHIS(), reg_load_ps1(&s))); \
						return *this; } \
					VT VT::operator op (float s) const { \
						return VecT(func(LOADTHIS(), reg_load1_ps(&s))); } \
					VT& VT::operator BOOST_PP_CAT(op,=) (const AVec& v) { \
						STORETHIS(func(LOADTHIS(), LOADPS(v.m))); \
						return *this; } \
					VT& VT::operator BOOST_PP_CAT(op,=) (const UVec& v) { \
						STORETHIS(func(LOADTHIS(), LOADPSU(v.m))); \
						return *this; } \
					VT::AVec VT::operator op (const AVec& v) const { \
						return AVec(func(LOADTHIS(), LOADPS(v.m))); } \
					VT::UVec VT::operator op (const UVec& v) const { \
						return UVec(func(LOADTHIS(), LOADPSU(v.m))); } \
					VT::AVec&& VT::operator op (AVec&& v) const { \
						STOREPS(v.m, func(LOADTHIS(), LOADPS(v.m))); \
						return std::forward<AVec>(v); } \
					VT::UVec&& VT::operator op (UVec&& v) const { \
						STOREPSU(v.m, func(LOADTHIS(), LOADPSU(v.m))); \
						return std::forward<UVec>(v); } \
					VT operator op (float fv, const VT& v) { \
						return VT(func(LOADTHISPS(v.m), reg_load1_ps(&fv))); }
			DEF_OP(+, reg_add_ps)
			DEF_OP(-, reg_sub_ps)
			DEF_OP(*, reg_mul_ps)
			DEF_OP(/, _mmDivPs)
			#undef DEF_OP
			VT VT::addV(const UVec& v) const {
				return *this + v; }
			VT VT::subV(const UVec& v) const {
				return *this - v; }
			VT VT::mulF(float s) const {
				return *this * s; }
			VT VT::mulM(const MatT<DIM,DIM,align>& m) const {
				return *this * m; }
			VT VT::divF(float s) const {
				return *this / s; }
			VT VT::invert() const {
				return -(*this); }
			bool VT::equal(const UVec& v) const {
				return *this == v; }
			std::string VT::toString() const {
				return ToString(*this);
			}

			std::ostream& operator << (std::ostream& os, const VT& v) {
				os << BOOST_PP_IF(ALIGN, 'A', ' ') << "Vec" << DIM << '[';
				for(int i=0 ; i<DIM-1 ; i++)
					os << v.m[i] << ',';
				return os << v.m[DIM-1] << ']';
			}

			template <bool A>
			float VT::dot(const VecT<DIM,A>& v) const {
				return _sumup(reg_mul_ps(LOADTHISZ(), v.loadPSZ()));
			}
			template float VT::dot(const VecT<DIM,false>& v) const;
			template float VT::dot(const VecT<DIM,true>& v) const;

			float VT::sum() const {
				return _sumup(LOADTHISZ());
			}
			float VT::average() const {
				return sum() * spn::Rcp22Bit(DIM);
			}
			template <bool A>
			float VT::distance(const VecT<DIM,A>& v) const {
				return std::sqrt(dist_sq(v));
			}
			template float VT::distance(const VecT<DIM,false>&) const;
			template float VT::distance(const VecT<DIM,true>&) const;

			template <bool A>
			float VT::dist_sq(const VecT<DIM,A>& v) const {
				auto tv = v - *this;
				return tv.len_sq();
			}
			template float VT::dist_sq(const VecT<DIM,false>&) const;
			template float VT::dist_sq(const VecT<DIM,true>&) const;

			template <bool A>
			VT VT::getMin(const VecT<DIM,A>& v) const {
				return VT(reg_min_ps(LOADTHIS(), v.loadPS())); }
			template VT VT::getMin(const VecT<DIM,false>&) const;
			template VT VT::getMin(const VecT<DIM,true>&) const;

			template <bool A>
			void VT::selectMin(const VecT<DIM,A>& v) {
				STORETHIS(reg_min_ps(LOADTHIS(), v.loadPS())); }
			template void VT::selectMin(const VecT<DIM,false>&);
			template void VT::selectMin(const VecT<DIM,true>&);

			template <bool A>
			VT VT::getMax(const VecT<DIM,A>& v) const {
				return VT(reg_max_ps(LOADTHIS(), v.loadPS())); }
			template VT VT::getMax(const VecT<DIM,false>&) const;
			template VT VT::getMax(const VecT<DIM,true>&) const;

			template <bool A>
			void VT::selectMax(const VecT<DIM,A>& v) {
				STORETHIS(reg_max_ps(LOADTHIS(), v.loadPS())); }
			template void VT::selectMax(const VecT<DIM,false>&);
			template void VT::selectMax(const VecT<DIM,true>&);

			VT VT::operator - () const {
				return *this * -1.0f;
			}

			template <bool A>
			bool VT::operator == (const VecT<DIM,A>& v) const {
				reg128 r0 = reg_cmpeq_ps(LOADTHISZ(), v.loadPSZ());
				r0 = reg_and_ps(r0, reg_shuffle_ps(r0, r0, _REG_SHUFFLE(1,0,3,2)));
				r0 = reg_and_ps(r0, reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,1,2,3)));
				return reg_cvttss_si32(r0) != 0;
			}
			template <bool A>
			bool VT::operator != (const VecT<DIM,A>& v) const {
				return !(this->operator == (v));
			}
			template bool VT::operator == (const VecT<DIM,false>&) const;
			template bool VT::operator == (const VecT<DIM,true>&) const;
			template bool VT::operator != (const VecT<DIM,false>&) const;
			template bool VT::operator != (const VecT<DIM,true>&) const;

			float VT::normalize() {
				float len = length();
				*this *= spn::Rcp22Bit(len);
				return len;
			}
			VT VT::normalization() const {
				float tmp = length();
				reg128 r0 = reg_load_ps1(&tmp);
				r0 = reg_div_ps(LOADTHIS(), r0);
				return VT(r0);
			}
			/*! \return ベクトルの長さ */
			float VT::length() const {
				return std::sqrt(len_sq());
			}
			/*! \return ベクトル長の2乗 */
			float VT::len_sq() const {
				reg128 r0 = LOADTHISZ();
				r0 = reg_mul_ps(r0, r0);
				SUMVEC(r0)

				float ret;
				reg_store_ss(&ret, r0);
				return ret;
			}
			void VT::saturate(float fMin, float fMax) {
				*this = saturation(fMin, fMax);
			}
			VT VT::saturation(float fMin, float fMax) const {
				reg128 xm = reg_max_ps(LOADTHIS(), reg_load1_ps(&fMin));
				return VT(reg_min_ps(xm, reg_load1_ps(&fMax)));
			}
			void VT::lerp(const UVec& v, float r) {
				*this = l_intp(v,r);
			}
			template <bool A>
			VT VT::l_intp(const VecT<DIM,A>& v, float r) const {
				reg128 ths = LOADTHIS();
				ths = reg_add_ps(ths, reg_mul_ps(reg_sub_ps(v.loadPS(), ths), reg_load1_ps(&r)));
				return VT(ths);
			}
			template VT VT::l_intp(const VecT<DIM,false>&, float) const;
			template VT VT::l_intp(const VecT<DIM,true>&, float) const;

			#if DIM>=3
				VT VT::FromPacked(uint32_t val) {
					alignas(16) const uint32_t tmp[4] = {val, val>>8, val>>16, val>>24};
					alignas(16) const uint32_t mask[4] = {0xff,0xff,0xff,0xff};

					reg128 x = reg_and_ps(reg_load_ps((const float*)mask), reg_load_ps((const float*)tmp));
					x = reg_cvtepi32_ps((reg128i)x);

					const float f255 = 1.f/255;
					x = reg_mul_ps(x, reg_load1_ps(&f255));
					return VecT(x);
				}
				uint32_t VT::toPacked() const {
					reg128 x = reg_mul_ps(reg_set_ps(255,255,255,255), LOADTHISZ());
					reg128i x2 = reg_cvtps_epi32(x);
					x2 = reg_packs_epi32(x2,x2);
					x2 = reg_packus_epi16(x2,x2);
					float ret;
					reg_store_ss(&ret, (reg128)x2);
					return *reinterpret_cast<uint32_t*>(&ret);
				}
			#endif
		}
	#elif BOOST_PP_ITERATION_FLAGS() == 2
		// 他次元ベクトルとの演算を定義
		namespace spn {
			#if ALIGN==1
				//! AVec -> Vec へ暗黙変換
				VT::operator VecT<DIM,false>& () { return *reinterpret_cast<VecT<DIM,false>*>(this); }
				VT::operator const VecT<DIM,false>& () const { return *reinterpret_cast<const VecT<DIM,false>*>(this); }
			#endif
			#if DIM==4
				#define _Vec3 VecT<3,ALIGNB>
				/*! \return x,y,z成分 */
				const _Vec3& VT::asVec3() const {
					return reinterpret_cast<const _Vec3&>(*this);
				}
				/*! \return x,y,zそれぞれをwで割った値 */
				_Vec3 VT::asVec3Coord() const {
					reg128 r0 = reg_rcp_ps(reg_load_ps1(&w));
					return _Vec3(reg_mul_ps(LOADTHIS(), r0));
				}
			#elif DIM==3
				const VecT<2,ALIGNB>& VT::asVec2() const {
					return reinterpret_cast<const VecT<2,ALIGNB>&>(*this);
				}
				//! ベクトルに垂直なベクトルを適当に定める
				VT VT::verticalVector() const {
					VT ret = VT(1,0,0) % *this;
					float len_s = ret.len_sq();
					if(len_s < 1e-6f) {
						ret = VT(0,0,1) % *this;
						return ret.normalization();
					}
					return ret * RSqrt(len_s);
				}
				/*! \return this X v の外積ベクトル */
				template <bool A>
				VT VT::cross(const VecT<DIM,A>& v) const {
					reg128 r0 = LOADTHIS(),
							r1 = v.loadPS();
					// r0[y,z,x]
					reg128 m0 = reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,0,2,1)),
					// r0[z,x,y]
							m1 = reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,1,0,2)),
					// r1[z,x,y]
							m2 = reg_shuffle_ps(r1, r1, _REG_SHUFFLE(0,1,0,2)),
					// r1[y,z,x]
							m3 = reg_shuffle_ps(r1, r1, _REG_SHUFFLE(0,0,2,1));
					r0 = reg_mul_ps(m0,m2);
					r1 = reg_mul_ps(m1,m3);
					r0 = reg_sub_ps(r0, r1);
					return VT(r0);
				}
				template VT VT::cross(const VecT<DIM,false>&) const;
				template VT VT::cross(const VecT<DIM,true>&) const;

				//! 外積計算cross()と同義
				template <bool A>
				void VT::operator %= (const VecT<DIM,A>& v) {
					*this = cross(v);
				}
				template void VT::operator %= (const VecT<DIM,false>&);
				template void VT::operator %= (const VecT<DIM,true>&);

				//! 外積計算cross()と同義
				template <bool A>
				VT VT::operator % (const VecT<DIM,A>& v) const {
					return cross(v);
				}
				template VT VT::operator % (const VecT<DIM,false>&) const;
				template VT VT::operator % (const VecT<DIM,true>&) const;

				VT VT::modV(const UVec& v) const {
					return *this % v; }
				VT VT::mulQ(const QuatT<ALIGNB>& q) const {
					return *this * q; }
				std::tuple<VT,bool> VT::luaPlaneDivide(const VecT<DIM,false>& v, const PlaneT<false>& p) const {
					return planeDivide(v, p);
				}
				#define _Vec4 VecT<4,ALIGNB>
				_Vec4 VT::asVec4(float w) const {
					return _Vec4(x,y,z,w);
				}
			#elif DIM==2
				//! counter clockwise
				/*! 半時計回りが正数を返す */
				float VT::ccw(const UVec& v) const {
					return x*v.y - y*v.x;
				}
				//! clockwise
				/*! 時計回りが正数を返す */
				float VT::cw(const UVec& v) const {
					return -x*v.y + y*v.x;
				}
				#define _Vec3 VecT<3,ALIGNB>
				_Vec3 VT::asVec3(float z) const {
					return _Vec3(x,y,z);
				}
				#if ALIGN==0
					float VT::Ccw(const UVec& v0, const UVec& v1, const UVec& v2) {
						return (v0-v1).ccw(v2-v1);
					}
					float VT::Cw(const UVec& v0, const UVec& v1, const UVec& v2) {
						return (v0-v1).cw(v2-v1);
					}
				#endif
			#endif
		}
	#else
		#include "random.hpp"
		namespace spn {
			/*	Pseudo-code:
				template <int N, bool A>
				VecT<N,ALIGNB> VT::operator * (const MatT<DIM,N,A>& m) const {
					reg128 ths = LOADTHIS(),
							accum = reg_setzero_ps();
					for(int i=0 ; i<DIM ; i++) {
						reg128 tmp = reg_shuffle_ps(ths, ths, _REG_SHUFFLE(i,i,i,i));
						accum = reg_add_ps(accum, reg_mul_ps(tmp, LOADPS_(N)(m.ma[i])));
					}
					return VecT<N,ALIGNB>(accum);
				}
			*/
			#define LOOP_MULOP(z,n,loadf)		{reg128 tmp=reg_shuffle_ps(ths,ths, _REG_SHUFFLE(n,n,n,n)); \
						accum = reg_add_ps(accum, reg_mul_ps(tmp, loadf(mat.ma[n])));}
			#define DEF_MULOPA(z,n,align)	template <> VecT<n,ALIGNB> VT::operator * (const MatT<DIM,n,BOOLNIZE(align)>& mat) const { \
						reg128 ths = LOADTHIS(), \
						accum = reg_setzero_ps(); \
						BOOST_PP_REPEAT_##z(DIM, LOOP_MULOP, BOOST_PP_CAT(LOADPS_, BOOST_PP_CAT(AFLAG(align),n))) \
						return VecT<n,ALIGNB>(accum); }
			#define DEF_MULOPA_2(z,n,align)	template <> VT& VT::operator *= (const MatT<DIM,DIM,BOOLNIZE(align)>& mat) { \
					return *this = *this * mat; }

			#define DEF_MULOP(z,align,dummy)	BOOST_PP_REPEAT_FROM_TO_##z(2,5, DEF_MULOPA, align) DEF_MULOPA_2(dummy,dummy,align)
			BOOST_PP_REPEAT(2, DEF_MULOP, NOTHING)
			#undef DEF_MULOP

			#if DIM==3
				VT& VT::operator *= (const QuatT<ALIGNB>& q) {
					*this = (*this * q);
					return *this;
				}
				VT VT::operator * (const QuatT<ALIGNB>& q) const {
					auto tq = q * QuatT<ALIGNB>(*this,0) * q.conjugation();
					return tq.getVector();
				}
				VT&& VT::operator * (QuatT<ALIGNB>&& q) const {
					VT& rVT = *reinterpret_cast<VT*>(&q);
					rVT = *this * q;
					return std::move(rVT);
				}
				void VT::flip(const Plane& plane) {
					const auto& nml = plane.getNormal();
					float d = plane.dot(*this);
					*this += nml*-d*2;
				}
				template <bool A>
				std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,A>& v, const PlaneT<false>& p) const {
					// 線分が平面をまたぐか
					float distf = p.dot(*this);
					float distb = p.dot(v);
					if(distf * distb >= 0.0f)
						return std::make_tuple(VecT(),false);

					float ratio = fabs(distf) / (fabs(distf) + fabs(distb));
					// 平面と線分の交点 -> tv
					Vec3 tv = v - (*this);
					tv *= ratio;
					VecT cp = tv + (*this);
					return std::make_tuple(cp,true);
				}
				template std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,true>&,const PlaneT<false>&) const;
				template std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,false>&,const PlaneT<false>&) const;
			#endif

			VT VT::luaRandom(MTRandom& mt, const RangeF& r) {
				return Random(mt.getUniformF<float>(), r);
			}
			VT VT::luaRandomWithLength(MTRandom& mt, float th, const RangeF& r) {
				return RandomWithLength(mt.getUniformF<float>(), th, r);
			}
			VT VT::luaRandomWithAbs(MTRandom& mt, const RangeF& rTh, const RangeF& r) {
				return RandomWithAbs(mt.getUniformF<float>(), rTh, r);
			}
		}
	#endif
	#include "local_unmacro.hpp"
	#undef ALIGN
	#undef ALIGNB
	#undef Vec
	#undef VT
#endif


/*
 * Copyright (c) 2020, Robobrain.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Konstantinos Konstantinidis */

#pragma once

#include "kalman-cpp/kalman.hpp"
#include <Eigen/Dense>
#include <ros/console.h>

using namespace Eigen;

typedef std::pair<double, double> Point;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

const double pi = 3.141592653589793238463;

class LshapeTracker {
public:

  LshapeTracker(const double& x_corner, const double& y_corner, const double& L1, const double& L2, const double& theta, const double& dt);
  LshapeTracker();
  void update(const double& thetaL1, const double& x_corner, const double& y_corner, const double& L1, const double& L2, const double& dt, const int cluster_size);
  void BoxModel(double& x, double& y,double& vx, double& vy,double& theta, double& psi, double& omega, double& L1, double& L2, double& length, double& width);

private:
  int current_size;
  double test1, test2, test3;
  double x_old, y_old, L1_old, L2_old, old_thetaL1;

  KalmanFilter shape_kf;
  KalmanFilter dynamic_kf;

  void ClockwisePointSwitch();
  void CounterClockwisePointSwitch();
  double findTurn(const double& new_angle, const double& old_angle);
  void detectCornerPointSwitch(const double& from, const double& to, const double dt);
  void detectCornerPointSwitchMahalanobis(const double& from, const double& to, const double dt);
  void detectCornerPointSwitchMahalanobis(const double& from, const double& to, const double L1, const double L2, const double x_corner, const double y_corner);
  /*! \brief Finds orientations of tracked object
   *
   * Given the orientation of L1 the other three angles of the rectangle are calculated.
   * Then they are compared with the speed of the object, to estimate it's direction.
   *
   * \angle angle of one edge of the box
   * \vx velocity in the x axis
   * \vy velocity in the y axis
   * \orientation orientation of tracked object, based on it's speed
   */
  void findOrientation(double& psi, double& length, double& width);
};


#include <NTL/GF2E.h>

NTL_START_IMPL

NTL_TLS_GLOBAL_DECL(SmartPtr<GF2EInfoT>, GF2EInfo_stg)

NTL_CHEAP_THREAD_LOCAL
GF2EInfoT *GF2EInfo = 0;

GF2EInfoT::GF2EInfoT(const GF2X& NewP)
{
   build(p, NewP);
   _card_exp = p.n;

   long sz = p.size;

// The following crossovers were set using the programs
// GF2EXKarCross.cpp, GF2EXModCross.cpp, GF2EXModCross.cpp,
// and GF2EXGCDCross.cpp.
// To use these programs, one has to remove the #if 0 guards
// in GF2EX.cpp on mul_disable_plain, BuildPlain, and DivRemPlain.

// There are three different configurations that are treated separately:
//   * with gf2x lib and with pclmul instruction available
//   * without gf2x lib but with pclmul
//   * without gf2X lib and without pclmul
// It is possible that one could be using gf2x lib on a platform without
// pclmul, in which case the crossovers used here are not optimal.  It is also
// possible that one could be using gf2x lib with pclmul, but compile NTL with
// NATIVE=off, so that NTL assumes there is no pclmul.  Again, this will lead
// to crossovers that are not optimal.

// The crossovers were calculated based on a Skylake Xeon processor:
// Intel(R) Xeon(R) Gold 6132 CPU @ 2.60GHz.

#if (defined(NTL_GF2X_LIB) && defined(NTL_HAVE_PCLMUL))

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 3;
      else
         KarCross = 4;
   }
   else if (sz <= 6) KarCross = 8;
   else if (sz <= 9) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 15;
      else
         ModCross = 20;
   }
   else if (sz <=  9) ModCross =  60;
   else if (sz <= 18) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross =  50;
      else
         DivCross =  75;
   }
   else if (sz <=  2) DivCross = 100;
   else if (sz <=  3) DivCross = 150;
   else if (sz <=  4) DivCross = 200;
   else if (sz <=  6) DivCross = 250;
   else if (sz <=  9) DivCross = 225;
   else if (sz <= 15) DivCross = 125;
   else if (sz < 125) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 225;
   }
   else if (sz <=  2) GCDCross =  450;
   else if (sz <=  4) GCDCross =  600;
   else if (sz <  12) GCDCross = 1150;
   else               GCDCross =  600;

#elif (defined(NTL_HAVE_PCLMUL))

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 5;
      else
         KarCross = 8;
   }
   else if (sz <= 5) KarCross = 8;
   else if (sz <= 9) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 30;
      else
         ModCross = 45;
   }
   else if (sz <=  2) ModCross = 110;
   else if (sz <=  3) ModCross = 105;
   else if (sz <=  4) ModCross =  65;
   else if (sz <=  5) ModCross =  60;
   else if (sz <=  6) ModCross =  55;
   else if (sz <=  8) ModCross =  50;
   else if (sz <= 12) ModCross =  30;
   else if (sz <= 18) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross =  75;
      else
         DivCross = 125;
   }
   else if (sz <=  2) DivCross = 450;
   else if (sz <=  3) DivCross = 425;
   else if (sz <=  4) DivCross = 375;
   else if (sz <=  6) DivCross = 250;
   else if (sz <=  8) DivCross = 225;
   else if (sz <= 16) DivCross = 125;
   else if (sz <= 45) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 225;
   }
   else if (sz < 12) GCDCross = 1150;
   else              GCDCross =  850;

#else

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 4;
      else
         KarCross = 12;
   }
   else if (sz <= 3) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 45;
      else
         ModCross = 65;
   }
   else if (sz <=  2) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross = 175;
      else
         DivCross = 250;
   }
   else if (sz <=  4) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 850;
   }
   else if (sz <  8) GCDCross =  850;
   else if (sz < 12) GCDCross =  600;
   else              GCDCross =  450;

#endif

}

const ZZ& GF2E::cardinality()
{
   if (!GF2EInfo) LogicError("GF2E::cardinality: undefined modulus");

   do { // NOTE: thread safe lazy init
      Lazy<ZZ>::Builder builder(GF2EInfo->_card);
      if (!builder()) break;
      UniquePtr<ZZ> p;
      p.make();
      power(*p, 2, GF2EInfo->_card_exp);
      builder.move(p);
   } while (0);

   return *GF2EInfo->_card;
}

void GF2E::init(const GF2X& p)
{
   GF2EContext c(p);
   c.restore();
}

void GF2EContext::save()
{
   NTL_TLS_GLOBAL_ACCESS(GF2EInfo_stg);
   ptr = GF2EInfo_stg;
}

void GF2EContext::restore() const
{
   NTL_TLS_GLOBAL_ACCESS(GF2EInfo_stg);
   GF2EInfo_stg = ptr;
   GF2EInfo = GF2EInfo_stg.get();
}

GF2EBak::~GF2EBak()
{
   if (MustRestore) c.restore();
}

void GF2EBak::save()
{
   c.save();
   MustRestore = true;
}

void GF2EBak::restore()
{
   c.restore();
   MustRestore = false;
}

const GF2E& GF2E::zero()
{
   static const GF2E z(INIT_NO_ALLOC); // GLOBAL (assumes C++11 thread-safe init)
   return z;
}

istream& operator>>(istream& s, GF2E& x)
{
   GF2X y;

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(GF2E& x, const GF2E& a, const GF2E& b)
{
   GF2E t;

   inv(t, b);
   mul(x, a, t);
}

void div(GF2E& x, GF2 a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void div(GF2E& x, long a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void inv(GF2E& x, const GF2E& a)
{
   InvMod(x._GF2E__rep, a._GF2E__rep, GF2E::modulus());
}

NTL_END_IMPL


/*
 * Copyright 2013-2014, Unbounded Robotics Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

// Author: Michael Ferguson

#include <math.h>
#include <Eigen/Eigen>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <simple_grasping/shape_extraction.h>

namespace simple_grasping
{

bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                  const pcl::ModelCoefficients::Ptr model,
                  pcl::PointCloud<pcl::PointXYZRGB>& output,
                  shape_msgs::SolidPrimitive& shape,
                  geometry_msgs::Pose& pose)
{
  // Used to decide between various shapes
  double min_volume = 1000.0;  // the minimum volume shape found thus far.
  Eigen::Matrix3f transformation;  // the transformation for the best-fit shape

  // Compute z height as maximum distance from planes
  double height = 0.0;
  for (size_t i = 0; i < input.size(); ++i)
  {
    Eigen::Vector4f pp(input[i].x, input[i].y, input[i].z, 1);
    Eigen::Vector4f m(model->values[0], model->values[1], model->values[2], model->values[3]);
    double distance_to_plane = fabs(pp.dot(m));
    if (distance_to_plane > height)
      height = distance_to_plane;
  }

  // Project object into 2d, using plane model coefficients
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::ProjectInliers<pcl::PointXYZRGB> projection;
  projection.setModelType(pcl::SACMODEL_PLANE);
  projection.setInputCloud(input.makeShared());  // stupid API
  projection.setModelCoefficients(model);
  projection.filter(*flat);

  // Rotate plane so that Z=0
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
  Eigen::Vector3f normal(model->values[0], model->values[1], model->values[2]);
  Eigen::Quaternionf qz; qz.setFromTwoVectors(normal, Eigen::Vector3f::UnitZ());
  Eigen::Matrix3f plane_rotation = qz.toRotationMatrix();
  Eigen::Matrix3f inv_plane_rotation = plane_rotation.inverse();

  for (size_t i = 0; i < flat->size(); ++i)
  {
    pcl::PointXYZRGB p;
    p.getVector3fMap() = plane_rotation * (*flat)[i].getVector3fMap();
    flat_projected->push_back(p);
  }

  // Find the convex hull
  pcl::PointCloud<pcl::PointXYZRGB> hull;
  pcl::ConvexHull<pcl::PointXYZRGB> convex_hull;
  convex_hull.setInputCloud(flat_projected);
  convex_hull.setDimension(2);
  convex_hull.reconstruct(hull);

  // Try fitting a rectangle
  shape_msgs::SolidPrimitive rect;  // the best-fit rectangle
  rect.type = rect.BOX;
  rect.dimensions.resize(3);
  for (size_t i = 0; i < hull.size() - 1; ++i)
  {
    // For each pair of hull points, determine the angle
    double rise = hull[i+1].y - hull[i].y;
    double run = hull[i+1].x - hull[i].x;
    // and normalize..
    {
      double l = sqrt((rise * rise) + (run * run));
      rise = rise/l;
      run = run/l;
    }

    // Build rotation matrix from change of basis
    Eigen::Matrix3f rotation;
    rotation(0, 0) = run;
    rotation(0, 1) = rise;
    rotation(0, 2) = 0.0;
    rotation(1, 0) = -rise;
    rotation(1, 1) = run;
    rotation(1, 2) = 0.0;
    rotation(2, 0) = 0.0;
    rotation(2, 1) = 0.0;
    rotation(2, 2) = 1.0;
    Eigen::Matrix3f inv_rotation = rotation.inverse();

    // Project hull to new coordinate system
    pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
    for (size_t j = 0; j < hull.size(); ++j)
    {
      pcl::PointXYZRGB p;
      p.getVector3fMap() = rotation * hull[j].getVector3fMap();
      projected_cloud.push_back(p);
    }

    // Compute min/max
    double x_min = 1000.0;
    double x_max = -1000.0;
    double y_min = 1000.0;
    double y_max = -1000.0;
    for (size_t j = 0; j < projected_cloud.size(); ++j)
    {
      if (projected_cloud[j].x < x_min)
        x_min = projected_cloud[j].x;
      if (projected_cloud[j].x > x_max)
        x_max = projected_cloud[j].x;

      if (projected_cloud[j].y < y_min)
        y_min = projected_cloud[j].y;
      if (projected_cloud[j].y > y_max)
        y_max = projected_cloud[j].y;
    }

    // Is this the best estimate?
    double area = (x_max - x_min) * (y_max - y_min);
    if (area*height < min_volume)
    {
      transformation = inv_plane_rotation * inv_rotation;

      rect.dimensions[0] = (x_max - x_min);
      rect.dimensions[1] = (y_max - y_min);
      rect.dimensions[2] = height;

      Eigen::Vector3f pose3f((x_max + x_min)/2.0, (y_max + y_min)/2.0,
                             projected_cloud[0].z + height/2.0);
      pose3f = transformation * pose3f;
      pose.position.x = pose3f(0);
      pose.position.y = pose3f(1);
      pose.position.z = pose3f(2);

      Eigen::Quaternionf q(transformation);
      pose.orientation.x = q.x();
      pose.orientation.y = q.y();
      pose.orientation.z = q.z();
      pose.orientation.w = q.w();

      min_volume = area * height;
      shape = rect;
    }
  }

  // Try fitting a cylinder
  shape_msgs::SolidPrimitive cylinder;  // the best-fit cylinder
  cylinder.type = cylinder.CYLINDER;
  cylinder.dimensions.resize(2);
  for (size_t i = 0; i < hull.size(); ++i)
  {
    for (size_t j = i + 1; j < hull.size(); ++j)
    {
      // For each pair of hull points determine the center point
      //  between them as a possible cylinder
      pcl::PointXYZRGB p;
      p.x = (hull[i].x + hull[j].x) / 2.0;
      p.y = (hull[i].y + hull[j].y) / 2.0;
      double radius = 0.0;
      // Find radius from this point
      for (size_t k = 0; k < hull.size(); ++k)
      {
        double dx = hull[k].x - p.x;
        double dy = hull[k].y - p.y;
        double r = sqrt((dx * dx) + (dy * dy));
        if (r > radius)
          radius = r;
      }
      // Is this cylinder the best match?
      double volume = M_PI * radius * radius * height;
      if (volume < min_volume)
      {
        transformation = inv_plane_rotation;

        cylinder.dimensions[0] = height;
        cylinder.dimensions[1] = radius;

        Eigen::Vector3f pose3f(p.x, p.y, hull[0].z + height/2.0);
        pose3f = transformation * pose3f;
        pose.position.x = pose3f(0);
        pose.position.y = pose3f(1);
        pose.position.z = pose3f(2);

        min_volume = volume;
        shape = cylinder;
      }
    }
  }

  // TODO: Try fitting a sphere?

  // Project input to new frame
  Eigen::Vector3f origin(pose.position.x, pose.position.y, pose.position.z);
  for (size_t j = 0; j < input.size(); ++j)
  {
    pcl::PointXYZRGB p;
    p.getVector3fMap() = transformation * (input[j].getVector3fMap() - origin);
    output.push_back(p);
  }
  return true;
}

bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                  pcl::PointCloud<pcl::PointXYZRGB>& output,
                  shape_msgs::SolidPrimitive& shape,
                  geometry_msgs::Pose& pose)
{
  // Find lowest point, use as z height
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  coefficients->values.resize(4);
  coefficients->values[0] = 0.0;
  coefficients->values[1] = 0.0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 1000.0;  // z-height
  for (size_t i = 0; i < input.size(); ++i)
  {
    if (input[i].z < coefficients->values[3])
      coefficients->values[3] = input[i].z;
  }
  coefficients->values[3] = -coefficients->values[3];
  return extractShape(input, coefficients, output, shape, pose);
}

bool extractUnorientedBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                                  shape_msgs::SolidPrimitive& shape,
                                  geometry_msgs::Pose& pose)
{
  double x_min = 1000.0;
  double x_max = -1000.0;
  double y_min = 1000.0;
  double y_max = -1000.0;
  double z_min = 1000.0;
  double z_max = -1000.0;

  for (size_t i = 0; i < input.size(); ++i)
  {
    if (input[i].x < x_min)
      x_min = input[i].x;
    if (input[i].x > x_max)
      x_max = input[i].x;

    if (input[i].y < y_min)
      y_min = input[i].y;
    if (input[i].y > y_max)
      y_max = input[i].y;

    if (input[i].z < z_min)
      z_min = input[i].z;
    if (input[i].z > z_max)
      z_max = input[i].z;
  }

  pose.position.x = (x_min + x_max)/2.0;
  pose.position.y = (y_min + y_max)/2.0;
  pose.position.z = (z_min + z_max)/2.0;

  shape.type = shape.BOX;
  shape.dimensions.push_back(x_max-x_min);
  shape.dimensions.push_back(y_max-y_min);
  shape.dimensions.push_back(z_max-z_min);

  return true;
}

}  // namespace simple_grasping