/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120210    T. Secretin       File created.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *
 *    Notes
 *      This code is an implementation of the method developed by Dario Izzo from ESA/ACT and
 *      publicly available at: http://keptoolbox.sourceforge.net/.
 *      After verification and validation, it was proven that this algorithm is faster and more
 *      robust than the implemented Lancaster & Blanchard and Gooding method. Notably, this method
 *      does not suffer from the near-pi singularity (pi-transfers are by nature singular).
 *
 */

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterIzzo.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"

#include <Eigen/Geometry>

namespace tudat
{
namespace mission_segments
{

//! Execute Lambert targeting solver.
void LambertTargeterIzzo::execute( )
{
    // Call Izzo's Lambert targeting routine.
    solveLambertProblemIzzo( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                             timeOfFlight, gravitationalParameter, cartesianVelocityAtDeparture,
                             cartesianVelocityAtArrival, isRetrograde_, convergenceTolerance_,
                             maximumNumberOfIterations_ );
}

//! Get radial velocity at departure.
double LambertTargeterIzzo::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterIzzo::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterIzzo::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterIzzo::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterIzzo::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/heat_flow.h>
#include <cinolib/laplacian.h>
#include <cinolib/vertex_mass.h>
#include <cinolib/linear_solvers.h>
#include <Eigen/Sparse>

namespace cinolib
{

template<class M, class V, class E, class P>
CINO_INLINE
ScalarField heat_flow(const AbstractMesh<M,V,E,P> & m,
                      const std::vector<uint>     & heat_charges,
                      const double                  time,
                      const int                     laplacian_mode,
                      const bool                    hard_contraint_bcs)
{
    assert(heat_charges.size() > 0);

    ScalarField heat(m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode);
    Eigen::SparseMatrix<double> MM  = mass_matrix(m);
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(m.num_verts());

    if (hard_contraint_bcs) // heat flow as a boundary problem (charges do not lose heat)
    {
        std::map<uint,double> bcs;
        for(uint vid: heat_charges) bcs[vid] = 1.0;
        solve_square_system_with_bc(MM - time * L, rhs, heat, bcs);
    }
    else // heat flow as a diffusion problem (charges lose heat)
    {
        for(uint vid : heat_charges) rhs[vid] = 1.0;
        solve_square_system(MM - time * L, rhs, heat);
    }

    return heat;
}

}


/***
clang++-10 -std=c++11 -o pinnochio_timing.exe time_CPU_pinnochio.cpp -O3 -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_WITH_URDFDOM -lboost_system -L/opt/openrobots/lib -lpinocchio -lurdfdom_model -lpthread
***/
#include "utils/experiment_helpers.h" // include constants and other experiment consistency helpers

#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fixes a warning I get

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/time.h>

#define EIGEN_DEFAULT_TO_ROW_MAJOR
#include <Eigen/Dense>
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)

using namespace Eigen;
using namespace pinocchio;

void dynamicsGradientThreaded_inner(Model *model, Data *datas, MatrixXd *dqdd_dqs, MatrixXd *dqdd_dqds, \
                                    VectorXd *qs, VectorXd *qds, VectorXd *qdds, MatrixXd *Minvs, int tid, int kStart, int kMax){
   MatrixXd drnea_dq = MatrixXd::Zero(model->nq,model->nq);
   MatrixXd drnea_dv = MatrixXd::Zero(model->nv,model->nv);
   MatrixXd drnea_da = MatrixXd::Zero(model->nv,model->nv);
   for(int k = kStart; k < kMax; k++){
      computeRNEADerivatives(*model,datas[tid],qs[k],qds[k],qdds[k],drnea_dq,drnea_dv,drnea_da);
      dqdd_dqs[k] = -Minvs[k]*drnea_dq;
      dqdd_dqds[k] = -Minvs[k]*drnea_dv;
   }
}

template<int NUM_THREADS, int NUM_TIME_STEPS>
void dynamicsGradientReusableThreaded(Model *model, Data *datas, MatrixXd *dqdd_dqs, MatrixXd *dqdd_dqds, \
                                    VectorXd *qs, VectorXd *qds, VectorXd *qdds, MatrixXd *Minvs, ReusableThreads<NUM_THREADS> *threads){
      for (int tid = 0; tid < NUM_THREADS; tid++){
         int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
         if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
         threads->addTask(tid, dynamicsGradientThreaded_inner, std::ref(model), std::ref(datas), std::ref(dqdd_dqs), std::ref(dqdd_dqds),
                                                   std::ref(qs), std::ref(qds), std::ref(qdds), std::ref(Minvs), tid, kStart, kMax);
      }
      threads->sync();
}

template<int TEST_ITERS, int NUM_THREADS_TEST, int NUM_TIME_STEPS_TEST>
void test(){
   // Setup timer
   struct timespec start, end;

   // Import URDF model and prepare pinnochio
   Model model;
   std::string urdf_filename = "utils/iiwa_14.urdf";
   pinocchio::urdf::buildModel(urdf_filename,model);
   // model.gravity.setZero();
   model.gravity.linear(Eigen::Vector3d(0,0,-9.81));
   Data datas[NUM_THREADS_TEST];
   for(int i = 0; i < NUM_THREADS_TEST; i++){datas[i] = Data(model);}

   // allocate and load on CPU
   VectorXd qs[NUM_TIME_STEPS_TEST];
   VectorXd qds[NUM_TIME_STEPS_TEST];
   VectorXd qdds[NUM_TIME_STEPS_TEST];
   VectorXd us[NUM_TIME_STEPS_TEST];
   MatrixXd Minvs[NUM_TIME_STEPS_TEST];
   MatrixXd dqdd_dqs[NUM_TIME_STEPS_TEST];
   MatrixXd dqdd_dqds[NUM_TIME_STEPS_TEST];
   #pragma unroll
   for(int i = 0; i < NUM_TIME_STEPS_TEST; i++){
      qs[i] = VectorXd::Zero(model.nq);
      qds[i] = VectorXd::Zero(model.nv);
      qdds[i] = VectorXd::Zero(model.nv);
      us[i] = VectorXd::Zero(model.nv);
      Minvs[i] = MatrixXd::Zero(model.nq,model.nq);
      dqdd_dqs[i] = MatrixXd::Zero(model.nv,model.nq);
      dqdd_dqds[i] = MatrixXd::Zero(model.nv,model.nv);
      #if TEST_FOR_EQUIVALENCE
         for(int j = 0; j < model.nq; j++){qs[i][j] = 0.1; qds[i][j] = 0.1; us[i][j] = 0.1;}
      #else
         for(int j = 0; j < model.nq; j++){qs[i][j] = getRand<double>(); qds[i][j] = getRand<double>(); us[i][j] = getRand<double>();}
      #endif
      aba(model,datas[0],qs[i],qds[i],us[i]);
      computeMinverse(model,datas[0],qs[i]);
      datas[0].Minv.template triangularView<Eigen::StrictlyLower>() = datas[0].Minv.transpose().template triangularView<Eigen::StrictlyLower>();
      qdds[i] = datas[0].ddq;
      Minvs[i] = datas[0].Minv;
   }

   // Single call
   if(NUM_TIME_STEPS_TEST == 1){
      #if TEST_FOR_EQUIVALENCE
         int SINGLE_TEST_ITERS = TEST_ITERS*1;
      #else
         int SINGLE_TEST_ITERS = TEST_ITERS*10;
      #endif
      MatrixXd drnea_dq = MatrixXd::Zero(model.nq,model.nq);
      MatrixXd drnea_dv = MatrixXd::Zero(model.nv,model.nv);
      MatrixXd drnea_da = MatrixXd::Zero(model.nv,model.nv);

      clock_gettime(CLOCK_MONOTONIC,&start);
      for(int i = 0; i < SINGLE_TEST_ITERS; i++){int ind = 0;
         computeRNEADerivatives(model,datas[0],qs[ind],qds[ind],qdds[ind],drnea_dq,drnea_dv,drnea_da);
         dqdd_dqs[ind] = -Minvs[ind]*drnea_dq;
         dqdd_dqds[ind] = -Minvs[ind]*drnea_dv;

         #if TEST_FOR_EQUIVALENCE
            std::cout << "q,qd,qdd,u" << std::endl;
            std::cout << qs[0].transpose() << std::endl;
            std::cout << qds[0].transpose() << std::endl;
            std::cout << qdds[0].transpose() << std::endl;
            std::cout << us[0].transpose() << std::endl;
            std::cout << "Minv" << std::endl << Minvs[0] << std::endl;
            std::cout << "dRNEA_dq" << std::endl << drnea_dq << std::endl;
            std::cout << "dRNEA_dqd" << std::endl << drnea_dv << std::endl;
            std::cout << "dqdd_dq" << std::endl << dqdd_dqs[0] << std::endl;
            std::cout << "dqdd_dqd" << std::endl << dqdd_dqds[0] << std::endl;
         #endif
      }
      clock_gettime(CLOCK_MONOTONIC,&end);
      printf("Single Call vaf+dc/du+dqdd/du %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(SINGLE_TEST_ITERS));

      clock_gettime(CLOCK_MONOTONIC,&start);
      for(int i = 0; i < SINGLE_TEST_ITERS; i++){int ind = 0;
         computeRNEADerivatives(model,datas[0],qs[ind],qds[ind],qdds[ind],drnea_dq,drnea_dv,drnea_da);
      }
      clock_gettime(CLOCK_MONOTONIC,&end);
      printf("Single Call vaf+dc/du %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(SINGLE_TEST_ITERS));

      clock_gettime(CLOCK_MONOTONIC,&start);
      for(int i = 0; i < SINGLE_TEST_ITERS; i++){int ind = 0;
         rnea(model,datas[0],qs[ind],qds[ind],qdds[ind]);
      }
      clock_gettime(CLOCK_MONOTONIC,&end);
      printf("Single Call vaf %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(SINGLE_TEST_ITERS));

      clock_gettime(CLOCK_MONOTONIC,&start);
      for(int i = 0; i < SINGLE_TEST_ITERS; i++){int ind = 0;
         dqdd_dqs[ind] = -Minvs[ind]*drnea_dq;
         dqdd_dqds[ind] = -Minvs[ind]*drnea_dv;
      }
      clock_gettime(CLOCK_MONOTONIC,&end);
      printf("Single Call dqdd/du %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(SINGLE_TEST_ITERS));
      printf("----------------------------------------\n");
   }
   // multi call with threadPools
   else{
      ReusableThreads<NUM_THREADS_TEST> threads;
      std::vector<double> times = {};
      for(int iter = 0; iter < TEST_ITERS; iter++){
         clock_gettime(CLOCK_MONOTONIC,&start);
         dynamicsGradientReusableThreaded<NUM_THREADS_TEST,NUM_TIME_STEPS_TEST>(&model,datas,dqdd_dqs,dqdd_dqds,qs,qds,qdds,Minvs,&threads);
         clock_gettime(CLOCK_MONOTONIC,&end);
         times.push_back(time_delta_us_timespec(start,end));
      }
      printf("[N:%d]: CPU ReusableThreaded: ",NUM_TIME_STEPS_TEST); printStats<PRINT_DISTRIBUTIONS_GLOBAL>(&times);
      printf("----------------------------------------\n");
   }
}

int main(int argc, const char ** argv)
{
   test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,1>();
   #if !TEST_FOR_EQUIVALENCE
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,10>();
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,16>();
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,32>();
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,64>();
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,128>();
      test<TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL,256>();
   #endif
   return 0;
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2014 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 vsevolod vlaskine

#include <Eigen/Geometry>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include "polygon.h"

namespace snark {

template < typename C > static Eigen::Vector3d normal_impl( const C& corners )
{
    const Eigen::Vector3d& cross = ( corners[1] - corners[0] ).cross( corners[0] - corners[2] );
    return cross / cross.norm();
}

template < typename C > static Eigen::Vector3d projection_impl( const C& corners, const Eigen::Vector3d& rhs )
{
    const Eigen::Vector3d& n = normal_impl( corners );
    return rhs - n * ( rhs - corners[0] ).dot( n );
}

// See https://blogs.msdn.microsoft.com/rezanour/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests
static inline bool is_inside( const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c, const Eigen::Vector3d& p )
{
    const Eigen::Vector3d& u = b - a;
    const Eigen::Vector3d& v = c - a;
    const Eigen::Vector3d& w = p - a;

    const Eigen::Vector3d& v_cross_w = v.cross( w );
    const Eigen::Vector3d& v_cross_u = v.cross( u );

    if( v_cross_w.dot( v_cross_u ) < 0 ) { return false; }

    const Eigen::Vector3d& u_cross_w = u.cross( w );
    const Eigen::Vector3d& u_cross_v = -v_cross_u;

    if( u_cross_w.dot( u_cross_v ) < 0 ) { return false; }

    double denom = u_cross_v.norm();
    double r = v_cross_w.norm();
    double t = u_cross_w.norm();

    return( r + t <= denom );
}

template < typename C > static bool includes_impl( const C& corners, const Eigen::Vector3d& rhs )
{
    for( std::size_t i = 2; i < corners.size(); ++i )
    {
        if( is_inside( corners[0], corners[i-1], corners[i], rhs ) ) { return true; }
    }
    return false;
}

static inline double distance_to_line( const Eigen::Vector3d from, const Eigen::Vector3d& to, const Eigen::Vector3d& point )
{
    typedef Eigen::ParametrizedLine< double, 3 > line_t;
    line_t line( from, ( to - from ).normalized() );
    Eigen::Vector3d projection = line.projection( point );
    bool is_between = ( ( projection - from ).squaredNorm() + ( projection - to ).squaredNorm() ) <= ( to - from ).squaredNorm();
    return is_between ? ( projection - point ).norm() : std::min( ( from - point ).norm(), ( to - point ).norm() );
}

double convex_polygon::distance_from_border_to( const Eigen::Vector3d& rhs ) const
{
    double distance = distance_to_line( corners.front(), corners.back(), rhs );
    for( unsigned int i = 1; i < corners.size(); ++i )
    {
        double d = distance_to_line( corners[ i - 1 ], corners[i], rhs );
        if( d < distance ) { distance = d; }
    }
    return distance;
}

Eigen::Vector3d convex_polygon::normal() const { return normal_impl( corners ); }

bool convex_polygon::is_valid() const
{
    if( corners.size() < 3 ) { return false; }
    COMMA_THROW( comma::exception, "todo" );
    for( std::size_t i = 1; i < corners.size(); ++i )
    {
        // todo
    }
    return true;
}

Eigen::Vector3d convex_polygon::projection_of( const Eigen::Vector3d& rhs ) const { return projection_impl( corners, rhs ); }

bool convex_polygon::includes( const Eigen::Vector3d& rhs ) const { return includes_impl( corners, rhs ); }

Eigen::Vector3d triangle::normal() const { return normal_impl( corners ); }

bool triangle::is_valid() const
{
    if( corners.size() != 3 ) { return false; }
    const Eigen::Vector3d& cross = ( corners[1] - corners[0] ).cross( corners[0] - corners[2] );
    return !comma::math::equal( cross.norm(), 0 );
}

Eigen::Vector3d triangle::projection_of( const Eigen::Vector3d& rhs ) const { return projection_impl( corners, rhs ); }

bool triangle::includes( const Eigen::Vector3d& rhs ) const { return includes_impl( corners, rhs ); }

double triangle::circumscribing_radius() const
{
    COMMA_THROW( comma::exception, "todo" );
    const Eigen::Vector3d& a = corners[1] - corners[0];
    const Eigen::Vector3d& b = corners[2] - corners[1];
    const Eigen::Vector3d& c = corners[0] - corners[2];
    return a.norm() / ( std::sqrt( 1 - b.dot( c ) / ( b.squaredNorm() * c.squaredNorm() ) ) * 2 );
}

} // namespace snark {


#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <QDebug>
#include <QPrinter>

#include <cmath>
#include <iostream>

#include "MotionModes/crab.h"
#include "MotionModes/motion_mode.h"
#include "MotionModes/tangent_aligned.h"
#include "wheel.h"

#include <dlib/global_optimization.h>

MainWindow::MainWindow(QWidget* parent) : QMainWindow(parent), ui(new Ui::MainWindow), scene(new CustomScene)
{
  ui->setupUi(this);

  ui->graphicsView->setScene(scene);
  new QGraphicsViewZoom(ui->graphicsView);

  Eigen::MatrixX2d cp1, cp2;
  cp1.resize(4, 2);
  cp2.resize(4, 2);

  cp1 << -1, 0, -1, -4. / 3, 1, -4. / 3, 1.5, -0.5;
  cp1 *= 300;
  cp2 << 1.5, -0.5, 1, 2, 2, 1, 3, 1;
  cp2 *= 150;
  cp2.col(0) *= 2;

  curve_1 = new qCurve(Bezier::Curve(cp1).splitCurve(0.5).second, ui, 1);
  curve_2 = new qCurve(cp2, ui, 2);

  curve_2->applyContinuity(*curve_1, {1.4});

  curve_1->elevateOrder();
  curve_1->elevateOrder();
  curve_1->elevateOrder();

  curve_2->elevateOrder();
  curve_2->elevateOrder();
  curve_2->elevateOrder();

  scene->addItem(curve_1);
  scene->addItem(curve_2);

  curve_1->setDraw_control_points(true);
  curve_2->setDraw_control_points(true);

  ui->graphicsView->centerOn(scene->itemsBoundingRect().center());

  // plots
  ui->plot2->yAxis2->setVisible(true);

  // add title layout element:
  //  ui->plot1->plotLayout()->insertRow(0);
  //  ui->plot2->plotLayout()->insertRow(0);
  //  ui->plot3->plotLayout()->insertRow(0);
  //  ui->plot1->setFont(QFont("Times", 12));
  //  ui->plot2->setFont(QFont("Times", 12));
  //  ui->plot3->setFont(QFont("Times", 12));
  //  ui->plot1->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot1, "Speed limit and speed profiles"));
  //  ui->plot2->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot2, "Angular velocities"));
  //  ui->plot3->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot3, "Vehicle orientation and steering
  //  angles"));

  // set labels:
  ui->plot1->xAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->xAxis->setLabelFont(QFont("Times", 10));
  ui->plot1->xAxis->setLabel("Distance along path [m]");
  ui->plot2->xAxis->setLabel("Distance along path [m]");

  ui->plot1->yAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->yAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->yAxis2->setLabelFont(QFont("Times", 10));
  ui->plot1->yAxis->setLabel("Speed [m/s]");
  ui->plot2->yAxis->setLabel("Vehicle orientation and wheel steering angle [°]");
  ui->plot2->yAxis2->setLabel("Angular velocity [°/s]");

  // set ticks:
  ui->plot1->xAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->xAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot1->yAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->yAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->yAxis2->setTickLabelFont(QFont("Times", 10));

  // show legend
  ui->plot1->legend->setVisible(true);
  ui->plot2->legend->setVisible(true);
  ui->plot1->legend->setFont(QFont("Times", 10));
  ui->plot2->legend->setFont(QFont("Times", 10));
  ui->plot1->legend->setRowSpacing(-5);
  ui->plot2->legend->setRowSpacing(-5);
  ui->plot1->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignBottom | Qt::AlignRight);
  ui->plot2->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignTop | Qt::AlignRight);

  // open in new window
  //  ui->plot1->setParent(nullptr);
  //  ui->plot2->setParent(nullptr);
  ui->plot1->setMinimumSize(400, 300);
  ui->plot2->setMinimumSize(400, 300);
  ui->plot1->show();
  ui->plot2->show();

  auto saveAs = []() {
    QFileDialog dialog(nullptr, "Export PDF");
    dialog.setNameFilter("PDF (*.pdf)");
    dialog.setFileMode(QFileDialog::AnyFile);
    dialog.setAcceptMode(QFileDialog::AcceptSave);
    dialog.setDefaultSuffix("pdf");
    if (dialog.exec())
      return dialog.selectedFiles().first();
    return QString();
  };

  connect(ui->plot1, &QCustomPlot::mouseRelease, this, [&](QMouseEvent* e) {
    if (e->button() == Qt::RightButton)
    {
      auto file_path = saveAs();
      if (file_path == QString())
        return;
      ui->plot1->savePdf(file_path, 0, 0, QCP::epNoCosmetic);
    }
  });
  connect(ui->plot2, &QCustomPlot::mouseRelease, this, [&](QMouseEvent* e) {
    if (e->button() == Qt::RightButton)
    {
      auto file_path = saveAs();
      if (file_path == QString())
        return;
      ui->plot2->savePdf(file_path, 0, 0, QCP::epNoCosmetic);
    }
  });

  //    QPrinter printer(QPrinter::HighResolution);
  //    printer.setPageSize(
  //        QPageSize(scene->itemsBoundingRect().size().scaled(90, 90, Qt::AspectRatioMode::KeepAspectRatio),
  //                  QPageSize::Unit::Point));
  //    printer.setOrientation(QPrinter::Portrait);
  //    printer.setOutputFormat(QPrinter::PdfFormat);
  //    printer.setFullPage(true);
  //    auto file_path = saveAs();
  //    if (file_path == QString())
  //      return;
  //    printer.setOutputFileName(file_path);

  //    QPainter p;

  //    if (!p.begin(&printer))
  //    {
  //      qDebug() << "Error!";
  //      return;
  //    }

  //    this->scene->render(&p);
  //    p.end();

  //    int sl = file_path.lastIndexOf('/') + 1;
  //    QString file_path2 = file_path.left(sl) + "angle" + file_path.right(6);
  //    ui->plot2->savePdf(file_path2, 0, 0, QCP::epNoCosmetic);
  //    QString file_path3 = file_path.left(sl) + "speed" + file_path.right(6);
  //    ui->plot1->savePdf(file_path3, 0, 0, QCP::epNoCosmetic);
  //    // ovdje dodati export i ostalih

  ui->plot1->setInteraction(QCP::iRangeDrag, true);
  ui->plot2->setInteraction(QCP::iRangeDrag, true);

  ui->plot1->axisRect()->setRangeDrag(Qt::Horizontal);
  ui->plot2->axisRect()->setRangeDrag(Qt::Horizontal);

  ui->plot1->setInteraction(QCP::iRangeZoom, true);
  ui->plot2->setInteraction(QCP::iRangeZoom, true);

  ui->plot1->axisRect()->setRangeZoom(Qt::Horizontal);
  ui->plot2->axisRect()->setRangeZoom(Qt::Horizontal);

  // curves
  connect(ui->optimize, &QPushButton::pressed, this, &MainWindow::applyContinuity);
  //  connect(ui->alpha1, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->alpha2, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->n1, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->n2, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->mm1, qOverload<int>(&QComboBox::currentIndexChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->mm2, qOverload<int>(&QComboBox::currentIndexChanged), this, &MainWindow::applyContinuity);

  // vehicle tab
  ui->vehicle->header()->resizeSection(0, 150);
  ui->vehicle->header()->setSectionResizeMode(1, QHeaderView::Stretch);
  ui->vehicle->header()->setStretchLastSection(false);

  static auto addWheel = [&]() {
    Wheel* temp = new Wheel(ui->vehicle->topLevelItemCount(), ui->vehicle);
    ui->vehicle->addTopLevelItem(temp);
    temp->expand();
    connect(temp, &Wheel::wheelChanged, this, &MainWindow::updatePlot);
  };

  static auto removeWheel = [&]() { delete ui->vehicle->takeTopLevelItem(ui->vehicle->topLevelItemCount() - 1); };

  for (uint k = 0; k < ui->wheel_num->value(); k++)
    addWheel();
  connect(scene, &QGraphicsScene::changed, this, &MainWindow::updatePlot);

  connect(ui->wheel_num, qOverload<int>(&QSpinBox::valueChanged), this, [&](int n) {
    while (n > ui->vehicle->topLevelItemCount())
      addWheel();
    while (n < ui->vehicle->topLevelItemCount())
      removeWheel();
    updatePlot();
  });

  //  curve_1->setVisible(false);
  //  curve_2->setVisible(false);

  //  ui->wheel_num->setValue(6);

  //  static const QVector<QColor> matlab_colors(
  //      {{0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, {162, 20,
  //      47}});
  //  QPainterPath vehicle_frame;

  //  vehicle_frame = QPainterPath();
  //  vehicle_frame.moveTo(50, 0);
  //  vehicle_frame.lineTo(0, 0);
  //  vehicle_frame.lineTo(0, -50);
  //  vehicle_frame.lineTo(-3,-47);
  //  vehicle_frame.lineTo(0, -50);
  //  vehicle_frame.lineTo(3,-47);

  //  QVector<QString> w;
  //  w.push_back(u8"w\u2081");
  //  w.push_back(u8"w\u2082");
  //  w.push_back(u8"w\u2083");
  //  w.push_back(u8"w\u2084");
  //  w.push_back(u8"w\u2085");
  //  w.push_back(u8"w\u2086");

  //    scene->addPath(vehicle_frame, QPen(matlab_colors[0], 3));
  //    for(int k = 0; k < ui->wheel_num->value(); k++)
  //    {
  //      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
  //      auto R = wheel->getR();
  //      scene->addRect(QRect(R.x()*10-100, -(R.y()*10-50), 200, -100), QPen(matlab_colors[k+1], 1));
  //      scene->addLine(0,0,R.x()*10, -R.y()*10);
  //      auto x = scene->addText(w[k], QFont("Times", 24));
  //      x->setPos(R.x()*10, -R.y()*10);
  //    }

  //  scene->update();

  //  {
  //      QPrinter printer( QPrinter::HighResolution );
  //      printer.setPageSize(QPageSize(qCurve(cp2*5).boundingRect().size(), QPageSize::Unit::Point));
  //      printer.setOrientation( QPrinter::Portrait );
  //      printer.setOutputFormat( QPrinter::PdfFormat );
  //      printer.setFullPage(true);
  //      printer.setOutputFileName( "/home/mirko/Pictures/motion_modes/" + qCurve::getName());

  //      QPainter p;

  //      if( !p.begin( &printer ) )
  //      {
  //          qDebug() << "Error!";
  //          return;
  //      }
  //      this->scene->render( &p );
  //      p.end();
  //  }

  //  auto dC = curve_1->derivativeAt(1);
  //  ui->alpha2->setValue(std::atan2(dC.y(), dC.x()) * 180 / M_PI);
  //  ui->mm2->setCurrentIndex(2);
}

MainWindow::~MainWindow() { delete ui; }

void MainWindow::applyContinuity()
{
  Romb::MotionModes::MotionMode *mm_1, *mm_2;
  double alpha_1 = ui->alpha1->value() * M_PI / 180;
  double alpha_2 = ui->alpha2->value() * M_PI / 180;
  double n_1 = ui->n1->value();
  double n_2 = ui->n2->value();

  switch (ui->mm1->currentIndex())
  {
  case 0:
    mm_1 = new Romb::MotionModes::TangentAligned(alpha_1);
    break;
  case 1:
    mm_1 = new Romb::MotionModes::Crab(alpha_1);
    break;
  case 2:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, n_1);
    break;
  default:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, -n_1);
    break;
  }

  switch (ui->mm2->currentIndex())
  {
  case 0:
    mm_2 = new Romb::MotionModes::TangentAligned(alpha_2);
    break;
  case 1:
    mm_2 = new Romb::MotionModes::Crab(alpha_2);
    break;
  case 2:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, n_2);
    break;
  default:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, -n_2);
    break;
  }

