//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NEXT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NEXT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/bitfloating.hpp>
#include <boost/simd/function/simd/bitinteger.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/inc.hpp>
#include <boost/simd/function/simd/is_equal.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(next_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return inc(a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD(next_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_else(is_equal(a0, Inf<A0>()), a0,  bitfloating(inc(bitinteger(a0))));
      }
   };

} } }
#endif


#pragma once
#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
#include <vector>
TRAJOPT_IGNORE_WARNINGS_POP

namespace util
{
inline std::vector<double> toDblVec(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& x)
{
  return std::vector<double>(x.data(), x.data() + x.size());
}
inline Eigen::VectorXd toVectorXd(const std::vector<double>& x)
{
  return Eigen::Map<const Eigen::VectorXd>(x.data(), static_cast<long int>(x.size()));
}
}


#ifndef OBJECT_HPP
#define	OBJECT_HPP
#include <armadillo>

#include "Ambient.hpp"
#include "Light.hpp"
#include "Hittable.hpp"

using namespace arma;
using namespace std;

class Object :public Hittable {

private:
    vec kd, ks, normal;
    mat transf;
    double p;
    int tipo;

public:

    Object();

    void setTipo(int tipo) {
        this->tipo = tipo;
    }

    int getTipo() const {
        return tipo;
    }

    void setP(double p) {
        this->p = p;
    }

    double getP() const {
        return p;
    }

    void setTransf(mat transf) {
        this->transf = transf;
    }

    mat getTransf() const {
        return transf;
    }

    void setNormal(vec normal) {
        this->normal = normal;
    }

    vec getNormal() const {
        return normal;
    }

    void setKs(vec ks) {
        this->ks = ks;
    }

    vec getKs() const {
        return ks;
    }

    void setKd(vec kd) {
        this->kd = kd;
    }

    vec getKd() const {
        return kd;
    }

    virtual bool colide(const vec &d, double &T2, const vec &origem){
        return false;
    }

    virtual vec getA(){
        return 0;
    }

    virtual vec getB(){
        return 0;
    }

    virtual vec getC(){
        return 0;
    }

    vec cor(const vec &d, const vector<Light> &luz, const vector<Object *> objetos, const Ambient ambiente, const vec &vetor, const unsigned int &indice);

};

#endif	/* OBJECT_HPP */


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)

BOOST_AUTO_TEST_CASE(vsRX)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Vector3 axis;
  axis << 1.0, 0.0, 0.0;

  Model modelRX, modelRevoluteUnaligned;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelRevoluteUnaligned joint_model_RU(axis);

  addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia);
  addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia);

  Data dataRX(modelRX);
  Data dataRevoluteUnaligned(modelRevoluteUnaligned);

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
  Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd aRevoluteUnaligned(aRX);

  forwardKinematics(modelRX, dataRX, q, v);
  forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);

  computeAllTerms(modelRX, dataRX, q, v);
  computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);

  BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1]));
  BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1]));
  BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
  BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector()));

  BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
  BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));

  // InverseDynamics == rnea
  tauRX = rnea(modelRX, dataRX, q, v, aRX);
  tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);

  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaRX = aba(modelRX,dataRX, q, v, tauRX);
  Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUnaligned,dataRevoluteUnaligned, q, v, tauRevoluteUnaligned);

  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));

  // CRBA
  crba(modelRX, dataRX,q);
  crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q);

  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnaligned.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRX;jacobianRX.resize(6,1); jacobianRX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero();
  computeJointJacobians(modelRX, dataRX, q);
  computeJointJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianRX);
  getJointJacobian(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, LOCAL, jacobianRevoluteUnaligned);

  BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointRevoluteUnboundedUnaligned)

  BOOST_AUTO_TEST_CASE(vsRUX)
  {
    using namespace pinocchio;
    typedef SE3::Vector3 Vector3;
    typedef SE3::Matrix3 Matrix3;

    Vector3 axis;
    axis << 1.0, 0.0, 0.0;

    Model modelRUX, modelRevoluteUboundedUnaligned;

    Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
    SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

    JointModelRevoluteUnboundedUnaligned joint_model_RUU(axis);
    typedef traits< JointRevoluteUnboundedUnalignedTpl<double> >::TangentVector_t TangentVector;

    addJointAndBody(modelRUX,JointModelRUBX(),0,pos,"rux",inertia);
    addJointAndBody(modelRevoluteUboundedUnaligned,joint_model_RUU,0,pos,"revolute-unbounded-unaligned",inertia);

    Data dataRUX(modelRUX);
    Data dataRevoluteUnboundedUnaligned(modelRevoluteUboundedUnaligned);

    Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRUX.nq);
    TangentVector v = TangentVector::Ones (modelRUX.nv);
    Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRUX.nv);
    Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUboundedUnaligned.nv);
    Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRUX.nv);
    Eigen::VectorXd aRevoluteUnaligned(aRX);

    forwardKinematics(modelRUX, dataRUX, q, v);
    forwardKinematics(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);

    computeAllTerms(modelRUX, dataRUX, q, v);
    computeAllTerms(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);

    BOOST_CHECK(dataRevoluteUnboundedUnaligned.oMi[1].isApprox(dataRUX.oMi[1]));
    BOOST_CHECK(dataRevoluteUnboundedUnaligned.liMi[1].isApprox(dataRUX.liMi[1]));
    BOOST_CHECK(dataRevoluteUnboundedUnaligned.Ycrb[1].matrix().isApprox(dataRUX.Ycrb[1].matrix()));
    BOOST_CHECK(dataRevoluteUnboundedUnaligned.f[1].toVector().isApprox(dataRUX.f[1].toVector()));

    BOOST_CHECK(dataRevoluteUnboundedUnaligned.nle.isApprox(dataRUX.nle));
    BOOST_CHECK(dataRevoluteUnboundedUnaligned.com[0].isApprox(dataRUX.com[0]));

    // InverseDynamics == rnea
    tauRX = rnea(modelRUX, dataRUX, q, v, aRX);
    tauRevoluteUnaligned = rnea(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v, aRevoluteUnaligned);

    BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));

    // ForwardDynamics == aba
    Eigen::VectorXd aAbaRX = aba(modelRUX, dataRUX, q, v, tauRX);
    Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUboundedUnaligned,dataRevoluteUnboundedUnaligned, q, v, tauRevoluteUnaligned);

    BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));

    // CRBA
    crba(modelRUX, dataRUX,q);
    crba(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q);

    BOOST_CHECK(dataRUX.M.isApprox(dataRevoluteUnboundedUnaligned.M));

    // Jacobian
    Data::Matrix6x jacobianRUX;jacobianRUX.resize(6,1); jacobianRUX.setZero();
    Data::Matrix6x jacobianRevoluteUnboundedUnaligned;
    jacobianRevoluteUnboundedUnaligned.resize(6,1); jacobianRevoluteUnboundedUnaligned.setZero();

    computeJointJacobians(modelRUX, dataRUX, q);
    computeJointJacobians(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q);
    getJointJacobian(modelRUX, dataRUX, 1, LOCAL, jacobianRUX);
    getJointJacobian(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, 1, LOCAL, jacobianRevoluteUnboundedUnaligned);

    BOOST_CHECK(jacobianRUX.isApprox(jacobianRevoluteUnboundedUnaligned));
  }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointRevoluteUnbounded)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionRevoluteTpl<double,0,0> mp_x(2.);
    Motion mp_dense_x(mp_x);

    BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
    BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

    BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

    MotionRevoluteTpl<double,0,1> mp_y(2.);
    Motion mp_dense_y(mp_y);

    BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
    BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

    BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

    MotionRevoluteTpl<double,0,2> mp_z(2.);
    Motion mp_dense_z(mp_z);

    BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
    BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

    BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
  }