  dlib::thread_pool tp(std::thread::hardware_concurrency());
  std::function<void(std::vector<double>, Bezier::Curve&, Bezier::Curve&)> applyParams;
  std::function<double(std::vector<double>)> evaluate = [&](std::vector<double> params) {
    Bezier::Curve temp1 = *curve_1;
    Bezier::Curve temp2 = *curve_2;
    applyParams(params, temp1, temp2);

    Data sim = calculateData(temp1, temp2, true);

    auto time = [](QVector<double> v, QVector<double> s) {
      double res{0.0};
      for (int k = 1; k < s.size(); k++)
        res += 2 * (s[k] - s[k - 1]) / (v[k] + v[k - 1]);
      return res;
    };

    return time(sim.v_sim1, sim.s1) + time(sim.v_sim2, sim.s2);
  };

  constexpr std::chrono::seconds opt_max_duration(15);

  if (mm_1->type == "aligned" && mm_2->type == "aligned")
  {
    ui->alpha2->setValue(alpha_1);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      curve_2.applyContinuity(curve_1, {params[0], params[1], params[2]});
    };
    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2) {
          return evaluate({x0, x1, x2});
        },
        {0.01, 0, 0}, {5, 5, 5}, opt_max_duration);

    applyParams(std::vector<double>(result.x.begin(), result.x.end()), *curve_1, *curve_2);
  }
  if ((mm_1->type == "aligned" || mm_1->type == "exponential_1") && mm_2->type == "crab")
  {
    auto dc = curve_1->derivativeAt(1);
    ui->alpha2->setValue((std::atan2(dc.y(), dc.x()) + alpha_1) * 180 / M_PI);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      auto dc = curve_1.derivativeAt(1);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 3, curve_1.endPoints().second - params[2] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[3] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[4] * dc);
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3, double x4) {
          return evaluate({x0, x1, x2, x3, x4});
        },
        {0.1, 0, 0, 0.1, 0}, {5, 5, 5, 5, 5}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3), result.x(4)}, *curve_1, *curve_2);
  }
  if ((mm_2->type == "aligned" || mm_2->type == "exponential_2") && mm_1->type == "crab")
  {
    auto dc = curve_1->derivativeAt(1);
    ui->alpha2->setValue((std::atan2(dc.y(), dc.x()) + alpha_1) * 180 / M_PI);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_2, Bezier::Curve& curve_1) {
      auto dc = curve_1.derivativeAt(0);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 3, curve_1.endPoints().second - params[2] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[3] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[4] * dc);
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3, double x4) {
          return evaluate({x0, x1, x2, x3, x4});
        },
        {0.1, 0, 0, 0.1, 0}, {5, 5, 5, 5, 5}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3), result.x(4)}, *curve_1, *curve_2);
  }

  if (mm_1->type == "aligned" && mm_2->type == "exponential_2")
  {
    ui->alpha2->setValue(ui->alpha1->value());
    applyParams = [&](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      auto dc = curve_1.derivativeAt(1);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[2] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[3] * dc);

      double beta_1 = curve_1.derivativeAt(1).norm() / curve_2.derivativeAt(0).norm();
      double beta_2 = curve_1.derivativeAt(1).norm() / curve_2.derivativeAt(0).norm();

      double dC3_scale = n_2 * n_2 * (beta_1 * beta_1 * beta_1);

      double beta_3 = (curve_2.derivativeAt(3, 0).norm() * dC3_scale -
                       beta_1 * beta_1 * beta_1 * curve_2.derivativeAt(2, 0).norm() -
                       2 * beta_1 * beta_2 * curve_2.derivativeAt(2, 0).norm()) /
                      curve_2.derivativeAt(0).norm();

      curve_2.applyContinuity(curve_1, {beta_1, beta_2, beta_3});
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3) {
          return evaluate({x0, x1, x2, x3});
        },
        {0.0, 0, 0.0, 0.0}, {3, 3, 3, 3}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3)}, *curve_1, *curve_2);
  }

  scene->update();
  updatePlot();
}

void MainWindow::updatePlot()
{
  const QString _max(u8"\u2098\u2090\u2093");
  const QString _sim(u8"\u209b\u1d62\u2098");

  Data data = calculateData(*curve_1, *curve_2);
  int N = ui->vehicle->topLevelItemCount();
  static const QVector<QColor> matlab_colors(
      {{0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, {162, 20, 47}});

  ////// PLOTANJE
  QPen pen;
  pen.setStyle(Qt::DashLine);
  static QCPItemStraightLine* infLine1 = new QCPItemStraightLine(ui->plot1);
  static QCPItemStraightLine* infLine2 = new QCPItemStraightLine(ui->plot2);
  infLine1->setPen(pen);
  infLine2->setPen(pen);
  infLine1->point1->setCoords(data.s1.back(), 0); // location of point 1 in plot coordinate
  infLine1->point2->setCoords(data.s1.back(), 1); // location of point 2 in plot coordinate
  infLine2->point1->setCoords(data.s1.back(), 0); // location of point 1 in plot coordinate
  infLine2->point2->setCoords(data.s1.back(), 1); // location of point 2 in plot coordinate

  /// plot brzina
  {
    constexpr int n_v = 4;
    while (ui->plot1->graphCount() < 4 * N + n_v)
      ui->plot1->addGraph();
    while (ui->plot1->graphCount() > 4 * N + n_v)
      ui->plot1->removeGraph(ui->plot1->graphCount() - 1);

    auto g_v1 = ui->plot1->graph(0);
    auto g_v2 = ui->plot1->graph(1);
    auto g_v_sim1 = ui->plot1->graph(2);
    auto g_v_sim2 = ui->plot1->graph(3);

    g_v1->setData(data.s1, data.v1, true);
    g_v2->setData(data.s2, data.v2, true);
    g_v_sim1->setData(data.s1, data.v_sim1, true);
    g_v_sim2->setData(data.s2, data.v_sim2, true);

    g_v1->setName("v" + _max);
    g_v2->removeFromLegend();
    g_v_sim1->setName("v" + _sim);
    g_v_sim2->removeFromLegend();

    pen.setStyle(Qt::SolidLine);
    pen.setColor(matlab_colors[0]);
    pen.setWidth(3);
    g_v1->setPen(pen);
    g_v2->setPen(pen);
    pen.setStyle(Qt::DashLine);
    g_v_sim1->setPen(pen);
    g_v_sim2->setPen(pen);

    for (int k = 0; k < N; k++)
    {
      auto g_w_v1 = ui->plot1->graph(n_v + 4 * k);
      auto g_w_v2 = ui->plot1->graph(n_v + 4 * k + 1);
      auto g_w_v_sim1 = ui->plot1->graph(n_v + 4 * k + 2);
      auto g_w_v_sim2 = ui->plot1->graph(n_v + 4 * k + 3);

      g_w_v1->setName("w" + QString::number(k + 1) + _max);
      g_w_v2->removeFromLegend();
      g_w_v_sim1->setName("w" + QString::number(k + 1) + _sim);
      g_w_v_sim2->removeFromLegend();

      g_w_v1->setData(data.s1, data.v_w1[k], true);
      g_w_v2->setData(data.s2, data.v_w2[k], true);
      g_w_v_sim1->setData(data.s1, data.v_w_sim1[k], true);
      g_w_v_sim2->setData(data.s2, data.v_w_sim2[k], true);

      pen.setWidth(1);
      pen.setColor(matlab_colors[k + 1]);
      pen.setStyle(Qt::SolidLine);
      g_w_v1->setPen(pen);
      g_w_v2->setPen(pen);
      pen.setStyle(Qt::DashLine);
      g_w_v_sim1->setPen(pen);
      g_w_v_sim2->setPen(pen);
    }

    ui->plot1->rescaleAxes();
    ui->plot1->yAxis->scaleRange(1.05);
    ui->plot1->replot();
  }

  /// plot kutnih brzina
  {
    constexpr int n_w = 4;
    while (ui->plot2->graphCount() < 4 * N + n_w)
    {
      ui->plot2->addGraph();
      ui->plot2->addGraph(ui->plot2->xAxis, ui->plot2->yAxis2);
    }
    while (ui->plot2->graphCount() > 4 * N + n_w)
      ui->plot2->removeGraph(ui->plot2->graphCount() - 1);

    auto g_theta1 = ui->plot2->graph(0);
    auto g_theta2 = ui->plot2->graph(2);
    auto g_w_sim1 = ui->plot2->graph(1);
    auto g_w_sim2 = ui->plot2->graph(3);

    g_theta1->setName(u8"\u03b8");
    g_theta2->removeFromLegend();
    g_w_sim1->setName(u8"\u03c9" + _sim);
    g_w_sim2->removeFromLegend();

    g_theta1->setData(data.s1, data.theta1, true);
    g_theta2->setData(data.s2, data.theta2, true);
    g_w_sim1->setData(data.s1, data.w_sim1, true);
    g_w_sim2->setData(data.s2, data.w_sim2, true);

    pen.setStyle(Qt::SolidLine);
    pen.setColor(matlab_colors[0]);
    pen.setWidth(3);
    g_theta1->setPen(pen);
    g_theta2->setPen(pen);
    pen.setStyle(Qt::DashLine);
    g_w_sim1->setPen(pen);
    g_w_sim2->setPen(pen);

    for (int k = 0; k < N; k++)
    {
      auto g_delta1 = ui->plot2->graph(n_w + 4 * k + 0);
      auto g_delta2 = ui->plot2->graph(n_w + 4 * k + 2);
      auto g_w_w_sim1 = ui->plot2->graph(n_w + 4 * k + 1);
      auto g_w_w_sim2 = ui->plot2->graph(n_w + 4 * k + 3);

      g_delta1->setName(u8"\u03b4" + QString::number(k + 1));
      g_delta2->removeFromLegend();
      g_w_w_sim1->setName(u8"\u03c9w" + QString::number(k + 1) + _sim);
      g_w_w_sim2->removeFromLegend();

      g_delta1->setData(data.s1, data.delta1[k], true);
      g_delta2->setData(data.s2, data.delta2[k], true);
      g_w_w_sim1->setData(data.s1, data.w_w_sim1[k], true);
      g_w_w_sim2->setData(data.s2, data.w_w_sim2[k], true);

      pen.setStyle(Qt::SolidLine);
      pen.setWidth(1);
      pen.setColor(matlab_colors[k + 1]);
      g_delta1->setPen(pen);
      g_delta2->setPen(pen);
      pen.setStyle(Qt::DashLine);
      g_w_w_sim1->setPen(pen);
      g_w_w_sim2->setPen(pen);
    }

    ui->plot2->rescaleAxes();
    ui->plot2->yAxis->scaleRange(1.05);
    ui->plot2->yAxis2->scaleRange(1.05);
    ui->plot2->replot();
  }

  auto print = [](double x) {
    std::ostringstream res;
    res.precision(3);

    if (x < 0)
      res << "{" << std::fixed << x << "}";
    else
      res << "  " << std::fixed << x << " ";
    return res;
  };

  std::stringstream bezier1, bezier2;
  auto cp1 = curve_1->controlPoints();
  auto cp2 = curve_1->controlPoints();
  for (auto& p : cp1)
  {
    p = {std::round(p.x() * 10) / 1000., std::round(p.y() * 10) / 1000.};
    bezier1 << "( " << print(p.x()).str() << ",&\\ " << print(p.y()).str() << " ) \\\\\n";
  }
  for (auto& p : cp1)
  {
    p = {std::round(p.x() * 10) / 1000., std::round(p.y() * 10) / 1000.};
    bezier2 << "( " << print(p.x()).str() << ",&\\ " << print(p.y()).str() << " ) \\\\\n";
  }
  cp1.back() = {std::round(cp1.back().x() * 10) / 1000., std::round(cp1.back().y() * 10) / 1000.};
  bezier1 << "( " << print(cp1.back().x()).str() << ",&\\ " << print(cp1.back().y()).str() << " )";
  cp2.back() = {std::round(cp2.back().x() * 10) / 1000., std::round(cp2.back().y() * 10) / 1000.};
  bezier2 << "( " << print(cp2.back().x()).str() << ",&\\ " << print(cp2.back().y()).str() << " )";

  ui->bezier1->setText(QString::fromStdString(bezier1.str()));
  ui->bezier2->setText(QString::fromStdString(bezier2.str()));
}

MainWindow::Data MainWindow::calculateData(const Bezier::Curve& curve_1, const Bezier::Curve& curve_2, bool partial)
{
  ///////// DATA
  ///
  ///
  ///
  ///
  constexpr double step = 0.005;
  constexpr int samples = static_cast<int>(1. / step + 1);
  constexpr double a = 0.5;
  int N = ui->vehicle->topLevelItemCount();
  Romb::MotionModes::MotionMode *mm_1, *mm_2;
  double alpha_1 = ui->alpha1->value() * M_PI / 180;
  double alpha_2 = ui->alpha2->value() * M_PI / 180;
  double n_1 = ui->n1->value();
  double n_2 = ui->n2->value();
  double v_max_1 = ui->vmax1->value() * 100;
  double v_max_2 = ui->vmax2->value() * 100;
  switch (ui->mm1->currentIndex())
  {
  case 0:
    mm_1 = new Romb::MotionModes::TangentAligned(alpha_1);
    break;
  case 1:
    mm_1 = new Romb::MotionModes::Crab(alpha_1);
    break;
  case 2:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, n_1);
    break;
  default:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, -n_1);
    break;
  }
  switch (ui->mm2->currentIndex())
  {
  case 0:
    mm_2 = new Romb::MotionModes::TangentAligned(alpha_2);
    break;
  case 1:
    mm_2 = new Romb::MotionModes::Crab(alpha_2);
    break;
  case 2:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, n_2);
    break;
  default:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, -n_2);
    break;
  }

  Data results(N, samples, partial);

  auto& t = results.t;
  auto& s1 = results.s1;
  auto& s2 = results.s2;
  auto& v1 = results.v1;
  auto& v2 = results.v2;
  auto& v_sim1 = results.v_sim1;
  auto& v_sim2 = results.v_sim2;
  auto& w_sim1 = results.w_sim1;
  auto& w_sim2 = results.w_sim2;
  auto& theta1 = results.theta1;
  auto& theta2 = results.theta2;
  auto& delta1 = results.delta1;
  auto& delta2 = results.delta2;
  auto& v_w1 = results.v_w1;
  auto& v_w2 = results.v_w2;
  auto& v_w_sim1 = results.v_w_sim1;
  auto& v_w_sim2 = results.v_w_sim2;
  auto& w_w_sim1 = results.w_w_sim1;
  auto& w_w_sim2 = results.w_w_sim2;

  double S = curve_1.length() / 100;

  /////////// izracun limita brzine
  ///
  ///
  ///
  ///
  for (int idx = 0; idx < samples; idx++)
  {
    t[idx] = idx * step;
    s1[idx] = curve_1.length(t[idx]) / 100;
    s2[idx] = S + curve_2.length(t[idx]) / 100;
    v1[idx] = v_max_1;
    v2[idx] = v_max_2;

    for (int k = 0; k < N; k++)
    {
      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));

      // single wheel limit #1
      v_w1[k][idx] = wheel->vMax(curve_1, mm_1, t[idx]);
      if (v_w1[k][idx] < v1[idx])
        v1[idx] = v_w1[k][idx];

      // single wheel limit #2
      v_w2[k][idx] = wheel->vMax(curve_2, mm_2, t[idx]);
      if (v_w2[k][idx] < v2[idx])
        v2[idx] = v_w2[k][idx];
    }
    v1[idx] /= 100;
    v2[idx] /= 100;

    // combined speed limit
    for (int k = 0; k < N; k++)
    {
      v_w1[k][idx] = static_cast<Wheel*>(ui->vehicle->topLevelItem(k))->Rv(curve_1, mm_1, t[idx]) * v1[idx];
      v_w2[k][idx] = static_cast<Wheel*>(ui->vehicle->topLevelItem(k))->Rv(curve_2, mm_2, t[idx]) * v2[idx];
    }
  }

  //////// simulacija brzine
  ///
  ///
  ///
  ///
  QVector<double> v_full = v1;
  QVector<double> s_full = s1;

  v_full.append(v2);
  s_full.append(s2);

  int last_idx = std::numeric_limits<int>::infinity();

  auto fill_sim = [&](double v, int idx) {
    auto fill_all = [&](double v, int idx) {
      auto& v_w_sim = idx < samples ? v_w_sim1 : v_w_sim2;
      auto& curve = idx < samples ? curve_1 : curve_2;
      auto& mm = idx < samples ? mm_1 : mm_2;
      idx = idx < samples ? idx : idx - samples;

      for (int k = 0; k < N; k++)
      {
        auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
        v_w_sim[k][idx] = wheel->Rv(curve, mm, t[idx]) * v;
      }
    };

    for (int k = last_idx + 1; k <= idx; k++)
    {
      if (k == samples)
      {
        v_sim2[0] = v_sim1[k - 1];
        fill_all(v_sim2[0], k);
      }
      else
      {
        double dS = s_full[k] - s_full[k - 1];
        auto& v_sim = k < samples ? v_sim1 : v_sim2;
        int temp_idx = k < samples ? k : k - samples;
        if (v > v_sim[temp_idx - 1])
          v_sim[temp_idx] = std::min(v, std::sqrt(v_sim[temp_idx - 1] * v_sim[temp_idx - 1] + 2 * a * dS));
        else
          v_sim[temp_idx] = std::max(v, std::sqrt(v_sim[temp_idx - 1] * v_sim[temp_idx - 1] - 2 * a * dS));
        fill_all(v_sim[temp_idx], k);
      }
    }

    last_idx = idx;
  };

  std::function<void(int, double, int, double)> d_and_c = [&](int idx1, double v_1, int idx3, double v_3) {
    if (idx1 + 1 == idx3)
    {
      fill_sim(v_1, idx1);
      return;
    }

    int idx2 =
        static_cast<int>(std::min_element(v_full.begin() + idx1 + 1, v_full.begin() + idx3 - 1) - v_full.begin());

    double v_reachable_1 = std::sqrt(v_1 * v_1 + 2 * a * (s_full[idx2] - s_full[idx1]));
    double v_reachable_2 = std::sqrt(v_3 * v_3 + 2 * a * (s_full[idx3] - s_full[idx2]));

    if (v_reachable_1 > std::min(v_reachable_2, v_full[idx2]))
      d_and_c(idx1, v_1, idx2, std::min({v_reachable_1, v_reachable_2, v_full[idx2]}));
    else
      fill_sim(v_1, idx1);

    if (v_reachable_2 > std::min(v_reachable_1, v_full[idx2]))
      d_and_c(idx2, std::min({v_reachable_1, v_reachable_2, v_full[idx2]}), idx3, v_3);
    else
      fill_sim(std::min({v_reachable_1, v_reachable_2, v_full[idx2]}), idx2);
  };

  d_and_c(0, 0.0, 2 * samples - 1, 0.0);
  fill_sim(0.0, 2 * samples - 1);

  // return if only speed sim is wanted
  if (partial)
    return results;

  /////////// angular velocity and orientation sim
  ///
  ///
  ///
  ///
  ///

  for (int idx = 0; idx < samples; idx++)
  {
    theta1[idx] = mm_1->phi(curve_1, t[idx]) * 180 / M_PI;
    theta2[idx] = mm_2->phi(curve_2, t[idx]) * 180 / M_PI;
    w_sim1[idx] =
        v_sim1[idx] * mm_1->phiDerived(curve_1, t[idx]) / curve_1.derivativeAt(t[idx]).norm() * 100 * 180 / M_PI;
    w_sim2[idx] =
        v_sim2[idx] * mm_2->phiDerived(curve_2, t[idx]) / curve_2.derivativeAt(t[idx]).norm() * 100 * 180 / M_PI;

    for (int k = 0; k < N; k++)
    {
      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
      delta1[k][idx] = wheel->delta(curve_1, mm_1, t[idx]) * 180 / M_PI;
      delta2[k][idx] = wheel->delta(curve_2, mm_2, t[idx]) * 180 / M_PI;
      w_w_sim1[k][idx] = v_sim1[idx] * wheel->Rw(curve_1, mm_1, t[idx]) * 100 * 180 / M_PI;
      w_w_sim2[k][idx] = v_sim2[idx] * wheel->Rw(curve_2, mm_2, t[idx]) * 100 * 180 / M_PI;
    }
  }

  return results;
}