BOOST_AUTO_TEST_CASE(vsRX)
{
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model modelRX, modelRevoluteUnbounded;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelRUBX joint_model_RUX;
  addJointAndBody(modelRX,JointModelRX(),0,SE3::Identity(),"rx",inertia);
  addJointAndBody(modelRevoluteUnbounded,joint_model_RUX,0,SE3::Identity(),"revolute unbounded x",inertia);

  Data dataRX(modelRX);
  Data dataRevoluteUnbounded(modelRevoluteUnbounded);

  Eigen::VectorXd q_rx = Eigen::VectorXd::Ones(modelRX.nq);
  Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nq);
  double ca, sa; double alpha = q_rx(0); SINCOS(alpha, &sa, &ca);
  q_rubx(0) = ca;
  q_rubx(1) = sa;
  Eigen::VectorXd v_rx = Eigen::VectorXd::Ones(modelRX.nv);
  Eigen::VectorXd v_rubx = v_rx;
  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRX.nv);
  Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nv);
  Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRX.nv);
  Eigen::VectorXd aRevoluteUnbounded = aRX;

  forwardKinematics(modelRX, dataRX, q_rx, v_rx);
  forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);

  computeAllTerms(modelRX, dataRX, q_rx, v_rx);
  computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);

  BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1]));
  BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1]));
  BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
  BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector()));

  BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle));
  BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0]));

  // InverseDynamics == rnea
  tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX);
  tauRevoluteUnbounded = rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded);

  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaRX= aba(modelRX,dataRX, q_rx, v_rx, tauRX);
  Eigen::VectorXd aAbaRevoluteUnbounded = aba(modelRevoluteUnbounded,dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded);

  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded));

  // crba
  crba(modelRX, dataRX,q_rx);
  crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);

  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
  computeJointJacobians(modelRX, dataRX, q_rx);
  computeJointJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianPX);
  getJointJacobian(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, LOCAL, jacobianPrismaticUnaligned);

  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));

}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointRevolute)

BOOST_AUTO_TEST_CASE(spatial)
{
  typedef TransformRevoluteTpl<double,0,0> TransformX;
  typedef TransformRevoluteTpl<double,0,1> TransformY;
  typedef TransformRevoluteTpl<double,0,2> TransformZ;

  typedef SE3::Vector3 Vector3;

  const double alpha = 0.2;
  double sin_alpha, cos_alpha; SINCOS(alpha,&sin_alpha,&cos_alpha);
  SE3 Mplain, Mrand(SE3::Random());

  TransformX Mx(sin_alpha,cos_alpha);
  Mplain = Mx;
  BOOST_CHECK(Mplain.translation().isZero());
  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitX()).toRotationMatrix()));
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));

  TransformY My(sin_alpha,cos_alpha);
  Mplain = My;
  BOOST_CHECK(Mplain.translation().isZero());
  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitY()).toRotationMatrix()));
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));

  TransformZ Mz(sin_alpha,cos_alpha);
  Mplain = Mz;
  BOOST_CHECK(Mplain.translation().isZero());
  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitZ()).toRotationMatrix()));
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));

  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionRevoluteTpl<double,0,0> mp_x(2.);
  Motion mp_dense_x(mp_x);

  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

  MotionRevoluteTpl<double,0,1> mp_y(2.);
  Motion mp_dense_y(mp_y);

  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

  MotionRevoluteTpl<double,0,2> mp_z(2.);
  Motion mp_dense_z(mp_z);

  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionRevoluteUnaligned mp(MotionRevoluteUnaligned::Vector3(1.,2.,3.),6.);
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_SUITE_END()


// Original work Copyright (c) 2016, University of Minnesota
// Modified work Copyright 2019, Yue Peng
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// 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.
// THIS SOFTWARE IS PROVIDED "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 UNIVERSITY OF MINNESOTA, DULUTH 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.

#ifndef ADMM_PASSIVECOLLISION_HPP
#define ADMM_PASSIVECOLLISION_HPP

#include <Eigen/Dense>
#include "Collider.hpp"
#include "MCL/TetMesh.hpp"
#include "MCL/TriangleMesh.hpp"
#include "MCL/BVH.hpp"

namespace admm {

class Floor : public PassiveCollision {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;

	double m_y;
	Floor( double y ) : m_y(y) {}
	void signed_distance( const Vec3 &x, Payload &p ) const {
		double dx = ( x[1] - m_y );
		if( dx > p.dx ){ return; }
		p.dx = dx;
		p.point = Vec3(x[0],m_y,x[2]);
		p.normal = Vec3(0,1,0);
	}
};

class Sphere : public PassiveCollision {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;

	Vec3 center;
	double rad;
	Sphere( const Vec3 &c, double r ) : center(c), rad(r) {}
	void signed_distance( const Vec3 &x, Payload &p ) const {
		Vec3 dir = x - center;
		double dx = dir.norm() - rad;
		if( dx > p.dx ){ return; }
		dir.normalize();
		p.dx = dx;
		p.point = center + dir*rad;
		p.normal = dir;
	}
};

class PassiveMesh : public PassiveCollision {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;
	mcl::bvh::AABBTree<float,3> tri_tree;
	mcl::bvh::AABBTree<float,4> tet_tree;

	std::shared_ptr<mcl::TetMesh> mesh;
	PassiveMesh( std::shared_ptr<mcl::TetMesh> mesh_ ) : mesh(mesh_){ update_bvh(); }

	void update_bvh(){
		mesh->need_faces();
		tri_tree.init( &mesh->faces[0][0], &mesh->vertices[0][0], mesh->faces.size() );
		tet_tree.init( &mesh->tets[0][0], &mesh->vertices[0][0], mesh->tets.size() );
	}

	void signed_distance( const Vec3 &x, Payload &p ) const {

		// First, check if objet is inside mesh
		mcl::bvh::PointInTet<float> p_in_mesh( x.cast<float>(), &mesh->vertices[0][0], &mesh->tets[0][0] );
		bool hit = tet_tree.traverse( p_in_mesh );

		// If there is an odd number of intersections, we are inside the mesh
		if( hit ){
			mcl::bvh::NearestTriangle<float> nearest_tri( x.cast<float>(), &mesh->vertices[0][0], &mesh->faces[0][0] );
			tri_tree.traverse( nearest_tri );

			mcl::Vec3i hit_face = mesh->faces[ nearest_tri.hit_tri ];
			const mcl::Vec3f &p0 = mesh->vertices[hit_face[0]];
			const mcl::Vec3f &p1 = mesh->vertices[hit_face[1]];
			const mcl::Vec3f &p2 = mesh->vertices[hit_face[2]];
			mcl::Vec3f norm = (p1-p0).cross(p2-p0);
			norm.normalize();

			// Set the payload data
			p.dx = -1.0*(nearest_tri.proj.cast<double>()-x).norm();
			p.point = nearest_tri.proj.cast<double>();
			p.normal = norm.cast<double>();
		}

	} // end signed distance
};

/*
class PassiveMesh : public PassiveCollision {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;
	mcl::bvh::AABBTree<float,3> m_tree;

	std::shared_ptr<mcl::TriangleMesh> mesh;
	PassiveMesh( std::shared_ptr<mcl::TriangleMesh> mesh_ ) : mesh(mesh_){ update_bvh(); }

	void update_bvh(){
		m_tree.init( &mesh->faces[0][0], &mesh->vertices[0][0], mesh->faces.size() );
	}

	void signed_distance( const Vec3 &x, Payload &p ) const {

		// First, check if objet is inside mesh
		mcl::bvh::RayMultiHit<float> p_in_mesh( x.cast<float>(), &mesh->vertices[0][0], &mesh->faces[0][0] );
		m_tree.traverse( p_in_mesh );

		// If there is an odd number of intersections, we are inside the mesh
		if( p_in_mesh.hit_count % 2 == 1 ){
			mcl::bvh::NearestTriangle<float> nearest_tri( x.cast<float>(), &mesh->vertices[0][0], &mesh->faces[0][0] );
			m_tree.traverse( nearest_tri );

			mcl::Vec3i hit_face = mesh->faces[ nearest_tri.hit_tri ];
			const mcl::Vec3f &p0 = mesh->vertices[hit_face[0]];
			const mcl::Vec3f &p1 = mesh->vertices[hit_face[1]];
			const mcl::Vec3f &p2 = mesh->vertices[hit_face[2]];
			mcl::Vec3f norm = (p1-p0).cross(p2-p0);
			norm.normalize();

			// Set the payload data
			p.dx = -1.0*(nearest_tri.proj.cast<double>()-x).norm();
			p.point = nearest_tri.proj.cast<double>();
			p.normal = norm.cast<double>();
		}

	} // end signed distance
};
*/

} // end of namespace admm

#endif