MainWindow::Data::Data(int N, int samples, bool partial)
{
  t.resize(samples);
  s1.resize(samples);
  s2.resize(samples);
  v1.resize(samples);
  v2.resize(samples);

  v_sim1.resize(samples);
  v_sim2.resize(samples);
  v_w1.resize(N);
  v_w2.resize(N);
  v_w_sim1.resize(N);
  v_w_sim2.resize(N);
  if (!partial)
  {
    theta1.resize(samples);
    theta2.resize(samples);
    w_sim1.resize(samples);
    w_sim2.resize(samples);
    delta1.resize(N);
    delta2.resize(N);
    w_w_sim1.resize(N);
    w_w_sim2.resize(N);
  }

  for (int k = 0; k < N; k++)
  {
    v_w1[k].resize(samples);
    v_w2[k].resize(samples);
    v_w_sim1[k].resize(samples);
    v_w_sim2[k].resize(samples);
    if (!partial)
    {
      delta1[k].resize(samples);
      delta2[k].resize(samples);
      w_w_sim1[k].resize(samples);
      w_w_sim2[k].resize(samples);
    }
  }
}


#include <ql/quantlib.hpp>
//#include <ql/userconfig.hpp>
#include <iostream>
#include <vector>
#include <boost/foreach.hpp>

using namespace std;
using namespace QuantLib;

int main(void) {

    Calendar mycal = China();

        vector<Date> hl = mycal.holidayList(mycal, Date(11,Mar,2016), Date(20,Apr,2016), true);

        BOOST_FOREACH(Date& d, hl) {

            cout << d << endl;
        }
/*

    int n;
    for (n = Jan; n <= Dec; n++) {

        Date d(15, (Month)n, 2016);
        //cout << d << endl;
        cout << mycal.adjust(d, Preceding) << endl;
    }
*/

    return 0;
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef RECTANGLE_2_COORDINATES_HPP
#define RECTANGLE_2_COORDINATES_HPP

#include "coordinates.hpp"
#include "rjmcmc/geometry/Rectangle_2.hpp"
#include <boost/iterator/iterator_facade.hpp>
#include <boost/random/uniform_smallint.hpp>

//    void advance(int i) { m_i+=i; }

template<typename K>
struct rectangle_coordinates_iterator
        : public boost::iterator_facade<rectangle_coordinates_iterator<K>, const typename K::FT, boost::forward_traversal_tag>
{

    enum { dimension = 5 };
    rectangle_coordinates_iterator() : m_i(dimension) {}
    template<typename Engine>
    explicit rectangle_coordinates_iterator(const geometry::Rectangle_2<K>& r, Engine& e) : m_i(0) {
        boost::uniform_smallint<> die(0,3);
        init(r.rotate(die(e)));
    }
    explicit rectangle_coordinates_iterator(const geometry::Rectangle_2<K>& r           ) : m_i(0) { init(r); }
    typedef typename K::FT FT;

private:
    void init(const geometry::Rectangle_2<K>& r)
    {
        m_coord[0] = r.center().x();
        m_coord[1] = r.center().y();
        m_coord[2] = r.normal().x();
        m_coord[3] = r.normal().y();
        m_coord[4] = r.ratio();
    }

    friend class boost::iterator_core_access;
    void increment() { ++m_i; }
    const FT& dereference() const {
        //assert(m_i<dimension);
        return m_coord[m_i];
    }
    FT m_coord[dimension];
    unsigned int m_i;
};

template<typename K>
struct coordinates_iterator< geometry::Rectangle_2<K> >
{
    typedef rectangle_coordinates_iterator<K> type;
    enum { dimension = type::dimension };
};

template<typename K>
struct object_from_coordinates< geometry::Rectangle_2<K> > {
    template<typename Iterator> geometry::Rectangle_2<K> operator()(Iterator it) const {
        typename K::FT x = *it++;
        typename K::FT y = *it++;
        typename K::FT u = *it++;
        typename K::FT v = *it++;
        typename K::Point_2  p(x,y);
        typename K::Vector_2 n(u,v);
        return geometry::Rectangle_2<K>(p,n,*it);
    }
};

#endif // RECTANGLE_2_COORDINATES_HPP


/*
 * 根据2D图像和深度图生成点云
 * Author: YangQun
 * Date: 2018/5/9
 * Last Update:2018/5/9
 */
// C++ 标准库
#include <iostream>
#include <string>
#include <vector>

// 第三方库头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

// 工程头文件
#include "generate_point_cloud.h"
#include "param_reader.h"

using namespace std;

PointCloudGenerator::PointCloudGenerator(PinHoleCamera::Ptr cam, ParameterReader::Ptr param_reader)
// viewer_("viewer")	// 初始化点云显示窗口的名字
{
	cam_ = cam;
	is_show_ = param_reader->getParam<bool>("is_show_point_cloud");
	is_save_ = param_reader->getParam<bool>("is_save_point_cloud");
	grid_size_ = param_reader->getParam<double>("voxel_grid");
	is_filter_ = param_reader->getParam<bool>("is_filter");

	if (is_filter_) {
		voxel_.setLeafSize(grid_size_, grid_size_, grid_size_);
	}

	cloud_out_.reset(new PointCloud);

	if (is_show_) {
		viewer_ = new pcl::visualization::CloudViewer("viewer");
	}
	else {
		viewer_ = nullptr;
	}
}

PointCloudGenerator::~PointCloudGenerator()
{
	if (nullptr != viewer_)
		delete viewer_;
}

PointCloud::Ptr PointCloudGenerator::generatePointCloud(Frame::Ptr frame)
{
    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud ( new PointCloud );
    // 遍历深度图
    for (int m = 0; m <  frame->depth_img_.rows; m++)
        for (int n=0; n < frame->depth_img_.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = frame->depth_img_.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
		continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / cam_->factor();
            p.x = (n - cam_->cx()) * p.z / cam_->fx();
            p.y = (m - cam_->cy()) * p.z / cam_->fy();

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = frame->rgb_img_.ptr<uchar>(m)[n*3];
            p.g = frame->rgb_img_.ptr<uchar>(m)[n*3+1];
            p.r = frame->rgb_img_.ptr<uchar>(m)[n*3+2];

            // 把p加入到点云中
            cloud->points.push_back( p );
        }

	// 设置点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	// cout<<"point cloud size = "<<cloud->points.size()<<endl;
	cloud->is_dense = false;

// 	if (is_save_) {
// 		pcl::io::savePCDFile( "../data/pointcloud.pcd", *cloud );
// 		cout<<"Point cloud saved."<<endl;
// 	}

// 	if (is_show_) {
// 		viewer_->showCloud(cloud);
// 		// while ( ! viewer_->wasStopped()) { }
// 	}

	// 清除数据并退出
	// cloud->points.clear();

	return cloud;
}

int PointCloudGenerator::joinPointCloud(Frame::Ptr frame)
{
	// 转换成点云
	PointCloud::Ptr curr_cloud = generatePointCloud(frame);

	// 合并点云
	PointCloud::Ptr cloud_temp(new PointCloud);
	Eigen::Matrix3d rotation_matrix = frame->T_c2w_.rotation_matrix();
	Eigen::Vector3d translation_vector = frame->T_c2w_.translation();
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
	T.rotate(rotation_matrix);
	T.pretranslate(translation_vector);
	pcl::transformPointCloud(*curr_cloud, *cloud_temp, T.matrix());		// 将当前点云变换到世界坐标系下
	*cloud_temp += *cloud_out_;									// 在世界坐标系下将已有点云和当前帧点云进行拼接
	cloud_out_->clear();											// 清空旧的世界点云数据

	if (is_filter_) {
		// 拼接之后的点云进行体素滤波降采样
		voxel_.setInputCloud(cloud_temp);
		voxel_.filter(*cloud_out_);
	}
	else {
		cloud_out_ = cloud_temp;
	}

	// 可视化
	if (is_show_) {
		viewer_->showCloud(cloud_out_);
		// while ( ! viewer.wasStopped()) { }
	}

	return 0;
}

int PointCloudGenerator::savePointCloud()
{
	// 保存成pcd文件
	if (is_save_) {
		pcl::io::savePCDFile("../data/joinPointCloud.pcd", *cloud_out_);
	}

	return 0;
}


/*
 * IntegrationErrorTerm.hpp
 *
 *  Created on: Jan 23, 2013
 *      Author: hannes
 */

#ifndef QINTEGRALERRORTERM_HPP_
#define QINTEGRALERRORTERM_HPP_

#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/random.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/Optimizer.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/ExpressionErrorTerm.hpp>

#include <aslam/splines/OPTBSpline.hpp>
#include <bsplines/BSplinePose.hpp>
#include <bsplines/UnitQuaternionBSpline.hpp>
#include <bsplines/NumericIntegrator.hpp>
#include <sm/boost/null_deleter.hpp>

namespace aslam {
namespace backend {
namespace integration {

namespace algorithms = ::numeric_integrator::algorithms;
typedef algorithms::Default DefaultAlgorithm;

namespace internal {
	template <typename ErrorTermReceiver, typename ErrorTerm>
	inline void addErrorTermToProblem(ErrorTermReceiver & problem, ErrorTerm *et, bool problemOwnsErrorTerms){
		problem.addErrorTerm(problemOwnsErrorTerms ? boost::shared_ptr<ErrorTerm>(et) : boost::shared_ptr<ErrorTerm>(et, sm::null_deleter()));
	}
	template <typename ErrorTermReceiver, typename ErrorTerm>
	inline void addErrorTermToProblem(ErrorTermReceiver & problem, boost::shared_ptr<ErrorTerm> et, bool /* problemOwnsErrorTerms */){
		problem.addErrorTerm(et);
	}
}

/**
To add an error term of the form

	e = Int E(t)^T W(t) E(t) dt

to a problem use this function when E(t) is implemented as an ErrorTerm. If E is an expression use the next function.

The ErrorTermFactory functor must have an

	ErrorTerm* operator()(TTime t, double factor) const
or
	boost::shared_ptr<ErrorTerm> operator()(TTime t, double factor) const

member that returns E(t) with its invR = W * factor.
So don't forget to take the square root of factor when you use the setSqrtInvR method.

When a shared_ptr is returned the problemOwnsErrorTerms flag is ignored. Otherwise true makes the problem deleting the error terms in its destructor.
 */
template <typename Algorithm = DefaultAlgorithm, typename TTime, typename ErrorTermFactory, typename ErrorTermReceiver>
void addQuadraticIntegralErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ErrorTermFactory & errorTermFactory, bool problemOwnsErrorTerms = true)
{
	if(a == b) return;

	auto integrator = Algorithm().template getIntegrator<double>(a, b, numberOfPoints);
	SM_ASSERT_TRUE(std::runtime_error, !integrator.isAtEnd(), "too few integration points given : " << numberOfPoints);

	const double commonFactor = integrator.getCommonFactor();
	for(; !integrator.isAtEnd(); integrator.next()){
		double valueFactor = integrator.getValueFactor();
		internal::addErrorTermToProblem(problem, errorTermFactory(integrator.getIntegrationScalar(), commonFactor * valueFactor), problemOwnsErrorTerms);
	}
}

template <typename TTime, typename ErrorTermFactory, typename ErrorTermReceiver>
void addQuadraticIntegralErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ErrorTermFactory & errorTermFactory, bool problemOwnsErrorTerms = true){
	addQuadraticIntegralErrorTerms(problem, a, b, numberOfPoints, errorTermFactory, problemOwnsErrorTerms);
}

/**
To add an error term of the form

	e = \Int E(t)^T sqrtInvR^T sqrtInvR E(t) dt

to a problem use this function when E(t) is implemented as an VectorExpression.

The ExpressionFactory functor must have an

	VectorExpression<D> operator()(TTime t) const
or
	GenericMatrixExpression<D, 1> operator()(TTime t) const

member that returns E(t).
 */
template <typename Algorithm, typename TTime, typename ExpressionFactory, typename DerivedMatrix, typename ErrorTermReceiver>
void addQuadraticIntegralExpressionErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ExpressionFactory & expressionFactory, const Eigen::MatrixBase<DerivedMatrix> & sqrtInvR)
{
	addQuadraticIntegralErrorTerms<Algorithm>(
			problem, a, b, numberOfPoints,
			[&expressionFactory, &sqrtInvR](TTime t, double f){
				return toErrorTermSqrt(expressionFactory(t), sqrtInvR * sqrt(f));
			}
		);
}

// to specify a default for the algorithm,
template <typename TTime, typename ExpressionFactory, typename DerivedMatrix, typename ErrorTermReceiver>
void addQuadraticIntegralExpressionErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ExpressionFactory & expressionFactory, const Eigen::MatrixBase<DerivedMatrix> & sqrtInvR){
	addQuadraticIntegralExpressionErrorTerms<DefaultAlgorithm>(problem, a, b, numberOfPoints, expressionFactory, sqrtInvR);
}

}
}
}