//==============================================================================
//         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 NT2_CORE_FUNCTIONS_DELTAC_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_DELTAC_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/core/container/dsl/generative.hpp>
#include <nt2/sdk/meta/generative_hierarchy.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>

/*
In mathematics, the Kronecker delta or Kronecker's delta, named after Leopold Kronecker, is a function of two variables, usually integers.
The function is 1 if the variables are equal, and 0 otherwise:
\delta_{ij} = \begin{cases}
0 &\text{if } i \neq j   \\
1 &\text{if } i=j,   \end{cases}
*/

namespace nt2
{
  namespace tag
  {
    /*!
      @brief deltac generic tag

      Represents the deltac function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct deltac_ : ext::state_constant_<deltac_>
    {
      /// @brief Parent hierarchy
      typedef ext::state_constant_<deltac_> parent;

      /// @brief default value type for untyped calls
      typedef double                      default_type;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_deltac_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::deltac_, Site> dispatching_deltac_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::deltac_, Site>();
    }
    template<class... Args>
    struct impl_deltac_;
  }

  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::deltac_, deltac, 3)
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::deltac_, deltac, 4)
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::deltac_, deltac, 5)
}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr, int N>
  struct  value_type<tag::deltac_,Domain,N,Expr>
        : meta::generative_value<Expr>
  {};

  template<class Domain, class Expr, int N>
  struct  size_of<tag::deltac_,Domain,N,Expr>
        : meta::generative_size<Expr>
  {};
} }

#endif


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file rotr.hpp
 *
 * This header defines \c rotr algorithm, which rotates integer right.
 */

#ifndef BOOST_BIT_OPS_ROTATING_ROTR_HPP_INCLUDED_
#define BOOST_BIT_OPS_ROTATING_ROTR_HPP_INCLUDED_

#include <limits>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/integral_constant.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>

#if defined(_MSC_VER)
#include <intrin.h>
#endif

namespace boost {
namespace bit_ops {

namespace detail {

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR8)

template< typename T >
inline T rotr(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 1u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotr8(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR8)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR16)

template< typename T >
inline T rotr(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 2u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotr16(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR16)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR)

template< typename T >
inline T rotr(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 4u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotr(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR64)

template< typename T >
inline T rotr(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 8u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotr64(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTR64)

template< typename T, unsigned int N >
inline T rotr(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< T >((value >> count) | (value << (std::numeric_limits< T >::digits - count)));
}

} // namespace detail

//! Returns \a value rotated left by \a count bits
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type rotr(T value, unsigned int count) BOOST_NOEXCEPT
{
    return bit_ops::detail::rotr(value, count, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_ROTATING_ROTR_HPP_INCLUDED_


#pragma once

#include <vector>
#include <fstream>
#include <array>
#include <Eigen/Dense>

namespace Discregrid
{

class DiscreteGrid
{
public:

	using CoefficientVector = Eigen::Matrix<double, 32, 1>;
	using ContinuousFunction = std::function<double(Eigen::Vector3d const&)>;
	using MultiIndex = std::array<unsigned int, 3>;
	using Predicate = std::function<bool(Eigen::Vector3d const&, double)>;
	using SamplePredicate = std::function<bool(Eigen::Vector3d const&)>;

	DiscreteGrid() = default;
	DiscreteGrid(Eigen::AlignedBox3d const& domain, std::array<unsigned int, 3> const& resolution)
		: m_domain(domain), m_resolution(resolution), m_n_fields(0u)
	{
		auto n = Eigen::Matrix<unsigned int, 3, 1>::Map(resolution.data());
		m_cell_size = domain.diagonal().cwiseQuotient(n.cast<double>());
		m_inv_cell_size = m_cell_size.cwiseInverse();
		m_n_cells = n.prod();
	}
	virtual ~DiscreteGrid() = default;

	virtual void save(std::string const& filename) const = 0;
	virtual void load(std::string const& filename) = 0;

	virtual unsigned int addFunction(ContinuousFunction const& func, bool verbose = false,
		SamplePredicate const& pred = nullptr) = 0;

	double interpolate(Eigen::Vector3d const& xi, Eigen::Vector3d* gradient = nullptr) const
	{
		return interpolate(0u, xi, gradient);
	}

	virtual double interpolate(unsigned int field_id, Eigen::Vector3d const& xi,
		Eigen::Vector3d* gradient = nullptr) const = 0;

	/**
	 * @brief Determines the shape functions for the discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param x Location where the shape functions should be determined
	 * @param cell cell of x
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of x
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return Success of the function.
	 */
	virtual bool determineShapeFunctions(unsigned int field_id, Eigen::Vector3d const &x,
		std::array<unsigned int, 32> &cell, Eigen::Vector3d &c0, Eigen::Matrix<double, 32, 1> &N,
		Eigen::Matrix<double, 32, 3> *dN = nullptr) const = 0;

	/**
	 * @brief Evaluates the given discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param xi Location where the discrete function is evaluated
	 * @param cell cell of xi
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of xi
	 * @param gradient (Optional) if a pointer to a vector is passed the gradient of the discrete function will be evaluated
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return double Results of the evaluation of the discrete function at point xi
	 */
	virtual double interpolate(unsigned int field_id, Eigen::Vector3d const& xi, const std::array<unsigned int, 32> &cell, const Eigen::Vector3d &c0, const Eigen::Matrix<double, 32, 1> &N,
		Eigen::Vector3d* gradient = nullptr, Eigen::Matrix<double, 32, 3> *dN = nullptr) const = 0;

	virtual void reduceField(unsigned int field_id, Predicate pred) {}

	MultiIndex singleToMultiIndex(unsigned int i) const;
	unsigned int multiToSingleIndex(MultiIndex const& ijk) const;

	Eigen::AlignedBox3d subdomain(MultiIndex const& ijk) const;
	Eigen::AlignedBox3d subdomain(unsigned int l) const;

	Eigen::AlignedBox3d const& domain() const { return m_domain; }
	std::array<unsigned int, 3> const& resolution() const { return m_resolution; };
	Eigen::Vector3d const& cellSize() const { return m_cell_size;}
	Eigen::Vector3d const& invCellSize() const { return m_inv_cell_size;}

protected:

	Eigen::AlignedBox3d m_domain;
	std::array<unsigned int, 3> m_resolution;
	Eigen::Vector3d m_cell_size;
	Eigen::Vector3d m_inv_cell_size;
	std::size_t m_n_cells;
	std::size_t m_n_fields;
};
}


#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "SwarmPlannerGeometric.hpp"
#include "BoxGeneratorGeometric.hpp"
#include "InitTrajPlanner.hpp"
#include <mission.hpp>
#include <param.hpp>

#define _USE_MATH_DEFINES
#include <cmath>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

class TrajPlotter {
public:
    TrajPlotter(std::shared_ptr<SwarmPlanner> _swarm_planner,
                std::shared_ptr<BoxGenerator> _box_generator,
                std::shared_ptr<InitTrajPlanner> _init_traj_planner,
                SwarmPlanning::Mission _mission,
                SwarmPlanning::Param _param)
            : swarm_planner(_swarm_planner),
              box_generator(_box_generator),
              init_traj_planner(_init_traj_planner),
              mission(_mission),
              param(_param) {
        qn = mission.qn;
        M = init_traj_planner->ts_total.size();
        ts_total = init_traj_planner->ts_total;
        outdim = 3;
        outdim_pva = 3 * outdim;

        double dt = 0.1;
        t.resize(floor(ts_total.back() / dt));
        for (int i = 0; i < t.size(); i++) {
            t[i] = i * dt;
        }

        coef.resize(qn);
        quad_state.resize(qn);
        for (int qi = 0; qi < qn; qi++) {
            float rows = swarm_planner->msgs_traj_coef[qi].layout.dim.at(0).size;
            float cols = swarm_planner->msgs_traj_coef[qi].layout.dim.at(1).size;
            std::vector<double> data = swarm_planner->msgs_traj_coef[qi].data;
            coef[qi] = Eigen::Map<Eigen::MatrixXd>(data.data(), rows, cols);

            quad_state[qi].resize(outdim_pva);
            for (int i = 0; i < outdim_pva; i++) {
                quad_state[qi][i].resize(t.size());
            }
        }
    }

    void plot(bool log) {
        update_quad_state();
        update_distance_between_agents();
        if(log){
            plot_quad_dynamics();
            plot_distance_between_agents();
        }
        ROS_INFO_STREAM("Global min_dist between agents: " << global_min_dist);
        ROS_INFO_STREAM("Total flight distance: " << trajectory_length_sum());
    }

private:
    std::shared_ptr<SwarmPlanner> swarm_planner;
    std::shared_ptr<BoxGenerator> box_generator;
    std::shared_ptr<InitTrajPlanner> init_traj_planner;
    SwarmPlanning::Mission mission;
    SwarmPlanning::Param param;

    int qn, M, outdim, outdim_pva;
    double global_min_dist;

    std::vector<double> ts_total, t, max_dist, min_dist;
    std::vector<Eigen::MatrixXd> coef;
    std::vector<std::vector<std::vector<double>>> quad_state;

    void timeMatrix(double current_time, int &index, Eigen::MatrixXd &polyder) {
        double tseg = 0;
        double tcand;

        // find segment start time tseg
        for (int m = 0; m < M; m++) {
            tcand = init_traj_planner->ts_total[m];
            if (tcand < current_time) {
                tseg = tcand;
                index = m;
            } else {
                break;
            }
        }
        tseg = current_time - tseg;
        polyder.resize(3, param.N_poly + 1);
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < param.N_poly + 1; j++) {
                if (i <= j)
                    polyder(i, param.N_poly - j) =
                            ((i == 0) * 1 + (i == 1) * j + (i == 2) * j * (j - 1)) * pow(tseg, j - i);
                else
                    polyder(i, param.N_poly - j) = 0;
            }
        }
    }

    void update_quad_state() {
        for (int qi = 0; qi < qn; qi++) {
            for (int j = 0; j < t.size(); j++) {
                int index = 0;
                Eigen::MatrixXd pva, polyder;
                timeMatrix(t[j], index, polyder);
                pva = polyder * coef[qi].block((param.N_poly + 1) * index, 0, (param.N_poly + 1), 3);

                for (int i = 0; i < outdim_pva; i++) {
                    quad_state[qi][i][j] = pva(i / outdim, i % outdim);
                }
            }
        }
    }

    double trajectory_length_sum(){
        double length_sum = 0;
        for(int qi = 0; qi < qn; qi++){
            for (int i = 0; i < t.size() - 1; i++) {
                length_sum += sqrt(pow(quad_state[qi][0][i+1] - quad_state[qi][0][i], 2) +
                                   pow(quad_state[qi][1][i+1] - quad_state[qi][1][i], 2) +
                                   pow(quad_state[qi][2][i+1] - quad_state[qi][2][i], 2));
            }
        }
        return length_sum;
    }

    void plot_quad_dynamics() {
        plt::figure_size(1500, 1000);

        // Plot Quad Velocity
        plt::subplot(3, 2, 1);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][3]);
        }
        plt::title("velocity -x axis");

        plt::subplot(3, 2, 3);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][4]);
        }
        plt::title("velocity -y axis");

        plt::subplot(3, 2, 5);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][5]);
        }
        plt::title("velocity -z axis");

        // Plot Quad Acceleration
        plt::subplot(3, 2, 2);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][6]);
        }
        plt::title("acceleration -x axis");

        plt::subplot(3, 2, 4);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][7]);
        }
        plt::title("acceleration -y axis");

        plt::subplot(3, 2, 6);
        for(int qi = 0; qi < qn; qi++) {
            plt::named_plot("agent" + std::to_string(qi) , t, quad_state[qi][8]);
        }
        plt::title("acceleration -z axis");

        plt::legend();
        plt::show(false);
    }

    void update_distance_between_agents(){
        double max_dist_, min_dist_, dist;
        max_dist.resize(t.size());
        min_dist.resize(t.size());

        global_min_dist = SP_INFINITY;
        for(int i = 0; i < t.size(); i++) {
            max_dist_ = 0;
            min_dist_ = SP_INFINITY;
            for (int qi = 0; qi < qn; qi++) {
                for (int qj = qi + 1; qj < qn; qj++) {
                    dist = sqrt(pow(quad_state[qi][0][i] - quad_state[qj][0][i], 2) +
                                pow(quad_state[qi][1][i] - quad_state[qj][1][i], 2) +
                                pow((quad_state[qi][2][i] - quad_state[qj][2][i]) / param.downwash, 2));

                    if(dist > max_dist_){
                        max_dist_ = dist;
                    }
                    if(dist < min_dist_){
                        min_dist_ = dist;
                    }
                    if(dist < global_min_dist){
                        global_min_dist = dist;
                    }
                }
            }
            max_dist[i] = max_dist_;
            min_dist[i] = min_dist_;
        }
    }

    void plot_distance_between_agents(){
        plt::figure(1);
        plt::figure_size(1500, 1000);

        std::vector<double> collision_dist;
        collision_dist.resize(t.size());
        for(int i = 0; i < t.size(); i++){
            collision_dist[i] = 2 * mission.quad_size[0]; //TODO: heterogeous case
        }

        plt::plot(t, collision_dist);
        plt::plot(t, max_dist);
        plt::plot(t, min_dist);

        plt::title("Ellipsoidal Distance between Quadrotor");

        plt::show(false);
    }
};


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/comparing.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/group.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
namespace hana = boost::hana;

// group without a predicate
BOOST_HANA_CONSTANT_CHECK(
    hana::group(hana::make_tuple(hana::int_c<1>, hana::long_c<1>, hana::type_c<int>, hana::char_c<'x'>, hana::char_c<'x'>))
        == hana::make_tuple(
            hana::make_tuple(hana::int_c<1>, hana::long_c<1>),
            hana::make_tuple(hana::type_c<int>),
            hana::make_tuple(hana::char_c<'x'>, hana::char_c<'x'>)
        )
);

// group with a predicate
constexpr auto tuples = hana::make_tuple(
    hana::range_c<int, 0, 1>,
    hana::range_c<int, 0, 2>,
    hana::range_c<int, 1, 3>,
    hana::range_c<int, 2, 6>
);

BOOST_HANA_CONSTANT_CHECK(
    hana::group(tuples, hana::comparing(hana::length))
        == hana::make_tuple(
            hana::make_tuple(
                hana::range_c<int, 0, 1>
            ),
            hana::make_tuple(
                hana::range_c<int, 0, 2>,
                hana::range_c<int, 1, 3>
            ),
            hana::make_tuple(
                hana::range_c<int, 2, 6>
            )
        )
);

// group.by is syntactic sugar
static_assert(
    hana::group.by(hana::comparing(hana::decltype_),
                   hana::make_tuple(1, 2, 3, 'x', 'y', 4.4, 5.5))
        == hana::make_tuple(
            hana::make_tuple(1, 2, 3),
            hana::make_tuple('x', 'y'),
            hana::make_tuple(4.4, 5.5)
        )
, "");

int main() { }


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of Google
//

#include <boost/numeric/ublas/tensor.hpp>
#include <vector>
#include <iostream>
#include <type_traits>

void instantiate_tensor_dynamic()
{
  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using layout  = ublas::layout::first_order; // storage format
  using tensor  = boost::numeric::ublas::tensor_dynamic<value,layout>;
  constexpr auto ones = ublas::ones<value,layout>{};

  try {
    // tensor is resizable has dynamic dimensions
  	// elements are stored contiguously in memory using the 1st-format (column-major)

    tensor t1  = ones(3,4,2);
    std::cout << "t1 = " << t1 << std::endl;

    tensor t2  = 2 * ones(3,4,2);
    std::cout << "t2 = " << t2 << std::endl;

    tensor t3  = 3*t2 + t1;
    std::cout << "t3 = " << t3 << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the instantiate_tensor_dynamic function of instantiate-tensor." << std::endl;
    throw;
  }
}

void instantiate_tensor_dynamics_with_static_order()
{
  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using layout  = boost::numeric::ublas::layout::first_order; // storage format
  using tensor  = boost::numeric::ublas::tensor_static_rank<value,3u, layout>;
  constexpr auto ones = ublas::ones_static_rank<value,layout>{};

  try {
    // tensor type has static order and dynamic dimensions
    // elements are stored contiguously in memory using the 1st-format (column-major)

    auto t1  = ones(3,4,2);
    std::cout << "t1 = " << t1 << std::endl;

    tensor t2  = 2*ones(3,4,2);
    std::cout << "t2 = " << t2 << std::endl;

    tensor t3  = 3*t2 + t1;
    std::cout << "t3 = " << t3 << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the instantiate_tensor_dynamic function of instantiate-tensor." << std::endl;
    throw;
  }
}