#endif /* QINTEGRALERRORTERM_HPP_ */


/* Copyright (C) 2017 IBM Corp.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/*********************************************************************
vec_l: A module for handling vectors of type long
**********************************************************************/
#include <NTL/matrix.h>
#include "mat_l.h"
#include "vec_l.h"

#include <NTL/new.h>
#include <NTL/vec_long.h>

NTL_CLIENT

//x = a-b
void sub(vec_l& x, const vec_l& a, const vec_l& b)
{
    long n = a.length();
    if (b.length() != n) LogicError("vector sub: dimension mismatch");
    x.SetLength(n);

    for (long i = 0; i < n; i++)
        x[i] = a[i] - b[i];

}

//res = x*a, naive implementation

void mul(long& res, const vec_l& x, const vec_l& a)
{
    long val;
    long n = a.length();
    long i;
    val = 0;
    for (i = 0; i < n; i++)
    {
        val += a.at(i)*x.at(i);
    }
    res = val;
}

//res = a-b
vec_l operator-(const vec_l& a, const vec_l& b)
{
    vec_l res;
    sub(res, a, b);
    return res;
    //NTL_OPT_RETURN(vec_l, res);
}

//res = a-b
vec_l operator-(const vec_l& a, const long& b)
{
    vec_l res;

    int n = a.length();
    int i;
    res.SetLength(n);
    for (i = 0; i< n; i++)
        //res.put(i,(a.get(i) - b));
        res[i] = a[i] - b;
    return res;
    //NTL_OPT_RETURN(vec_l, res);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SUBS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SUBS_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/subs.hpp>
#include <boost/simd/include/functions/scalar/saturate.hpp>
#include <boost/simd/include/functions/scalar/adds.hpp>
#include <boost/simd/include/functions/scalar/is_greater.hpp>
#include <boost/simd/include/functions/scalar/is_equal.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is integer_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)
                            , (scalar_< integer_<A0> >)(scalar_< integer_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;
      return static_cast<result_type>(boost::simd::saturate<A0>(utype(a0)-utype(a1)));
    }
  };