void instantiate_tensor_static()
{

  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using layout  = ublas::layout::first_order; // storage format
  using shape   = ublas::extents<3,4,2>;
  using tensor  = ublas::tensor_static<value,shape,layout>;
  constexpr auto ones = ublas::ones_static<value,layout>{};

  try {
    // tensor type has static order and static dimensions
    // elements are stored contiguously in memory using the 1st-format (column-major)

    auto t1  = tensor{};
    std::cout << "t1 = " << t1 << std::endl;

    tensor t2  = 2 * ones(shape{});
    std::cout << "t2 = " << t2 << std::endl;

    tensor t3  = 3*t2 + t1;
    std::cout << "t3 = " << t3 << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the instantiate_tensor_dynamic function of instantiate-tensor." << std::endl;
    throw;
  }
}

int main()
{
  try{
    instantiate_tensor_dynamic();
    instantiate_tensor_dynamics_with_static_order();
    instantiate_tensor_static();
  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of instantiate-tensor." << std::endl;
  }
}


#ifndef TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_MATRIX_VALUE_HPP
#define TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_MATRIX_VALUE_HPP

#include <stan/math/rev/core/var.hpp>
#include <gtest/gtest.h>
#include <math/rev/mat/vectorize/build_rev_matrix.hpp>
#include <math/rev/mat/vectorize/expect_val_deriv_eq.hpp>
#include <Eigen/Dense>
#include <vector>

template <typename F>
void expect_rev_matrix_value() {
  using stan::math::var;
  using std::vector;
  typedef Eigen::Matrix<var, Eigen::Dynamic, Eigen::Dynamic> MatrixXvar;

  size_t num_cols = 3;
  size_t num_inputs = F::valid_inputs().size();
  MatrixXvar template_m(num_inputs, num_cols);

  for (int i = 0; i < template_m.size(); ++i) {
    MatrixXvar a = build_rev_matrix<F>(template_m);
    MatrixXvar b = build_rev_matrix<F>(template_m);
    MatrixXvar fb = F::template apply<MatrixXvar>(b);
    EXPECT_EQ(b.size(), fb.size());
    expect_val_deriv_eq(F::apply_base(a(i)), a(i), fb(i), b(i));
  }

  size_t vector_matrix_size = 2;
  for (size_t i = 0; i < vector_matrix_size; ++i) {
    for (int j = 0; j < template_m.size(); ++j) {
      vector<MatrixXvar> c;
      vector<MatrixXvar> d;
      for (size_t k = 0; k < vector_matrix_size; ++k) {
        c.push_back(build_rev_matrix<F>(template_m));
        d.push_back(build_rev_matrix<F>(template_m));
      }
      vector<MatrixXvar> fd = F::template apply<vector<MatrixXvar> >(d);
      EXPECT_EQ(d[i].size(), fd[i].size());
      EXPECT_EQ(d[i].rows(), fd[i].rows());
      EXPECT_EQ(d[i].cols(), fd[i].cols());
      expect_val_deriv_eq(F::apply_base(c[i](j)), c[i](j), fd[i](j), d[i](j));
    }
  }

  MatrixXvar e = build_rev_matrix<F>(template_m).block(1, 1, 1, 1);
  MatrixXvar f = build_rev_matrix<F>(template_m);
  MatrixXvar ffb = F::template apply<MatrixXvar>(f.block(1, 1, 1, 1));
  expect_val_deriv_eq(F::apply_base(e(0, 0)), e(0, 0), ffb(0, 0), f(1, 1));
}

#endif


/**
 * @file GeometryIO.hpp
 * @author bwu
 * @brief I/O functions of geometries
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_GEOMETRYIO_HPP
#define GENERIC_GEOMETRY_GEOMETRYIO_HPP
#include <boost/geometry/io/wkt/write.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include "BoostGeometryRegister.hpp"
#include "GeometryTraits.hpp"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>

#ifdef BOOST_GIL_IO_PNG_SUPPORT
#include "Rasterization.hpp"
#include "Utility.hpp"
#include "generic/tools/Color.hpp"
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil.hpp>
#include <png.h>
#endif

namespace {

using namespace generic::geometry;

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Point2D<num_type> & p)
{
    return os << boost::geometry::wkt(p);
}

template <typename num_type>
inline std::istream & operator >> (std::istream & is, Point2D<num_type> & p)
{
    std::string s;
    std::getline(is, s);
    try { boost::geometry::read_wkt(s, p); }
    catch (...) { is.setstate(std::ios::failbit); }
    return is;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Point3D<num_type> & p)
{
    char sp(32);
    return os << "POINT(" << p[0] << sp << p[1] << sp << p[2] << ")";
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Segment2D<num_type> & s)
{
    char sp(32);
    os << "SEGMENT(";
    os << s[0][0] << sp << s[0][1] << ",";
    os << s[1][0] << sp << s[1][1] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Segment3D<num_type> & s)
{
    char sp(32);
    os << "SEGMENT(";
    os << s[0][0] << sp << s[0][1] << sp << s[0][2] << ",";
    os << s[1][0] << sp << s[1][1] << sp << s[1][2] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Triangle2D<num_type> & t)
{
    char sp(32);
    os << "TRIANGLE(";
    os << t[0][0] << sp << t[0][1] << ",";
    os << t[1][0] << sp << t[1][1] << ",";
    os << t[2][0] << sp << t[2][1] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Triangle3D<num_type> & t)
{
    char sp(32);
    os << "TRIANGLE(";
    os << t[0][0] << sp << t[0][1] << sp << t[0][2] << ",";
    os << t[1][0] << sp << t[1][1] << sp << t[1][2] << ",";
    os << t[2][0] << sp << t[2][1] << sp << t[2][2] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Box2D<num_type> & b)
{
    char sp(32);
    os << "BOX(";
    os << b[0][0] << sp << b[0][1] << ",";
    os << b[1][0] << sp << b[1][1] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator<< (std::ostream & os, const Box3D<num_type> & b)
{
    char sp(32);
    os << "BOX(";
    os << b[0][0] << sp << b[0][1] << sp << b[0][2] << ",";
    os << b[1][0] << sp << b[1][1] << sp << b[1][2] << ")";
    return os;
}

template <typename num_type>
inline std::ostream & operator << (std::ostream & os, const Polygon2D<num_type> & p)
{
    return os << boost::geometry::wkt(p);
}

template <typename num_type>
inline std::istream & operator >> (std::istream & is, Polygon2D<num_type> & p)
{
    std::string s;
    std::getline(is, s);
    try { boost::geometry::read_wkt(s, p); }
    catch (...) { is.setstate(std::ios::failbit); }
    if(p.Size() && (p.ConstFront() == p.ConstBack())) p.PopBack();
    return is;
}

template <typename num_type>
inline std::ostream & operator << (std::ostream & os, const PolygonWithHoles2D<num_type> & pwh)
{
    return os << boost::geometry::wkt(pwh);
}

template <typename num_type>
inline std::istream & operator >> (std::istream & is, PolygonWithHoles2D<num_type> & pwh)
{
    using point_t = boost::geometry::model::point<num_type, 2, boost::geometry::cs::cartesian>;
    using polygon_t = boost::geometry::model::polygon<point_t>;
    polygon_t p;
    std::string s;
    std::getline(is, s);
    try { boost::geometry::read_wkt(s, p); }
    catch (...) { is.setstate(std::ios::failbit); }
    pwh = toPolygonWithHoles2D(p);
    return is;
}

template <typename geometry_t, typename std::enable_if<traits::is_geometry_t<geometry_t>::value, bool>::type = true>
inline std::string toString(const geometry_t & geometry)
{
    std::stringstream ss;
    ss << geometry;
    return ss.str();
}

}

namespace generic  {
namespace geometry {

///@brief class with geometrie I/O functions
class GeometryIO
{
public:
    /**
     * @brief writes a collection of geometry to WKT(Well-Know Text) file
     * @param[in] file the output *.wtk filename
     * @param[in] begin iterator to the beginning of the geometry collection
     * @param[in] end iterator to the ending of the geometry collection
     * @return whether write file successfully
     */
    template <typename geometry, typename iterator,
              typename std::enable_if<std::is_same<geometry,
              typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
    static bool Write(const std::string & file, iterator begin, iterator end)
    {
        std::ofstream out(file);
        if(!out.is_open()) return false;
        for(auto iter = begin; iter != end; ++iter){
            typename std::iterator_traits<iterator>::reference r = *iter;
            if(iter != begin) out << GENERIC_DEFAULT_EOL;
            out << r;
        }
        out.close();
        return true;
    }

    /**
     * @brief reads a collection of geometry from WKT(Well-Know Text) file
     * @param[in] file the input *.wtk filename
     * @param[in] result back insert iterator of the geometry collection
     * @param[out] err error message if failed to read
     * @return whether read file successfully
     */
    template <typename geometry, typename iterator>
    static bool Read(const std::string & file, iterator result, std::string * err = nullptr)
    {
        std::ifstream in(file);
        if(!in.is_open()) {
            if(err) *err = "Error: fail to open: " + file;
            return false;
        }

        size_t line = 0;
        while(!in.eof()){
            line++;
            if(in.peek() == '#'){
                std::string tmp;
                std::getline(in, tmp);
                continue;
            }
            geometry g; in >> g;
            if(in.fail()){
                if(err) *err = "Error: unrecognized syntax in line: " + std::to_string(line);
                return false;
            }
            *result++ = std::move(g);
        }
        in.close();
        return true;
    }