//   BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
//                                      , (A0)
//                                      , (scalar_<int8_<A0> >)(scalar_<int8_<A0> >)
//                                      )
//   {
//     typedef A0 result_type;
//     BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
//     {
//       typedef typename dispatch::meta::strip<A0>::type sA0;
//       typedef typename dispatch::meta::upgrade<A0>::type utype;
//       return static_cast<result_type>(boost::simd::saturate<int8_t>(utype(a0)-utype(a1)));
//       // TO DO why this specialization is needed and saturate<int8_t> cannot be replaced by saturate<A0> ?
//       // this replacement implies an error in isulpequal
//     }
//   };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is unsigned_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)
                            , (scalar_< unsigned_<A0> >)(scalar_< unsigned_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (gt(a0, a1))
        return a0-a1;
      else
        return Zero<A0>();
    }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)
                            , (scalar_< int64_<A0> >)(scalar_< int64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (eq(a1, Valmin<A0>()))
      {
        if (a0 >= Mone<A0>()) return Valmax<A0>();
          return adds(adds(a0, Valmax<A0>()),One<A0>());
      }
      return boost::simd::adds(a0, -a1);
    }
  };
} } }

#endif


// standard includes and external libraries
#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <ctime>
// project includes
#include "abstractclasses.h"
#include "bandit.h"
#include "oful.h"
#include "finitelinrep.h"
#include "utils.h"
#include "gzip.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