#ifdef BOOST_GIL_IO_PNG_SUPPORT

    /**
     * @brief draws a collection of geometry to an image file with png format
     * @param[in] filename the output *.png file name
     * @param[in] begin iterator to the beginning of the geometry collection
     * @param[in] end iterator to the beginning of the geometry collection
     * @param[in] width pixel width of the image
     * @param[in] color geometry outline color
     * @param[in] bgColor back ground color
     * @return whether read file successfully
     */
    template <typename geometry, typename iterator,
              typename std::enable_if<std::is_same<geometry,
              typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
    static bool WritePNG(const std::string & filename, iterator begin, iterator end, size_t width = 512,
                         int color = generic::color::black, int bgColor = generic::color::white)
    {
        using namespace boost::gil;
        using coor_t = typename geometry::coor_t;

        auto bbox = Extent(begin, end);
        auto stride = bbox.Length() / coor_t(width);
        size_t height = static_cast<size_t>(double(width) / bbox.Length() * bbox.Width());

        rgb8_image_t img(width, height);
        rgb8_image_t::view_t v = boost::gil::view(img);

        int r, g, b;
        generic::color::RGBFromInt(bgColor, r, g, b);
        fill_pixels(v, rgb8_pixel_t(r, g, b));

        WriteImgView<geometry>(v, begin, end, Vector2D<coor_t>(stride, stride), bbox[0], color);

        write_view(filename, boost::gil::view(img), png_tag());
        return true;
    }

private:
    template <typename geometry, typename iterator,
              typename std::enable_if<std::is_same<geometry,
              typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
    static void WriteImgView(boost::gil::rgb8_image_t::view_t & view, iterator begin, iterator end,
                             const Vector2D<typename geometry::coor_t> & stride,
                             const Point2D<typename geometry::coor_t> & ref, int rgb)
    {
        using namespace boost::gil;
        using coor_t = typename geometry::coor_t;

        auto width = view.width();
        auto height = view.height();

        int r, g, b;
        generic::color::RGBFromInt(rgb, r, g, b);
        auto color = rgb8_pixel_t(r, g, b);
        std::vector<std::array<int, 2> > grids;
        for(auto iter = begin;iter != end; ++iter){
            grids.clear();
            Rasterization::Rasterize(*iter, stride, grids, ref);
            for(const auto & grid : grids){
                if(grid[0] < 0 || grid[0] >= width) continue;
                if(grid[1] < 0 || grid[1] >= height) continue;
                view(grid[0], height - grid[1] - 1) = color;
            }
        }
    }

#endif//BOOST_GIL_IO_PNG_SUPPORT
};

}//namespace geometry
}//namespace generic

#endif//GENERIC_GEOMETRY_GEOMETRYIO_HPP


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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 NT2_TOOLBOX_IEEE_FUNCTION_SCALAR_FAST_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SCALAR_FAST_FREXP_HPP_INCLUDED
#include <boost/mpl/vector.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>

/////////////////////////////////////////////////////////////////////////////
// Compute fast_frexp(const A0& a0)
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fast_frexp_, tag::cpu_,
                            (A0),
                            (double_<A0>)
                           )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fast_frexp_(tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      int r1t;
      boost::fusion::at_c<0>(res) = ::frexp(a0, &r1t);
      boost::fusion::at_c<1>(res) = r1t;
      return res;
    }
  };
} }