size_t PREC = 4;   // for saving numbers are rounded to PREC decimals
size_t EVERY = 1;  // save EVERY round

int main()
{

    int n_derank = 0;
    int low_rank = 15;

    const char *files[7] = { "../../problem_data/jester/33/1/jester_post_d33_span33.npz", "../../problem_data/jester/33/1/jester_post_d26_span26.npz", "../../problem_data/jester/33/1/jester_post_d24_span24.npz", "../../problem_data/jester/33/1/jester_post_d23_span23.npz", "../../problem_data/jester/33/1/jester_post_d20_span20.npz", "../../problem_data/jester/33/1/jester_post_d17_span17.npz", "../../problem_data/jester/33/1/jester_post_d16_span16.npz" };

    const char *names[9] = {"d=33", "d=26", "d=24", "d=23", "d=20", "d=17", "d=16", "d=16 (derank)", "d=17 (derank)"};

    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    int seed = time(NULL);
    srand (seed);
    cout << "seed: " << seed << endl;
    int n_runs = 50, T = 1000000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 1.0;
    double bonus_scale = 1.;
    bool adaptive_ci = true;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    for (int j = 0; j < 7 + n_derank; j++) {

    // load reference representation

    auto start = TIC();
    // FiniteLinearRepresentation reference_rep=flr_loadjson("jester_post_d33_span33.json", noise_std, seed);
    FiniteLinearRepresentation reference_rep=flr_loadnpz("../../problem_data/jester/33/1/jester_post_d33_span33.npz", noise_std, seed,"features", "theta");
    auto tottime = TOC(start);
    int reference_rep_dim = reference_rep.features_dim();
    cout << "Loaded in " << tottime << endl;
    cout << "Ref_rep.dim: " << reference_rep_dim << endl;

    // load representation for OFUL
    start = TIC();
    // FiniteLinearRepresentation oful_rep = flr_loadjson("A.json", noise_std, seed);

    int l = j;
    if(j >= 7) {
	l = 7 + 6 - j;
    }
    FiniteLinearRepresentation oful_rep = flr_loadnpz(files[l], noise_std, seed, "features", "theta");

    if(j >= 7) {
        oful_rep = derank_hls(oful_rep, low_rank, false, true, true);
    }
    //oful_rep.normalize_features(10);

    tottime = TOC(start);
    int oful_rep_dim = oful_rep.features_dim();
    cout << "Loaded in " << tottime << endl;
    cout << "OFUL_rep.dim: " << oful_rep_dim << endl;

    cout << "Equal? " << reference_rep.is_equal(oful_rep, 1e-3) << endl;

    //just OFUL
    vec2double regrets, pseudo_regrets;

    #pragma omp parallel for
    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<int> localg(oful_rep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        // create same representation but with different seed
        FiniteLinearRepresentation lrep = reference_rep.copy(seeds[i]);
        ContBanditProblem<int> prb(lrep, localg);
        prb.reset();
        auto start = TIC();
        prb.run(T);
        auto tottime = TOC(start);
        cout << "time(" << i << "): " << tottime << endl;

        // store regret and pseudo regret
        regrets.push_back(prb.instant_regret);
        pseudo_regrets.push_back(prb.exp_instant_regret);

        // save in compressed json
        save_vector_csv_gzip(regrets, "OFUL-"+std::string(names[j])+"_regrets.csv.gz", EVERY, PREC);
        save_vector_csv_gzip(pseudo_regrets, "OFUL-"+std::string(names[j])+"_pseudoregrets.csv.gz", EVERY, PREC);
    }
    }

    return 0;
}


#define EIGEN_USE_BLAS
#define EIGEN_USE_LAPACKE

#include <pybind11/stl.h>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

/*
 ndarrayはメモリ配置がデフォルトではC言語で主に用いられるRowMajorなのに対し、
 EigenではデフォルトはfortranのColumnMajorである。
 ただ、両者とも指定することができるのでここではndarrayのRowMajorに揃える。
 毎回RowMajorを宣言するのも面倒なのでRMatrixというテンプレートクラスを作っておく。

 Reference: https://qiita.com/lucidfrontier45/items/5048ef74fbf32eeb9f08
 */

template <typename T>
using RMatrix = Eigen::Matrix<T, -1, -1, Eigen::RowMajor>;

template <typename T>
T add(T x, T y) {
    return x + y;
}

template <typename T>
RMatrix<T> multiply_array(RMatrix<T> m, T value) {
    return m * value;
}

template <typename T>
Eigen::Vector<T, -1> create_vector(int size) {
    Eigen::VectorXd vector = Eigen::VectorXd::Zero(size);

    return vector;
}

template <typename T>
RMatrix<T> create_matrix(int rows, int columns) {
    Eigen::MatrixXd matrix = Eigen::MatrixXd::Zero(rows, columns);
    return matrix;
}

template <typename T>
RMatrix<T> matmul(RMatrix<T> a, RMatrix<T> b) {
    auto result = a * b;
    return result;
}

namespace py = pybind11;

PYBIND11_MODULE(_C, m) {
    m.def("add", &add<int>, "Add two numbers");
    m.def("add", &add<double>, "Add two numbers");

    m.def("multiply_array", &multiply_array<float>, "");
    m.def("create_vector", &create_vector<double>, "");
    m.def("create_matrix", &create_matrix<double>, "");
    m.def("matmul", &matmul<float>,
          "The function of matrix multiplication. \
          This function using BLAS. \
          If you use Apple Silicon Mac, \
          you will indirectly use Apple AMX Coprocessor,\
          since it uses BLAS in Accelerate.");

    m.def("matmul", &matmul<double>,
          "The function of matrix multiplication. \
          This function using BLAS. \
          If you use Apple Silicon Mac, \
          you will indirectly use Apple AMX Coprocessor,\
          since it uses BLAS in Accelerate.");

}