NT2_REGISTER_DISPATCH(tag::fast_frexp_, tag::cpu_,
                            (A0),
                            (float_<A0>)
                           )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fast_frexp_(tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type mantissa;
      typedef typename meta::as_integer<A0,signed>::type          exponent;
      typedef boost::fusion::vector<mantissa,exponent>             type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      boost::fusion::at_c<0>(res) = ::frexpf(a0, &boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }

#endif
// modified by jt the 26/12/2010
// manually modified by jt the 29/12/2010


/*
 * car_longitudinal_model.hpp
 *
 * Created on: Oct 30, 2018 05:47
 * Description:
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef CAR_LONGITUDINAL_MODEL_HPP
#define CAR_LONGITUDINAL_MODEL_HPP

#include <boost/numeric/odeint.hpp>

namespace robotnav {
// Reference:
//  [1] Althoff, M., and A. Mergel. 2011. “Comparison of Markov Chain
//  Abstraction
//      and Monte Carlo Simulation for the Safety Assessment of Autonomous
//      Cars.” IEEE Transactions on Intelligent Transportation Systems 12 (4):
//      1237–47.
class CarLongitudinalModel {
 public:
  using control_type = double;
  using state_type = std::vector<double>;

  CarLongitudinalModel(control_type u) : u_(u) {}

  static constexpr double v_sw = 7.3;    // switching velocity
  static constexpr double v_max = 18.0;  // 18 m/s ~= 40 mph
  static constexpr double a_max = 7.0;   // 7.0 m/s^2

  // x1 = s, x2 = v
  void operator()(const state_type &x, state_type &xd, const double);

 private:
  control_type u_ = 0;
};
}  // namespace robotnav
#endif /* CAR_LONGITUDINAL_MODEL_HPP */


#ifndef FINDINTRA_CPP
#define FINDINTRA_CPP

#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <math.h>
#include <vector>

#include <gsl/gsl_cdf.h>

#include <scythestat/rng/mersenne.h>
#include <scythestat/distributions.h>
#include <scythestat/ide.h>
#include <scythestat/la.h>
#include <scythestat/matrix.h>
#include <scythestat/rng.h>
#include <scythestat/smath.h>
#include <scythestat/stat.h>
#include <scythestat/optimize.h>

#include <IRLS_glm/IRLS.h>

//#include <RInside.h>

#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <boost/test/unit_test.hpp>

#define INTTAG 0
#define BOOLTAG 1
#define DIETAG 2

#define TESTFREQTHRES 1
#define NEIGHBDIS 5
//#define MORANI 0.001
#define PCUTOFF 0.005
#define PRECISION 10e48
#define READBLOCK 409600
#define OPTDIST 2000
#define MAXDIST 100000

//using namespace std;
using std::abs;
using std::basic_string;
using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::exception;
using std::flush;
using std::ifstream;
using std::istringstream;
using std::make_pair;
using std::map;
using std::ofstream;
using std::ostringstream;
using std::pair;
using std::sort;
using std::string;
using std::vector;

int reader_dif(const string&, vector< vector< vector<int> > >&);
int reader_dsm(const string&, vector< vector<int> >&);
void reader_ictm(const string&, map<int, int>&);
int reader_coef(const string&, map<int, double>&);
int findIntraDomainInteraction(map<int, int>&, string, vector< vector<int> >&, vector< vector< vector<int> > >&, map<int, int>&, map<int, double>&, map< int, map< pair<int, int>, double > >&, double, double, double);
int rmDependentCol(vector< vector<double> >&, vector<double>&, vector< pair<int, int> >&, double, int&, vector< vector<double> >&);
int calStanScores(vector< vector<double> >&);
int calPearsonCorr(vector< vector<double> >&, vector< pair<int, int> >&, const double&, int, map<int, int>&);
double calBgFrags(map<int, double>&, int);
int writer_corrFile(map< pair<int, int>, double >&, string&);

void usage();

inline void usage()
{
    cout <<"Usage: findIntraDomainInteraction empericalDistributionFile interChrFreqFile domainSitesFile domainInteractionFreqFile chrNum MORANI CorrelationThresholdForScreen"<<endl;
}

//convtIndex is now 0 based
inline int convtIndex(int row_fun, int col_fun, int siteNum_fun)
{
    if (row_fun==col_fun || row_fun>=siteNum_fun || col_fun>=siteNum_fun || row_fun<0 || col_fun<0)
    {//cout<<"row or column exceed bound."<<endl;
        return -1;
    }
    else
    {
        int max_fun = 0, min_fun = 0;
        if (row_fun<col_fun)
        {
            //switch to 1 based
            max_fun = col_fun+1;
            min_fun = row_fun+1;
        }
        else
        {
            max_fun = row_fun+1;
            min_fun = col_fun+1;
        }
        return((2*siteNum_fun-min_fun)*(min_fun-1)/2+max_fun-min_fun) - 1;//-1 put the output to be 0 based
    }
}

//l=sigma(yi(theta*xi+offseti)-exp(theta*xi+offseti))
class PoissonModel {
    public:
    double operator() (const scythe::Matrix<double> beta){
        const int n = y_.rows();
        const int p = X_.cols();

        scythe::Matrix<double> eta = X_ * beta + offset_;
        scythe::Matrix<double> m = exp(eta);
        double loglike = 0.0;
        for (int i=0; i<n; ++i)
        loglike += y_(i) * log(m(i)) - m(i);
        return -1.0 * loglike;
    }
    scythe::Matrix<double> y_;
    scythe::Matrix<double> X_;
    scythe::Matrix<double> offset_;
};

int main(int argc, char* argv[])
{
    if (argc < 6)
    {
        usage();
        exit(1);
    }

    //string empDisFile = argv[1];
    string ictmFile = argv[1];
    string dsmFile = argv[2];
    string difFile = argv[3];
    string chrNum = argv[4];
    //double MORANI = atof(argv[6]);
    double corrThres = 0.0;
    string disCoefFile = argv[5];
    double disIntcp = 0.0;
    double disCoeff = 0.0;

    map<int, int> empDis_map;
    map<int, int> ictm_map;
    vector< vector<int> > dsm_map;
    vector< vector< vector<int> > > dif_map;
    map<int, double> disPolyCoef;

    //reader_ictm(empDisFile, empDis_map);
    //cout<<"\nReading empDis file done."<<endl;
    reader_ictm(ictmFile, ictm_map);
    cout<<"\nRead ictm file done."<<endl;
    reader_dsm(dsmFile, dsm_map);
    cout<<"\nRead dsm file done."<<endl;
    reader_dif(difFile, dif_map);
    cout<<"\nRead dif file done."<<endl;
    reader_coef(disCoefFile, disPolyCoef);
    cout<<"\nRead distance polynomial fit coefficients done."<<endl;

    map< int, map< pair<int, int>, double > > pVal_map;

    findIntraDomainInteraction(empDis_map, chrNum, dsm_map, dif_map, ictm_map, disPolyCoef, pVal_map, corrThres, disIntcp, disCoeff);

    return 0;
}

int reader_dif(const string& fileToread, vector< vector< vector<int> > >& dif_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char freq_str[15];
    int freq_int = 0;
    int domainNum = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    //ofstream testBadFile("test");
    vector< vector<int> > tempDomn;
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempFreq;
            //testBadFile<<lineStr<<endl;

            //pass head white space
            if(*it_lineStr == 'd')
            {
                if (tempDomn.begin() != tempDomn.end())
                {
                    ++domainNum;
                    dif_local.push_back(tempDomn);
                    tempDomn.clear();
                }
                continue;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = freq_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    freq_int = atoi(freq_str);
                    tempFreq.push_back(freq_int);
                }
            }

            tempDomn.push_back(tempFreq);
            ++peakNum;
        }
    }

    if(tempDomn.begin() != tempDomn.end())
    {
        dif_local.push_back(tempDomn);
    }

    inputFile.close();

    return peakNum;
}

int reader_dsm(const string& fileToread, vector< vector<int> >& dsm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char pos_str[15];
    int pos_int = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    //ofstream testBadFile("test");
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempPos;
            //testBadFile<<lineStr<<endl;

            //pass head white space
            while(*it_lineStr == ' '|| *it_lineStr == '\t')
            {
                ++it_lineStr;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = pos_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    pos_int = atoi(pos_str);
                    tempPos.push_back(pos_int);
                }
            }

            dsm_local.push_back(tempPos);
            ++peakNum;
        }
    }

    inputFile.close();

    return peakNum;
}

int reader_coef(const string& fileToread, map<int, double>& disPolyCoef_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char degree[15];
    int degree_int = 0;
    char coef[50];
    double coef_dbl = 0.0;

    char* it_lineStr;
    char* it_token;

    char lineStr[512];
    while(inputFile.getline(lineStr,512))
    {
        if (lineStr != NULL)
        {
            it_lineStr = lineStr;

            it_token = degree;
            while(*it_lineStr != '\t' && *it_lineStr != ' ' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            degree_int = atoi(degree);
	    ++it_lineStr;
	    while(*it_lineStr == ' ' || *it_lineStr == '\t')
	    {
		++it_lineStr;
	    }

            it_token = coef;
            while(*it_lineStr != '\t' && *it_lineStr != ' ' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            coef_dbl = atof(coef);

            disPolyCoef_local[degree_int] = coef_dbl;
        }
    }
    return 0;
}
void reader_ictm(const string& fileToread, map<int, int>& ictm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char pos[15];
    int pos_int = 0;
    char freq[15];
    int freq_int = 0;

    char* it_lineStr;
    char* it_token;

    char lineStr[512];
    while(inputFile.getline(lineStr,512))
    {
        if (lineStr != NULL)
        {
            it_lineStr = lineStr;

            it_token = pos;
            while(*it_lineStr != '\t')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            ++it_lineStr;
            pos_int = atoi(pos);

            it_token = freq;
            while(*it_lineStr != '\t' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            freq_int = atoi(freq);

            ictm_local[pos_int] = freq_int;
        }
    }
}

int findIntraDomainInteraction(map<int, int>& empDisMap_fun, string jobChr_fun, vector< vector<int> >& domainSitesMap_fun, vector< vector< vector<int> > >& domainCSinterFreq_fun, map<int, int>& csInterChromTotalMap_fun, map<int, double>& disPolyCoef_local, map< int, map< pair<int, int>, double > >& pVal_fun, double corrThres_fun, double disIntcp_fun, double disCoeff_fun)
{
    //string file_effBetas =  jobChr_fun + "_domainEffBetas";
    //ofstream outEffBetas(file_effBetas.c_str());

    //srand((unsigned)time(0));
    //double rndNum = rand()/(RAND_MAX + 1.0);

    int domainNum = domainSitesMap_fun.size();

    vector<double> freq_convert;
    vector<double> offset;//distance effect
    vector<double> effVec;//efficiency of cutting site and mappability, measured by the total inter chromosome hybrid frags
    //#pragma omp parallel for
    for (int domainIt = 0; domainIt < domainNum; ++domainIt)
    {
        map< pair<int, int>, double>& domainPval = pVal_fun[domainIt];
        vector<int>& sitesMap = domainSitesMap_fun[domainIt];
        vector< vector<int> >& csInterFreqMap = domainCSinterFreq_fun[domainIt];
        int siteNum = sitesMap.size();

        int size_convert = convtIndex(siteNum-2,siteNum-1,siteNum) + 1;//the convtIndex function's first two input is 0 based and the output is 0 based too
        for (int rowIt=0; rowIt<siteNum; ++rowIt)
        {
            for (int colIt=rowIt+1; colIt<siteNum; ++colIt)
            {
                int index_convert=convtIndex(rowIt,colIt,siteNum);

		if(abs(sitesMap[rowIt]-sitesMap[colIt])>OPTDIST && abs(sitesMap[rowIt]-sitesMap[colIt])<MAXDIST)//the index of freq_convert need to be changed because of this
		{
		    //log(y/ydist)=mu+betaeff*effvec
		    freq_convert.push_back(csInterFreqMap[rowIt][colIt]);
		    offset.push_back(calBgFrags(disPolyCoef_local, abs(sitesMap[rowIt]-sitesMap[colIt])));
		    //the digestion and ligation efficient of a cutting site should take log because, its log should proportional to log(freq)
		    effVec.push_back(log(sqrt(csInterChromTotalMap_fun[sitesMap[rowIt]])*sqrt(csInterChromTotalMap_fun[sitesMap[colIt]])+1));
		}
            }
        }
    }

    int size_convert_s = freq_convert.size();
    string file_freq =  jobChr_fun + "_freq";
    string file_offset =  jobChr_fun + "_offset";
    string file_effvec =  jobChr_fun + "_effvec";
    //ofstream outEffBetas(file_effBetas.c_str());
    ofstream outFreq(file_freq);
    ofstream outOffset(file_offset);
    ofstream outEffvec(file_effvec);
    for(int i=0;i<size_convert_s;++i)
    {
	outFreq<<freq_convert[i]<<endl;
	outOffset<<offset[i]<<endl;
	outEffvec<<effVec[i]<<endl;
    }
    cout<<"Out matrix done."<<endl;
    outFreq.close();
    outOffset.close();
    outEffvec.close();

    return 0;
}

int rmDependentCol(vector< vector<double> >& distMatrix_local, vector<double>& freqVec_local, vector< pair<int, int> >& oriCoor_local, double thres_local, int& testIndex_local, vector< vector<double> >& distMatRmDep_local)
{
    int rowNum_fun = distMatrix_local.size();
    if (rowNum_fun != freqVec_local.size())
    {
        cerr<<"Error: row number of the dist matrix is different from the size of the frequency vector."<<endl;
        exit(1);
    }
    int colNum_fun = (distMatrix_local.begin())->size();
    vector< vector<double> > distMatrix_std;
    for (int i = 0; i < colNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < rowNum_fun; ++j)
        {
            tempVec.push_back(distMatrix_local[j][i]);
        }
        distMatrix_std.push_back(tempVec);
    }
    distMatrix_std.push_back(freqVec_local);
    calStanScores(distMatrix_std);

    map< pair<int, int>, double > corrMap;
    map<int, int> rmIndex;
    int testFlag_local = calPearsonCorr(distMatrix_std, oriCoor_local, thres_local, testIndex_local, rmIndex);
    cout<<"testFlag: "<<testFlag_local<<endl;

    int oriTestIndex = testIndex_local;
    for (int m = 0; m < oriTestIndex; ++m)
    {
        if (rmIndex[m]==1)
        {
            testIndex_local--;
        }
    }

    for (int i = 0; i < rowNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < colNum_fun; ++j)
        {
            if (rmIndex[j]!=1)
            {
                tempVec.push_back(distMatrix_local[i][j]);
            }
        }
        distMatRmDep_local.push_back(tempVec);
    }

    return testFlag_local;
}

int calStanScores(vector< vector<double> >& distMatrix_std_fun)
{
    for (vector< vector<double> >::iterator colIt = distMatrix_std_fun.begin(), colIt_end = distMatrix_std_fun.end(); colIt != colIt_end; ++colIt) //colomn as in original matrix before transpose
    {
        int rowNum_fun = colIt->size();
        double sum = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            sum += *rowIt;
        }
        double mean = sum / rowNum_fun;

        double var = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            double diff = *rowIt - mean;
            var += diff * diff;
        }
        var = var / (rowNum_fun - 1);
        double std = sqrt(var);

        for (vector<double>::iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            *rowIt = (*rowIt - mean) / std;
        }
    }

    return 0;
}

int calPearsonCorr(vector< vector<double> >& distMatrix_std_fun, vector< pair<int, int> >& oriCoor_fun, const double& thres_fun, int testIndex_fun, map<int, int>& rmIndex_fun)
{
    int sigNum = 0;

    map< pair<int, int>, double > corrMap_fun;
    vector<double> xyCorr;
    int colNum_fun = distMatrix_std_fun.size(); // colomn as in original matrix
    int rowNum_fun = (distMatrix_std_fun.begin())->size();
    int freedom = rowNum_fun - 1;
    for (int i = 0; i < colNum_fun; ++i)
    {
        for (int j = i+1; j < colNum_fun; ++j)
        {
            double pearsonCorr = 0.0;
            for (int m = 0; m < rowNum_fun; ++m)
            {
                pearsonCorr += distMatrix_std_fun[i][m] * distMatrix_std_fun[j][m];
            }
            pearsonCorr /= freedom;

            //if (pearsonCorr >= pos_thres || pearsonCorr <= neg_thres)

            if (j==colNum_fun-1)
            {
                xyCorr.push_back(pearsonCorr);
            }
            else if (pearsonCorr >= thres_fun || pearsonCorr <= -thres_fun)
            {
                //cout<<pearsonCorr<<endl;
                corrMap_fun[make_pair(i, j)] = pearsonCorr;
            }
        }
    }

    //string corrFile = "corrFile";
    //writer_corrFile(corrMap_fun, corrFile);

    for (map< pair<int, int>, double >::const_iterator sigIt = corrMap_fun.begin(), sigIt_end = corrMap_fun.end(); sigIt != sigIt_end; ++sigIt)
    {
        int col1 = sigIt->first.first, col2 = sigIt->first.second;
        pair<int, int>& oriCol1 = oriCoor_fun[col1], oriCol2 = oriCoor_fun[col2];
        cout<<testIndex_fun<<"\t"<<col1<<"\t"<<col2<<"\t"<<oriCol1.first<<"\t"<<oriCol1.second<<"\t"<<xyCorr[col1]<<"\t"<<oriCol2.first<<"\t"<<oriCol2.second<<"\t"<<xyCorr[col2]<<endl;

        if(col1==testIndex_fun)
        {
            //if within the 3*3 square centered on the testing spot, there is a spot with higher freq than the testing spot, and this high freq spot has high corr with the testing spot, then skip the testing spot, since distMatrix_std_fun[colNum_fun-1][col1] is not the frequency of col1 (need the origin col1), use corr instead
            if(abs(oriCol1.first-oriCol2.first)>1 || abs(oriCol1.second-oriCol2.second)>1 || xyCorr[col1]>=xyCorr[col2])
            {
                rmIndex_fun[col2] = 1;
            }
            else
            {
                return 0;
            }
        }
        else if(col2==testIndex_fun)
        {
            if(abs(oriCol1.first-oriCol2.first)>1 || abs(oriCol1.second-oriCol2.second)>1 || xyCorr[col2]>=xyCorr[col1])
            {
                rmIndex_fun[col1] = 1;
            }
            else
            {
                return 0;
            }
        }
        else
        {
            //always keep the one with higher positive correlation
            //cout<<"break 1"<<endl;
            rmIndex_fun[xyCorr[col1]>xyCorr[col2]?col2:col1] = 1;
            //cout<<"break 2"<<endl;
        }
    }

    return 1;
}

int writer_corrFile(map< pair<int, int>, double >& corrMap_local, string& fileName)
{
    ofstream outputFile(fileName.c_str());

    for (map< pair<int, int>, double>::const_iterator corrIt = corrMap_local.begin(), corrIt_end = corrMap_local.end(); corrIt != corrIt_end; ++corrIt)
    {
        outputFile<<corrIt->first.first<<"\t"<<corrIt->first.second<<"\t"<<corrIt->second<<endl;
    }
    outputFile.close();

    return 0;
}

double calBgFrags(map<int, double>& coef_fun, int dist_fun)
{
    double logDist=log(dist_fun);
    double bgFrags=0.0;
    map<int, double>::const_iterator coefIt=coef_fun.begin(), coefIt_end=coef_fun.end();
    for( ; coefIt != coefIt_end; ++coefIt)
    {
	bgFrags+=pow(logDist, coefIt->first)*coefIt->second;
    }
    return bgFrags;
}

#endif