#ifdef MEX
#ifdef WITH_EMBREE
#  include "winding_number_ray.h"
#endif

#include <igl/parallel_for.h>
#include <igl/winding_number.h>
#include <igl/WindingNumberAABB.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/writeOBJ.h>
#include <igl/get_seconds.h>
#include <igl/per_face_normals.h>
#ifdef WITH_EMBREE
#  include <igl/embree/EmbreeIntersector.h>
#endif
#include <igl/random_dir.h>

#include <mex.h>
#include <Eigen/Dense>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/C_STR.h>
#include <iostream>

// Inputs:
//   nrhs  number of right hand side arguments
//   prhs  pointer to right hand side arguments
// Outputs:
//   V  #V by dim list of vertex positiobns
//   F  #F by dim+1 list of facet indices
//   O  #O by dim list of origin positions
//   hierarchical  Whether to use hierarchical evaluation
//   ray_cast  Whether to use Ray tracing
//   ray_parity  Use parity version
//   num_rays  Number of rays
//   twod_rays  2d rays only
//
void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  Eigen::MatrixXd & O,
  bool & hierarchical,
  bool & ray_cast,
  bool & ray_parity,
  int & num_rays,
  bool & twod_rays)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3 || dim == 2,
    "Mesh vertex list must be #V by 2 or 3 list of vertex positions");

  mexErrMsgTxt(dim == mxGetN(prhs[1]),
    "Mesh \"face\" simplex size must equal dimension");

  parse_rhs_double(prhs+0,V);
  parse_rhs_index(prhs+1,F);
  parse_rhs_double(prhs+2,O);
  mexErrMsgTxt(mxGetN(prhs[2]) == dim,
    "Origin list dimension must match vertex list dimension");
  // set number of faces

  // Default values
  hierarchical = true;
  ray_cast = false;
  ray_parity = false;
  num_rays = 32;
  twod_rays = false;
  {
    int i = 3;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),
        "Parameter names should be char strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Hierarchical",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        hierarchical = (mxLogical)*mxGetPr(prhs[++i]);
        mexErrMsgTxt(!hierarchical || dim==3,
          "Hierarchical only supported for dim = 3");
      }else if(strcmp("RayCast",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        ray_cast = (mxLogical)*mxGetPr(prhs[++i]);
        mexErrMsgTxt(!ray_cast || dim==3,
          "RayCast only supported for dim = 3");
      }else if(strcmp("RayParity",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        ray_parity = (mxLogical)*mxGetPr(prhs[++i]);
        mexErrMsgTxt(!ray_parity|| dim==3,
          "RayParity  only supported for dim = 3");
      }else if(strcmp("TwoDRays",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        twod_rays = (mxLogical)*mxGetPr(prhs[++i]);
      }else if(strcmp("NumRays",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        num_rays = (int)*mxGetPr(prhs[++i]);
      }else
      {
        mexErrMsgTxt(false,
          C_STR("Unsupported parameter: "<<name));
      }
      i++;
    }
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);

  Eigen::MatrixXd V,O;
  Eigen::MatrixXi F;
  bool hierarchical, ray_cast, ray_parity, twod_rays;
  int num_rays;
  parse_rhs(
    nrhs,prhs,V,F,O, hierarchical, ray_cast, ray_parity, num_rays, twod_rays);
  const int dim = V.cols();

  //// Set up openmp
  //int nProcessors=omp_get_max_threads();
  ////std::cout<<nProcessors<<std::endl;
  //omp_set_num_threads(min(nProcessors,2));

  Eigen::VectorXd W(O.rows(),1);
  double start_sec = igl::get_seconds();
  if(ray_cast)
  {
#ifdef WITH_EMBREE
    // Initialize Embree
    //cout<<"Flipping faces..."<<endl;
    MatrixXi FF;
    FF.resize(F.rows()*2,F.cols());
    FF << F, F.rowwise().reverse().eval();
    //cout<<"Computing normals..."<<endl;
    Eigen::MatrixXd N;
    per_face_normals(V,F,N);
    //cout<<"Initializing Embree..."<<endl;
    // Initialize intersector
    igl::embree::EmbreeIntersector ei;
    ei.init(V.cast<float>(),FF);

    // loop over origins
    const int no = O.rows();
    for(int o = 0;o<no;o++)
    {
      double Wo = 0;
      RowVector3d p = O.row(o);
      for(int r = 0;r<num_rays;r++)
      {
        int num_rays_shot = -1;
        Vector3d dir = random_dir();
        if(twod_rays)
        {
          dir[2] = 0;
          dir.normalize();
        }
        double w = 0;
        if(ray_parity)
        {
          winding_number_ray_parity(ei,N,p,dir,w,num_rays_shot);
        }else
        {
          winding_number_ray(ei,N,p,dir,w,num_rays_shot);
        }
        Wo += w;
      }
      W(o) = Wo/(double)num_rays;
    }

#else
    igl::matlab::mexErrMsgTxt(false,"Recompile with WITH_EMBREE defined");
#endif
  }else
  {
    if(hierarchical && dim == 3)
    {
      // Initialize hierarchy
      WindingNumberAABB< Eigen::RowVector3d, Eigen::MatrixXd, Eigen::MatrixXi>
        hier(V,F);
      // Build hierarchy
      hier.grow();
      // loop over origins
      igl::parallel_for(
        O.rows(),
        [&hier,&W,&O](const int o)
        {
          W(o) = hier.winding_number(O.row(o));
        },
        10000);
    }else
    {
      winding_number(V,F,O,W);
    }
  }
  //mexPrintf("Elapsed time is %g seconds\n",igl::get_seconds()-start_sec);

  switch(nlhs)
  {
    case 1:
    {
      igl::matlab::prepare_lhs_double(W,plhs+0);
      // Fall through
    }
    default:break;
  }

  std::cout.rdbuf(outbuf);
}

#endif


#include <iostream>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "dart/constraint/BoxedLcpConstraintSolver.hpp"
#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/BoxShape.hpp"
#include "dart/dynamics/FreeJoint.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/dynamics/WeldJoint.hpp"
#include "dart/math/Geometry.hpp"
#include "dart/simulation/World.hpp"

#include "GradientTestUtils.hpp"
#include "TestHelpers.hpp"

using namespace dart;
using namespace dynamics;
using namespace simulation;

/*
TEST(ScrewGeometry, EXP_JAC)
{
  srand(42);
  s_t EPS = 0.001;

  Eigen::Vector3s axis = Eigen::Vector3s::UnitX();
  Eigen::Matrix3s originalRotation = math::expMapRot(axis);

  Eigen::Vector3s perturb = Eigen::Vector3s::UnitY();
  Eigen::Matrix3s perturbedRotation = math::expMapRot(axis + perturb * EPS);
  Eigen::Matrix3s perturbation
      = originalRotation.transpose() * perturbedRotation;
  Eigen::Vector3s perturbPlus = math::logMap(perturbation);

  std::cout << "Perturb: " << std::endl << perturb << std::endl;
  std::cout << "+Perturb Recovered: " << std::endl << perturbPlus << std::endl;

  perturbedRotation = math::expMapRot(axis - perturb * EPS);
  perturbation = originalRotation.transpose() * perturbedRotation;
  Eigen::Vector3s perturbMinus = math::logMap(perturbation);

  std::cout << "-Perturb Recovered: " << std::endl << perturbMinus << std::endl;

  Eigen::Vector3s perturbGrad = (perturbPlus - perturbMinus) / (2 * EPS);

  std::cout << "Perturb Grad: " << std::endl << perturbGrad << std::endl;

  Eigen::Matrix3s perturbationRecovered = math::expMapRot(perturbGrad * EPS);

  std::cout << "Perturbation: " << std::endl << perturbation << std::endl;
  std::cout << "Perturbation Recovered: " << std::endl
            << perturbationRecovered << std::endl;
}
*/

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

This test sets up a configuration that looks like this:

          Force
            |
            v
          +---+
Force --> |   |
          +---+
      -------------
            ^
       Fixed ground

There's a box with six DOFs, a full FreeJoint, with a force driving it into the
ground. The ground has configurable friction in this setup.

*/
void testFreeBlockWithFrictionCoeff(
    s_t frictionCoeff, s_t mass, bool freeJoint)
{
  // World
  WorldPtr world = World::create();

  // Set up the LCP solver to be super super accurate, so our
  // finite-differencing tests don't fail due to LCP errors. This isn't
  // necessary during a real forward pass, but is helpful to make the
  // mathematical invarients in the tests more reliable.
  static_cast<constraint::BoxedLcpConstraintSolver*>(
      world->getConstraintSolver())
      ->makeHyperAccurateAndVerySlow();

  ///////////////////////////////////////////////
  // Create the box
  ///////////////////////////////////////////////

  SkeletonPtr box = Skeleton::create("box");

  std::pair<Joint*, BodyNode*> pair;
  if (freeJoint)
  {
    pair = box->createJointAndBodyNodePair<FreeJoint>(nullptr);
  }
  else
  {
    pair = box->createJointAndBodyNodePair<BallJoint>(nullptr);
  }
  Joint* boxJoint = pair.first;
  BodyNode* boxBody = pair.second;

  Eigen::Isometry3s fromParent = Eigen::Isometry3s::Identity();
  fromParent.translation() = Eigen::Vector3s::UnitX() * 2;
  boxJoint->setTransformFromParentBodyNode(fromParent);

  Eigen::Isometry3s fromChild = Eigen::Isometry3s::Identity();
  fromChild.translation() = Eigen::Vector3s::UnitX();
  boxJoint->setTransformFromChildBodyNode(fromChild);

  std::shared_ptr<BoxShape> boxShape(
      new BoxShape(Eigen::Vector3s(1.0, 1.0, 1.0)));
  boxBody->createShapeNodeWith<VisualAspect, CollisionAspect>(boxShape);
  boxBody->setFrictionCoeff(frictionCoeff);

  // Add a force driving the box down into the floor, and to the left
  boxBody->addExtForce(Eigen::Vector3s(1, -1, 0));
  // Prevent the mass matrix from being Identity
  boxBody->setMass(mass);

  // Move off the origin to X=1, rotate 90deg on the Y axis
  if (freeJoint)
  {
    box->setPosition(3, 1.0);
  }
  box->setPosition(1, 90 * M_PI / 180);

  world->addSkeleton(box);

  /*
  Eigen::Matrix4d realTransform = boxBody->getWorldTransform().matrix();
  Eigen::Matrix4d expTransform = math::expMapDart(box->getPositions()).matrix();
  if (realTransform != expTransform)
  {
    std::cout << "Transforms don't match!" << std::endl;
    std::cout << "Real: " << std::endl << realTransform << std::endl;
    std::cout << "Exp: " << std::endl << expTransform << std::endl;
    std::cout << "Diff: " << std::endl
              << (realTransform - expTransform) << std::endl;
    return;
  }
  */

  ///////////////////////////////////////////////
  // Create the floor
  ///////////////////////////////////////////////

  SkeletonPtr floor = Skeleton::create("floor");

  std::pair<WeldJoint*, BodyNode*> floorPair
      = floor->createJointAndBodyNodePair<WeldJoint>(nullptr);
  WeldJoint* floorJoint = floorPair.first;
  BodyNode* floorBody = floorPair.second;

  Eigen::Isometry3s floorPosition = Eigen::Isometry3s::Identity();
  floorPosition.translation() = Eigen::Vector3s(0, -(1.0 - 1e-2), 0);
  floorJoint->setTransformFromParentBodyNode(floorPosition);
  floorJoint->setTransformFromChildBodyNode(Eigen::Isometry3s::Identity());

  std::shared_ptr<BoxShape> floorShape(
      new BoxShape(Eigen::Vector3s(10.0, 1.0, 10.0)));
  floorBody->createShapeNodeWith<VisualAspect, CollisionAspect>(floorShape);
  floorBody->setFrictionCoeff(frictionCoeff);

  world->addSkeleton(floor);

  ///////////////////////////////////////////////
  // Run the tests
  ///////////////////////////////////////////////

  box->computeForwardDynamics();
  box->integrateVelocities(world->getTimeStep());
  Eigen::VectorXs timestepVel = box->getVelocities();
  Eigen::VectorXs timestepWorldVel = world->getVelocities();

  // world->step();

  /*
  server::GUIWebsocketServer server;
  server.renderWorld(world);
  server.serve(8070);

  while (server.isServing())
  {
  }
  */

  Eigen::VectorXs worldVel = world->getVelocities();
  // Test the classic formulation
  EXPECT_TRUE(verifyPerturbedScrewAxisForForce(world));
  /*
  EXPECT_TRUE(testScrews(world));
  EXPECT_TRUE(verifyAnalyticalJacobians(world));
  EXPECT_TRUE(verifyVelGradients(world, worldVel));
  EXPECT_TRUE(verifyAnalyticalBackprop(world));
  EXPECT_TRUE(verifyWrtMass(world));
  */
}

// #ifdef ALL_TESTS
TEST(GRADIENTS, FREE_BLOCK)
{
  testFreeBlockWithFrictionCoeff(1e7, 1, true);
}
// #endif

#ifdef ALL_TESTS
TEST(GRADIENTS, BALL_BLOCK)
{
  testFreeBlockWithFrictionCoeff(1e7, 1, false);
}
#endif

#ifdef ALL_TESTS
TEST(GRADIENTS, FREE_VELOCITY_INTEGRATION)
{
  // World
  WorldPtr world = World::create();
  world->setGravity(Eigen::Vector3s::UnitY());

  SkeletonPtr box = Skeleton::create("box");

  std::pair<Joint*, BodyNode*> pair
      = box->createJointAndBodyNodePair<FreeJoint>(nullptr);
  Joint* boxJoint = pair.first;
  BodyNode* boxBody = pair.second;

  // boxBody->addExtForce(Eigen::Vector3s(1, -1, 0));

  world->addSkeleton(box);

  std::cout << world->getMassMatrix() << std::endl;

  Eigen::Vector6s vel = Eigen::Vector6s::Zero();
  vel(0) = 1.0;
  vel(4) = 1.0;
  world->setVelocities(vel);

  world->step();
  world->step();
  BackpropSnapshotPtr snapshot = neural::forwardPass(world, true);
  Eigen::MatrixXs jacC
      = snapshot->getJacobianOfC(world, WithRespectTo::POSITION);

  // verifyF_c(world);
  // runVelocityTest(world);
  std::cout << jacC << std::endl;
}
#endif

#ifdef ALL_TESTS
TEST(GRADIENTS, NORMALIZED_SCREW_GRADIENT_ROTATE_X)
{
  Eigen::Vector6s screwX = Eigen::Vector6s::Zero();
  screwX(0) = 1.0;
  Eigen::Vector3s point = Eigen::Vector3s::UnitY();
  // if we rotate point by screwX, it should move in the negative Z direction
  s_t theta = -90 * 3.1415926535 / 180;
  // This should be at (0, 0, 1) = UnitZ()
  Eigen::Vector3s rotatedPoint = math::expMap(screwX * theta) * point;
  Eigen::Vector3s expectedPoint = -Eigen::Vector3s::UnitZ();
  EXPECT_TRUE(equals(rotatedPoint, expectedPoint, 1e-6));

  s_t EPS = 1e-7;
  Eigen::Vector3s perturbedPos = math::expMap(screwX * (theta + EPS)) * point;
  Eigen::Vector3s perturbedNeg = math::expMap(screwX * (theta - EPS)) * point;
  Eigen::Vector3s bruteForceGradient
      = (perturbedPos - perturbedNeg) / (2 * EPS);
  Eigen::Vector3s expectedGradient = Eigen::Vector3s::UnitY();
  EXPECT_TRUE(equals(bruteForceGradient, expectedGradient, 1e-9));

  Eigen::Vector3s analyticalGradient
      = math::gradientWrtTheta(screwX, point, theta);
  EXPECT_TRUE(equals(analyticalGradient, expectedGradient, 1e-9));
}
#endif

#ifdef ALL_TESTS
TEST(GRADIENTS, NORMALIZED_SCREW_GRADIENT_RANDOM_THETA_ZERO)
{
  for (int i = 0; i < 20; i++)
  {
    Eigen::Vector6s screwRand = Eigen::Vector6s::Random();
    screwRand.head<3>() = screwRand.head<3>().normalized();

    Eigen::Vector3s point = Eigen::Vector3s::UnitY();
    s_t theta = 0;

    s_t EPS = 1e-7;
    Eigen::Vector3s perturbedPos
        = math::expMap(screwRand * (theta + EPS)) * point;
    Eigen::Vector3s perturbedNeg
        = math::expMap(screwRand * (theta - EPS)) * point;
    Eigen::Vector3s bruteForceGradient
        = (perturbedPos - perturbedNeg) / (2 * EPS);

    Eigen::Vector3s analyticalGradient
        = math::gradientWrtTheta(screwRand, point, theta);
    EXPECT_TRUE(equals(analyticalGradient, bruteForceGradient, 1e-9));
  }
}
#endif

#ifdef ALL_TESTS
TEST(GRADIENTS, UNNORMALIZED_SCREW_GRADIENT_RANDOM_THETA_ZERO)
{
  for (int i = 0; i < 20; i++)
  {
    Eigen::Vector6s screwRand = Eigen::Vector6s::Random();
    // screwRand.head<3>() = screwRand.head<3>().normalized();

    Eigen::Vector3s point = Eigen::Vector3s::UnitY();
    s_t theta = 0;

    s_t EPS = 1e-7;
    Eigen::Vector3s perturbedPos
        = math::expMap(screwRand * (theta + EPS)) * point;
    Eigen::Vector3s perturbedNeg
        = math::expMap(screwRand * (theta - EPS)) * point;
    Eigen::Vector3s bruteForceGradient
        = (perturbedPos - perturbedNeg) / (2 * EPS);

    Eigen::Vector3s analyticalGradient
        = math::gradientWrtTheta(screwRand, point, theta);
    EXPECT_TRUE(equals(analyticalGradient, bruteForceGradient, 1e-9));
  }
}
#endif

/*
TEST(GRADIENTS, NORMALIZED_SCREW_GRADIENT_RANDOM_THETA_RANDOM)
{
  for (int i = 0; i < 20; i++)
  {
    Eigen::Vector6s screwRand = Eigen::Vector6s::Random();
    screwRand.head<3>() = screwRand.head<3>().normalized();

    Eigen::Vector3s point = Eigen::Vector3s::UnitY();
    s_t theta = rand() * 2 * 3.1415926535;

    s_t EPS = 1e-7;
    Eigen::Vector3s perturbedPos
        = math::expMap(screwRand * (theta + EPS)) * point;
    Eigen::Vector3s perturbedNeg
        = math::expMap(screwRand * (theta - EPS)) * point;
    Eigen::Vector3s bruteForceGradient
        = (perturbedPos - perturbedNeg) / (2 * EPS);

    Eigen::Vector3s analyticalGradient
        = math::gradientWrtTheta(screwRand, point, theta);
    EXPECT_TRUE(equals(analyticalGradient, bruteForceGradient, 1e-9));
  }
}
*/


#include <set>
using namespace std;

#include "Denoising.h"

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Triplet<double> Triplet;

bool operator<(const Vector3d& v1, const Vector3d& v2) {
    if(v1[0] != v2[0]) return v1[0] < v2[0];
    if(v1[1] != v2[1]) return v1[1] < v2[1];
    return v1[2] < v1[2];
}

bool operator>(const Vector3d& v1, const Vector3d& v2) {
    if(v1[0] != v2[0]) return v1[0] > v2[0];
    if(v1[1] != v2[1]) return v1[1] > v2[1];
    return v1[2] > v1[2];
}

double genrand_gauss(double mu, double sigma) {
    double z = sqrt(-2.0 * log(genrand_real2())) * sin(2.0 * M_PI * genrand_real2());
    return mu + sigma * z;
}

void addNoise(Mesh& mesh) {
    init_genrand((unsigned long)time(NULL));
    OpenMesh::VPropHandleT<Mesh::Point> vprop;
    mesh.add_property(vprop);

    Mesh::VertexIter v_it;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        double vx = genrand_gauss(0.0, 0.001);
        double vy = genrand_gauss(0.0, 0.001);
        double vz = genrand_gauss(0.0, 0.001);
        mesh.property(vprop, *v_it) = mesh.point(*v_it) + Mesh::Point(vx, vy, vz);
        mesh.set_point(*v_it, mesh.property(vprop, *v_it));
    }
    mesh.remove_property(vprop);
}

void denoise(Mesh& mesh) {
    // Prepare coefficient matrices R and D.
    Mesh::EdgeIter e_it;
    Mesh::FaceVertexIter fv_it;

    const int nVert = static_cast<int>(mesh.n_vertices());
    const int nEdge = static_cast<int>(mesh.n_edges());

    vector<Triplet> tripR;
    vector<Triplet> tripD;

    int ie = 0;
    for(e_it = mesh.edges_begin(); e_it != mesh.edges_end(); ++e_it) {
        Mesh::HalfedgeHandle heh = mesh.halfedge_handle(*e_it, 0);

        Mesh::VertexHandle vh1 = mesh.to_vertex_handle(heh);
        Mesh::VertexHandle vh3 = mesh.from_vertex_handle(heh);
        Mesh::VertexHandle vh2 = mesh.opposite_vh(heh);
        Mesh::VertexHandle vh4 = mesh.opposite_he_opposite_vh(heh);

        int i1 = vh1.idx();
        int i2 = vh2.idx();
        int i3 = vh3.idx();
        int i4 = vh4.idx();
        if(i1 == -1 || i2 == -1 || i3 == -1 || i4 == -1) {
            continue;
        }

        OpenMesh::Vec3f p1 = mesh.point(vh1);
        OpenMesh::Vec3f p2 = mesh.point(vh2);
        OpenMesh::Vec3f p3 = mesh.point(vh3);
        OpenMesh::Vec3f p4 = mesh.point(vh4);

        double S123 = 0.5 * OpenMesh::cross(p2 - p1, p2 - p3).norm();
        double S134 = 0.5 * OpenMesh::cross(p4 - p1, p4 - p3).norm();
        double l13  = (p1 - p3).norm();

        double coef1 = (S123 * OpenMesh::dot(p4 - p3, p3 - p1) + S134 * OpenMesh::dot(p1 - p3, p3 - p2)) / (l13 * l13 * (S123 + S134));
        double coef2 = S134 / (S123 + S134);
        double coef3 = (S123 * OpenMesh::dot(p3 - p1, p1 - p4) + S134 * OpenMesh::dot(p2 - p1, p1 - p3)) / (l13 * l13 * (S123 + S134));
        double coef4 = S123 / (S123 + S134);

        tripR.push_back(Triplet(ie, i1, coef1));
        tripR.push_back(Triplet(ie, i2, coef2));
        tripR.push_back(Triplet(ie, i3, coef3));
        tripR.push_back(Triplet(ie, i4, coef4));

        tripD.push_back(Triplet(ie, i1, 1.0));
        tripD.push_back(Triplet(ie, i2, -1.0));
        tripD.push_back(Triplet(ie, i3, 1.0));
        tripD.push_back(Triplet(ie, i4, -1.0));
        ie++;
    }

    SpMat R(nEdge, nVert);
    SpMat D(nEdge, nVert);
    R.setFromTriplets(tripR.begin(), tripR.end());
    D.setFromTriplets(tripD.begin(), tripD.end());

    // Store vertex positions in the Eigen::Vector
    Matrix pInit(nVert, 3);
    Matrix pVec(nVert, 3);
    Mesh::VertexIter v_it;
    int ip = 0;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        Mesh::Point p = mesh.point(*v_it);
        pInit(ip, 0) = p[0];
        pInit(ip, 1) = p[1];
        pInit(ip, 2) = p[2];

        pVec(ip, 0) = p[0];
        pVec(ip, 1) = p[1];
        pVec(ip, 2) = p[2];
        ip++;
    }

    // Prepare sparse matrix.
    vector<Triplet> tripI(nVert);
    for(int i=0; i<nVert; i++) {
        tripI[i] = Triplet(i, i, 1.0);
    }
    SpMat I(nVert, nVert);
    I.setFromTriplets(tripI.begin(), tripI.end());

    // Solve linear problem.
    double lambda = 1.0e-2;
    double alpha  = 0.1;
    double beta   = 1.0e-3;
    double bmax   = 1.0;
    double mu     = 2.0;

    // Matrix pVec = pInit;
    Matrix dlt(nEdge, 3);
    while(beta < bmax) {
        // solve sub-problem for delta (shrinkage operator)
        Matrix y = D * pVec;
        for(int i=0; i<nEdge; i++) {
            dlt(i, 0) = 0.0;
            for(int d=0; d<3; d++) {
                dlt(i, 0) += y(i, d) * y(i, d);
            }

            if(dlt(i, 0) < lambda / beta) {
                dlt(i, 0) = 0.0;
            }

            dlt(i, 1) = dlt(i, 0);
            dlt(i, 2) = dlt(i, 0);
        }

        // solve sub-problem for points (linear system)
        SpMat  A = I + alpha * R.transpose() * R + beta * D.transpose() * D;
        Matrix b = pInit + beta * D.transpose() * dlt;

        Eigen::ConjugateGradient<SpMat> solver;
        solver.compute(A);
        pVec = solver.solve(b);
        if(solver.info() != Eigen::Success) {
            printf("Solve linear system failed.\n");
        }

        // update parameters
        printf(".");
        beta  *= mu;
        alpha *= 0.5;
    }
    printf("\n");

    // Update vertex information_OBJMESH_H_
    OpenMesh::VPropHandleT<Mesh::Point> vprop;
    mesh.add_property(vprop);
    ip = 0;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        mesh.property(vprop, *v_it) = Mesh::Point(pVec(ip, 0), pVec(ip, 1), pVec(ip, 2));
        mesh.set_point(*v_it, mesh.property(vprop, *v_it));
        ip++;
    }
    mesh.remove_property(vprop);
}


#ifndef URASTER_HPP
#define URASTER_HPP

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>	//for .inverse().  Probably not needed
#include <vector>
#include <array>
#include <memory>
#include <functional>

namespace uraster {

	class Pixel {
		public:
			Eigen::Vector4f color;
			float& depth() {
				return color[3];
			}
			Pixel():color(0.0f,0.0f,0.0f,-1e10f) {
			}
	};

	bool _wireframe = false;

	//This is the framebuffer class.  It's a part of namespace uraster because the uraster needs to have a well-defined image class to render to.
	//It is templated because the output type need not be only colors, could contain anything (like a stencil buffer or depth buffer or gbuffer for deferred rendering)
	template<class PixelType>
		class Framebuffer {
			protected:
				std::vector<PixelType> data;
			public:
				const std::size_t width;
				const std::size_t height;
				//constructor initializes the array
				Framebuffer(std::size_t w,std::size_t h):
					data(w*h),
					width(w),height(h) {
					}
				//2D pixel access
				PixelType& operator()(std::size_t x,std::size_t y) {
					return data[y*width+x];
				}
				//const version
				const PixelType& operator()(std::size_t x,std::size_t y) const {
					return data[y*width+x];
				}
				void clear(const PixelType& pt=PixelType()) {
					std::fill(data.begin(),data.end(),pt);
				}
		};

	//This function runs the vertex shader on all the vertices, producing the varyings that will be interpolated by the rasterizer.
	//VertexVsIn can be anything, VertexVsOut MUST have a position() method that returns a 4D vector, and it must have an overloaded *= and += operator for the interpolation
	//The right way to think of VertexVsOut is that it is the class you write containing the varying outputs from the vertex shader.
	template<class VertexVsIn,class VertexVsOut,class VertShader>
		void run_vertex_shader(const VertexVsIn* b,const VertexVsIn* e,VertexVsOut* o,
				VertShader vertex_shader) {
			std::size_t n=e-b;
#pragma omp parallel for
			for(std::size_t i=0; i<n; i++) {
				o[i]=vertex_shader(b[i]);
			}
		}
	struct BarycentricTransform {
		private:
			Eigen::Vector2f offset;
			Eigen::Matrix2f Ti;
		public:
			BarycentricTransform(const Eigen::Vector2f& s1,const Eigen::Vector2f& s2,const Eigen::Vector2f& s3):
				offset(s3) {
					Eigen::Matrix2f T;
					T << (s1-s3),(s2-s3);
					Ti=T.inverse();
				}
			Eigen::Vector3f operator()(const Eigen::Vector2f& v) const {
				Eigen::Vector2f b;
				b=Ti*(v-offset);
				return Eigen::Vector3f(b[0],b[1],1.0f-b[0]-b[1]);
			}
	};

	void line(int x0, int y0, int x1, int y1, uraster::Framebuffer<uraster::Pixel>& fp, Eigen::Array2i ul, Eigen::Array2i lr)
	{
		int dx =  abs(x1-x0), sx = x0<x1 ? 1 : -1;
		int dy = -abs(y1-y0), sy = y0<y1 ? 1 : -1;
		int err = dx+dy, e2; /* error value e_xy */
		uraster::Pixel p;
		p.color = Eigen::Vector4f(1, 1, 1, 1);

		while(1) {
			if (x0 < ul[0]) x0 = ul[0];
			if (x0 >= lr[0]) x0 = lr[0]-1;
			if (y0 < ul[1]) y0 = ul[1];
			if (y0 >= lr[1]) y0 = lr[1]-1;
			fp(x0, y0) = p;
			//fp(x0+1, y0) = p;
			//fp(x0, y0+1) = p;
			//fp(x0+1, y0+1) = p;
			//fp(x0-1, y0) = p;
			//fp(x0, y0-1) = p;
			//fp(x0-1, y0-1) = p;
			if (x0==x1 && y0==y1) break;
			e2 = 2*err;
			if (e2 > dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */
			if (e2 < dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */
		}
	}

	//This function takes in 3 varyings vertices from the vertex shader that make up a triangle,
	//rasterizes the triangle and runs the fragment shader on each resulting pixel.
	template<class PixelOut,class VertexVsOut,class FragShader>
		void rasterize_triangle(Framebuffer<PixelOut>& fb,const std::array<VertexVsOut,3>& verts,FragShader fragment_shader) {
			std::array<Eigen::Vector4f,3> points {{verts[0].position(),verts[1].position(),verts[2].position()}};
			//Do the perspective divide by w to get screen space coordinates.
			std::array<Eigen::Vector4f,3> epoints {{points[0]/points[0][3],points[1]/points[1][3],points[2]/points[2][3]}};
			auto ss1=epoints[0].head<2>().array(),ss2=epoints[1].head<2>().array(),ss3=epoints[2].head<2>().array();

			//calculate the bounding box of the triangle in screen space floating point.
			Eigen::Array2f bb_ul=ss1.min(ss2).min(ss3);
			Eigen::Array2f bb_lr=ss1.max(ss2).max(ss3);
			Eigen::Array2i isz(fb.width,fb.height);

			//convert bounding box to fixed point.
			//move bounding box from (-1.0,1.0)->(0,imgdim)
			Eigen::Array2i ibb_ul=((bb_ul*0.5f+0.5f)*isz.cast<float>()).cast<int>();
			Eigen::Array2i ibb_lr=((bb_lr*0.5f+0.5f)*isz.cast<float>()).cast<int>();
			ibb_lr+=1;	//add one pixel of coverage

			//clamp the bounding box to the framebuffer size if necessary
			ibb_ul=ibb_ul.max(Eigen::Array2i(0,0));
			ibb_lr=ibb_lr.min(isz);

			BarycentricTransform bt(ss1.matrix(),ss2.matrix(),ss3.matrix());

				static uraster::Framebuffer<uraster::Pixel> tp(fb.width, fb.height); // mask pixels to draw
			if (_wireframe) {

			int x001 = 0, x010 = 0, x100 = 0;
			int y001 = 0, y010 = 0, y100 = 0;
			Eigen::Vector2f ex_001(1.0, -1.0), ex_010(1.0, -1.0), ex_100(1.0, -1.0);
			// find the extremities in barycentric coordinates of each corner of the
			// vertex in viewport coordinates

			bool visible = false, set_1 = false, set_2 = false, set_3 = false;
			// tell the fragment shader to only draw these in $color
			for(int y=ibb_ul[1]; y<ibb_lr[1]; y++) {
				for(int x=ibb_ul[0]; x<ibb_lr[0]; x++) {
					Eigen::Vector2f ssc(x, y);
					ssc.array()/=isz.cast<float>();	//move pixel to relative coordinates
					ssc.array()-=0.5f;
					ssc.array()*=2.0f;

					//Compute barycentric coordinates of the pixel center
					Eigen::Vector3f bary=bt(ssc);

					//if the pixel has valid barycentric coordinates, the pixel is in the triangle
					if(!((bary.array() <= 1.0f).all() && (bary.array() >= 0.0f).all())) {
						continue;
					}
					visible = true;

					if (bary[0] + bary[1] < ex_001[0] || bary[2] > ex_001[1]) {
					//if (bary[2] > ex_001[1]) {
						ex_001[0] = bary[0] + bary[1];
						ex_001[1] = bary[2];
						x001 = x;
						y001 = y;
						set_1 = true;
					}
					if (bary[0] + bary[2] < ex_010[0] || (bary[1] > ex_010[1])) {
					//if (bary[1] > ex_010[1]) {
						ex_010[0] = bary[0] + bary[2];
						ex_010[1] = bary[1];
						x010 = x;
						y010 = y;
						set_2 = true;
					}
					if (bary[1] + bary[2] < ex_100[0] || (bary[0] > ex_100[1])) {
					//if (bary[0] > ex_100[1]) {
						ex_100[0] = bary[1] + bary[2];
						ex_100[1] = bary[0];
						x100 = x;
						y100 = y;
						set_3 = true;
					}
				}
			}

				tp.clear();
				if (visible && set_1 && set_2 && set_3) {
					line(x001, y001, x010, y010, tp, ibb_ul, ibb_lr);
					line(x010, y010, x100, y100, tp, ibb_ul, ibb_lr);
					line(x100, y100, x001, y001, tp, ibb_ul, ibb_lr);
				}
					}
				//

				//std::cout << coords.size() << " ";

				// calculate all display coordinates of projected edges of the triangle

				//std::cout << ex_001 << std::endl << ex_010 << std::endl << ex_100 << std::endl;
				//exit(1);
				auto a = verts[2].p - verts[1].p;
				auto b = verts[1].p - verts[0].p;
				Eigen::Vector3f c(a[0], a[1], a[2]);
				Eigen::Vector3f d(b[0], b[1], b[2]);
				auto normal = c.cross(d).normalized();

				//for all the pixels in the bounding box
				for(int y=ibb_ul[1]; y<ibb_lr[1]; y++) {
					for(int x=ibb_ul[0]; x<ibb_lr[0]; x++) {
						Eigen::Vector2f ssc(x,y);
						ssc.array()/=isz.cast<float>();	//move pixel to relative coordinates
						ssc.array()-=0.5f;
						ssc.array()*=2.0f;

						//Compute barycentric coordinates of the pixel center
						Eigen::Vector3f bary=bt(ssc);

						//if the pixel has valid barycentric coordinates, the pixel is in the triangle
						if(!((bary.array() < 1.0f).all() && (bary.array() > 0.0f).all())) {
							continue;
						}
						float d=bary[0]*epoints[0][2]+bary[1]*epoints[1][2]+bary[2]*epoints[2][2];
						//Reference the current pixel at that coordinate
						PixelOut& po=fb(x,y);
						// if the interpolated depth passes the depth test
						if(po.depth() >= d || d > 1.0) {
							continue;
						}
						//interpolate varying parameters
						VertexVsOut v;
						for(int i=0; i<3; i++) {
							VertexVsOut vt=verts[i];
							vt*=bary[i];
							v+=vt;
						}
						if (_wireframe) {
						if (tp(x, y).color[0] == 1) {
							v.p[3] = 0.0;
						} else {
							v.p[3] = 0.5;
						}
						}
						v.p[0] = normal[0];
						v.p[1] = normal[1];
						v.p[2] = normal[2];
						//call the fragment shader
						po=fragment_shader(v);
						po.depth()=d; //write the depth buffer
					}
				}
					}

					//This function rasterizes a set of triangles determined by an index buffer and a buffer of output verts.
					template<class PixelOut,class VertexVsOut,class FragShader>
						void rasterize(Framebuffer<PixelOut>& fb,const std::size_t* ib,const std::size_t* ie,const VertexVsOut* verts,
								FragShader fragment_shader) {
							std::size_t n=ie-ib;
#pragma omp parallel for
							for(std::size_t i=0; i<n; i+=3) {
								const std::size_t* ti=ib+i;
								std::array<VertexVsOut,3> tri {{verts[ti[0]],verts[ti[1]],verts[ti[2]]}};
								rasterize_triangle(fb,tri,fragment_shader);
							}
						}

					//This function does a draw call from an indexed buffer
					template<class PixelOut,class VertexVsOut,class VertexVsIn,class VertShader, class FragShader>
						void draw(Framebuffer<PixelOut>& fb,
								const VertexVsIn* vertexbuffer_b,const VertexVsIn* vertexbuffer_e,
								const std::size_t* indexbuffer_b,const std::size_t* indexbuffer_e,
								VertexVsOut* vcache_b,VertexVsOut* vcache_e,
								VertShader vertex_shader,
								FragShader fragment_shader, bool wireframe = false) {
							std::unique_ptr<VertexVsOut[]> vc;
							_wireframe = wireframe;
							if(vcache_b==NULL || (vcache_e-vcache_b) != (vertexbuffer_e-vertexbuffer_b)) {
								vcache_b=new VertexVsOut[(vertexbuffer_e-vertexbuffer_b)];
								vc.reset(vcache_b);
							}
							run_vertex_shader(vertexbuffer_b,vertexbuffer_e,vcache_b,vertex_shader);
							rasterize(fb,indexbuffer_b,indexbuffer_e,vcache_b,fragment_shader);
						}

					struct VertVsOut {
						Eigen::Vector4f p;
						Eigen::Vector3f color;

						VertVsOut():
							p(0.0f,0.0f,0.0f,0.0f),color(0.0f,0.0f,0.0f) {
							}
						const Eigen::Vector4f& position() const {
							return p;
						}
						VertVsOut& operator+=(const VertVsOut& tp) {
							p+=tp.p;
							color+=tp.color;
							return *this;
						}
						VertVsOut& operator*=(const float& f) {
							p*=f;
							color*=f;
							return *this;
						}
					};

					//VertVsOut example_vertex_shader(const Eigen::Vector3f& vin,const Eigen::Matrix4f& mvp,float t) {
					//	VertVsOut vout;
					//	vout.p=mvp*Eigen::Vector4f(vin[0],vin[1],vin[2],1.0f);
					//	//vout.p[3]=1.0f;
					//	vout.color=Eigen::Vector3f(1.0f,static_cast <float> (rand()) / static_cast <float> (RAND_MAX),0.0f);
					//	return vout;
					//}

					//Pixel example_fragment_shader(const VertVsOut& fsin) {
					//	Pixel p;
					//	p.color.head<3>()=fsin.color;
					//	return p;
					//}
					// example end

				}

#endif


#include "rotation.h"
#include "himan_common.h"
#include <Eigen/Geometry>

using namespace Eigen;

template <typename T>
void himan::geoutil::rotate(himan::geoutil::position<T>& p, const himan::geoutil::rotation<T>& r)
{
	// Map data structures to Eigen library objects
	Map<Matrix<T, 3, 1>> P(p.Data());
	Map<const Quaternion<T>> QR(r.Data());

	// Create a corresponding quaternion of the input position vector
	Quaternion<T> QP;
	QP.w() = 0;
	QP.vec() = P;

	// Apply spatial rotation through quaternion products
	Quaternion<T> rotatedP = QR * QP * QR.inverse();
	P = rotatedP.vec();
}
template void himan::geoutil::rotate<float>(himan::geoutil::position<float>&, const himan::geoutil::rotation<float>&);
template void himan::geoutil::rotate<double>(himan::geoutil::position<double>&,
                                             const himan::geoutil::rotation<double>&);

template <typename T>
himan::geoutil::position<T> himan::geoutil::rotate(const himan::geoutil::position<T>& p,
                                                   const himan::geoutil::rotation<T>& r)
{
	position<T> ret(p);
	rotate(ret, r);
	return ret;
}
template himan::geoutil::position<float> himan::geoutil::rotate<float>(const himan::geoutil::position<float>&,
                                                                       const himan::geoutil::rotation<float>&);
template himan::geoutil::position<double> himan::geoutil::rotate<double>(const himan::geoutil::position<double>&,
                                                                         const himan::geoutil::rotation<double>&);

template <typename T>
himan::geoutil::rotation<T> himan::geoutil::rotation<T>::FromRotLatLon(const T& latOfSouthPole, const T& lonOfSouthPole,
                                                                       const T& angleOfRot)
{
	himan::geoutil::rotation<T> ret;

	// Map data structures to Eigen library objects
	Map<Quaternion<T>> QRot(ret.Data());

	// Create a rotation quaternion from product of a series of rotations about principle axis
	QRot = AngleAxis<T>(lonOfSouthPole, Matrix<T, 3, 1>::UnitZ()) *
	       AngleAxis<T>(-(T(M_PI / 2.0) + latOfSouthPole), Matrix<T, 3, 1>::UnitY()) *
	       AngleAxis<T>(-angleOfRot, Matrix<T, 3, 1>::UnitZ());

	return ret;
}
template himan::geoutil::rotation<float> himan::geoutil::rotation<float>::FromRotLatLon(const float& latOfSouthPole,
                                                                                const float& lonOfSouthPole,
                                                                                const float& angleOfRot);
template himan::geoutil::rotation<double> himan::geoutil::rotation<double>::FromRotLatLon(const double& latOfSouthPole,
                                                                                const double& lonOfSouthPole,
                                                                                const double& angleOfRot);

template <typename T>
himan::geoutil::rotation<T> himan::geoutil::rotation<T>::ToRotLatLon(const T& latOfSouthPole, const T& lonOfSouthPole,
                                                                     const T& angleOfRot)
{
	himan::geoutil::rotation<T> ret;
	// Map data structures to Eigen library objects
	Map<Quaternion<T>> QRot(ret.Data());

	// Create a rotation quaternion from product of a series of rotations about principle axis
	QRot = AngleAxis<T>(angleOfRot, Matrix<T, 3, 1>::UnitZ()) *
	       AngleAxis<T>(T(M_PI / 2.0) + latOfSouthPole, Matrix<T, 3, 1>::UnitY()) *
	       AngleAxis<T>(-lonOfSouthPole, Matrix<T, 3, 1>::UnitZ());

	return ret;
}
template himan::geoutil::rotation<float> himan::geoutil::rotation<float>::ToRotLatLon(const float& latOfSouthPole,
                                                                                const float& lonOfSouthPole,
                                                                                const float& angleOfRot);
template himan::geoutil::rotation<double> himan::geoutil::rotation<double>::ToRotLatLon(const double& latOfSouthPole,
                                                                                const double& lonOfSouthPole,
                                                                                const double& angleOfRot);


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  InitializePose3.cpp
 *  @author Luca Carlone
 *  @author Frank Dellaert
 *  @date   August, 2014
 */

#include <gtsam/slam/InitializePose3.h>

#include <gtsam/slam/InitializePose.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>

#include <boost/math/special_functions.hpp>

#include <utility>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {

  GaussianFactorGraph linearGraph;

  for(const auto& factor: g) {
    Matrix3 Rij;
    double rotationPrecision = 1.0;

    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between){
      Rij = pose3Between->measured().rotation().matrix();
      Vector precisions = Vector::Zero(6);
      precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
      pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
      rotationPrecision = precisions[0]; // rotations first
    }else{
      cout << "Error in buildLinearOrientationGraph" << endl;
    }

    const auto& keys = factor->keys();
    Key key1 = keys[0], key2 = keys[1];
    Matrix M9 = Z_9x9;
    M9.block(0,0,3,3) = Rij;
    M9.block(3,3,3,3) = Rij;
    M9.block(6,6,3,3) = Rij;
    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
  }
  // prior on the anchor orientation
  linearGraph.add(
      initialize::kAnchorKey, I_9x9,
      (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
          .finished(),
          noiseModel::Isotropic::Precision(9, 1));
  return linearGraph;
}

/* ************************************************************************* */
// Transform VectorValues into valid Rot3
Values InitializePose3::normalizeRelaxedRotations(
    const VectorValues& relaxedRot3) {
  gttic(InitializePose3_computeOrientationsChordal);

  Values validRot3;
  for(const auto& it: relaxedRot3) {
    Key key = it.first;
    if (key != initialize::kAnchorKey) {
      Matrix3 M;
      M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized

      // ClosestTo finds rotation matrix closest to H in Frobenius sense
      Rot3 initRot = Rot3::ClosestTo(M.transpose());
      validRot3.insert(key, initRot);
    }
  }
  return validRot3;
}

/* ************************************************************************* */
NonlinearFactorGraph InitializePose3::buildPose3graph(
    const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_buildPose3graph);
  return initialize::buildPoseGraph<Pose3>(graph);
}

/* ************************************************************************* */
Values InitializePose3::computeOrientationsChordal(
    const NonlinearFactorGraph& pose3Graph) {
  gttic(InitializePose3_computeOrientationsChordal);

  // regularize measurements and plug everything in a factor graph
  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);

  // Solve the LFG
  VectorValues relaxedRot3 = relaxedGraph.optimize();

  // normalize and compute Rot3
  return normalizeRelaxedRotations(relaxedRot3);
}

/* ************************************************************************* */
Values InitializePose3::computeOrientationsGradient(
    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
    const size_t maxIter, const bool setRefFrame) {
  gttic(InitializePose3_computeOrientationsGradient);

  // this works on the inverse rotations, according to Tron&Vidal,2011
  Values inverseRot;
  inverseRot.insert(initialize::kAnchorKey, Rot3());
  for(const auto key_value: givenGuess) {
    Key key = key_value.key;
    const Pose3& pose = givenGuess.at<Pose3>(key);
    inverseRot.insert(key, pose.rotation().inverse());
  }

  // Create the map of edges incident on each node
  KeyVectorMap adjEdgesMap;
  KeyRotMap factorId2RotMap;

  createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);

  // calculate max node degree & allocate gradient
  size_t maxNodeDeg = 0;
  VectorValues grad;
  for(const auto key_value: inverseRot) {
    Key key = key_value.key;
    grad.insert(key,Z_3x1);
    size_t currNodeDeg = (adjEdgesMap.at(key)).size();
    if(currNodeDeg > maxNodeDeg)
      maxNodeDeg = currNodeDeg;
  }

  // Create parameters
  double b = 1;
  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
  double a = (M_PI*M_PI)/(2*f0);
  double rho = 2*a*b;
  double mu_max = maxNodeDeg * rho;
  double stepsize = 2/mu_max; // = 1/(a b dG)

  double maxGrad;
  // gradient iterations
  size_t it;
  for (it = 0; it < maxIter; it++) {
    //////////////////////////////////////////////////////////////////////////
    // compute the gradient at each node
    maxGrad = 0;
    for (const auto key_value : inverseRot) {
      Key key = key_value.key;
      Vector gradKey = Z_3x1;
      // collect the gradient for each edge incident on key
      for (const size_t& factorId : adjEdgesMap.at(key)) {
        Rot3 Rij = factorId2RotMap.at(factorId);
        Rot3 Ri = inverseRot.at<Rot3>(key);
        auto factor = pose3Graph.at(factorId);
        const auto& keys = factor->keys();
        if (key == keys[0]) {
          Key key1 = keys[1];
          Rot3 Rj = inverseRot.at<Rot3>(key1);
          gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
        } else if (key == keys[1]) {
          Key key0 = keys[0];
          Rot3 Rj = inverseRot.at<Rot3>(key0);
          gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
        } else {
          cout << "Error in gradient computation" << endl;
        }
      }  // end of i-th gradient computation
      grad.at(key) = stepsize * gradKey;

      double normGradKey = (gradKey).norm();
      if(normGradKey>maxGrad)
        maxGrad = normGradKey;
    } // end of loop over nodes

    //////////////////////////////////////////////////////////////////////////
    // update estimates
    inverseRot = inverseRot.retract(grad);

    //////////////////////////////////////////////////////////////////////////
    // check stopping condition
    if (it>20 && maxGrad < 5e-3)
      break;
  } // enf of gradient iterations

  // Return correct rotations
  const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
  Values estimateRot;
  for(const auto key_value: inverseRot) {
    Key key = key_value.key;
    if (key != initialize::kAnchorKey) {
      const Rot3& R = inverseRot.at<Rot3>(key);
      if(setRefFrame)
        estimateRot.insert(key, Rref.compose(R.inverse()));
      else
        estimateRot.insert(key, R.inverse());
    }
  }
  return estimateRot;
}

/* ************************************************************************* */
void InitializePose3::createSymbolicGraph(
    const NonlinearFactorGraph& pose3Graph, KeyVectorMap* adjEdgesMap,
    KeyRotMap* factorId2RotMap) {
  size_t factorId = 0;
  for (const auto& factor : pose3Graph) {
    auto pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between) {
      Rot3 Rij = pose3Between->measured().rotation();
      factorId2RotMap->emplace(factorId, Rij);

      Key key1 = pose3Between->key1();
      if (adjEdgesMap->find(key1) != adjEdgesMap->end()) {  // key is already in
        adjEdgesMap->at(key1).push_back(factorId);
      } else {
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap->emplace(key1, edge_id);
      }
      Key key2 = pose3Between->key2();
      if (adjEdgesMap->find(key2) != adjEdgesMap->end()) {  // key is already in
        adjEdgesMap->at(key2).push_back(factorId);
      } else {
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap->emplace(key2, edge_id);
      }
    } else {
      cout << "Error in createSymbolicGraph" << endl;
    }
    factorId++;
  }
}

/* ************************************************************************* */
Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
  Vector3 logRot = Rot3::Logmap(R1.between(R2));

  double th = logRot.norm();
  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
    Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation
    logRot = Rot3::Logmap(R1pert.between(R2));
    th = logRot.norm();
  }
  // exclude small or invalid rotations
  if (th > 1e-5 && th == th) {  // nonzero valid rotations
    logRot = logRot / th;
  } else {
    logRot = Z_3x1;
    th = 0.0;
  }

  double fdot = a*b*th*exp(-b*th);
  return fdot*logRot;
}

/* ************************************************************************* */
Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  return computeOrientationsChordal(pose3Graph);
}

///* ************************************************************************* */
Values InitializePose3::computePoses(const Values& initialRot,
                                      NonlinearFactorGraph* posegraph,
                                      bool singleIter) {
  gttic(InitializePose3_computePoses);
  return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
}

/* ************************************************************************* */
Values InitializePose3::initialize(const NonlinearFactorGraph& graph,
                                   const Values& givenGuess, bool useGradient) {
  gttic(InitializePose3_initialize);
  Values initialValues;

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values orientations;
  if (useGradient)
    orientations = computeOrientationsGradient(pose3Graph, givenGuess);
  else
    orientations = computeOrientationsChordal(pose3Graph);

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(orientations, &pose3Graph);
}

/* ************************************************************************* */
Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
  return initialize(graph, Values(), false);
}

} // namespace gtsam


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

  @copyright 2016 NumScale SAS

  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_CONSTANT_BITINCREMENT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_BITINCREMENT_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Bitincrement Bitincrement (function template)

  Generates the minimal increment constant for a given type.

  @headerref{<boost/simd/constant/bitincrement.hpp>}

  @par Description

  1.  @code
      template<typename T> T Bitincrement();
      @endcode

  2.  @code
      template<typename T> T Bitincrement( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to the minimal increment value for @c T.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that is defined as :

  - the constant `T(1)` if @c T models IntegerValue
  - the constant `next(T(0))` if @c T models IEEEValue

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/bitincrement.hpp>
#include <boost/simd/constant/simd/bitincrement.hpp>

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_SCALAR_FUNCTION_COPYSIGN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_COPYSIGN_HPP_INCLUDED

#include <boost/simd/function/scalar/abss.hpp>
#include <boost/simd/function/scalar/bitwise_notand.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/signnz.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return abss(a0)*signnz(a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_SIMD_HAS_COPYSIGN
      return ::copysign(a0, a1);
      // _copysign appears to be bogus for a1 = -0 in old MSVCRT
    #elif defined(BOOST_SIMD_HAS__COPYSIGN) && !defined(__MSVCRT__)
      return ::_copysign(a0, a1);
    #else
      return bitwise_or(bitofsign(a1), bitwise_notand(Signmask<A0>(), a0));
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          , bd::scalar_< bd::single_<A0> >
                                    )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_SIMD_HAS_COPYSIGNF
      return ::copysignf(a0, a1);
    #elif defined(BOOST_SIMD_HAS__COPYSIGNF)
      return ::_copysignf(a0, a1);
    #else
       return bitwise_or(bitofsign(a1), bitwise_notand(Signmask<A0>(), a0));
    #endif
    }
  };
} } }

#endif


#ifndef _BAYES_FILTER__MODELS
#define _BAYES_FILTER__MODELS

/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id: models.hpp 634 2010-08-15 16:39:44Z mistevens $
 */

/*
 * Predict and Observe models
 *  These models extend, adapt and simplify the fundamental Bayesian filter models
 *  Simple : Simplify model construction and use
 *  General: Generalise a model so it include properties of more then one model
 *  Adapted: Adapt one model type into another
 */
#include <boost/function.hpp>

/* Filter namespace */
namespace Bayesian_filter
{

typedef boost::function1<const FM::Vec&, const FM::Vec&> State_function;
// A generalised function of state. Compatible with predict and observe models

class Simple_additive_predict_model : public Additive_predict_model
// Additive predict model initialised from function and model matricies
{
	State_function ff;
public:
	Simple_additive_predict_model (State_function f_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: G, q are conformantly dimensioned (not checked)

	// No default assignment operator

	virtual const FM::Vec& f(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linrz_predict_model : public Linrz_predict_model
// Linrz predict model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_predict_model (State_function f_init, const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: Fx, G, q are conformantly dimensioned (not checked)

	// No default assignment operator

	virtual const FM::Vec& f(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linear_predict_model : public Linear_predict_model
// Linear predict model initialised from model matricies
{
public:
	Simple_linear_predict_model (const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: Fx, q and G are conformantly dimensioned (not checked)
};

class Simple_linrz_correlated_observe_model : public Linrz_correlated_observe_model
// Linrz observe model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_correlated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init);
	// Precondition: Hx, Z are conformantly dimensioned (not checked)
	// No default assignment operator

	virtual const FM::Vec& h(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linrz_uncorrelated_observe_model : public Linrz_uncorrelated_observe_model
// Linrz observe model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_uncorrelated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::Vec& Zv_init);
	// Precondition: Hx, Zv are conformantly dimensioned (not checked)
	// No default assignment operator

	virtual const FM::Vec& h(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linear_correlated_observe_model : public Linear_correlated_observe_model
// Linear observe model initialised from model matrices
{
public:
	Simple_linear_correlated_observe_model (const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init);
	// Precondition: Hx, Z are conformantly dimensioned (not checked)
};

class Simple_linear_uncorrelated_observe_model : public Linear_uncorrelated_observe_model
// Linear observe model initialised from model matrices
{
public:
	Simple_linear_uncorrelated_observe_model (const FM::Matrix& Hx_init, const FM::Vec& Zv_init);
	// Precondition: Hx, Zv are conformantly dimensioned (not checked)
};

/*
 * Model Adaptors: Constructed with a reference to another model
 */

class Adapted_Correlated_additive_observe_model : public Correlated_additive_observe_model
/*
 * Adapt Uncorrelated_additive_observe_model to an equivalent
 * Correlated_additive_observe_model_adaptor
 */
{
public:
	Adapted_Correlated_additive_observe_model (Uncorrelated_additive_observe_model& adapt);
	const FM::Vec& h(const FM::Vec& x) const
	{
		return unc.h(x);
	}
	inline void normalise (FM::Vec& z_denorm, const FM::Vec& z_from) const
	{
		unc.normalise (z_denorm, z_from);
	};
private:
	Uncorrelated_additive_observe_model& unc;
};

class Adapted_Linrz_correlated_observe_model : public Linrz_correlated_observe_model
/*
 * Adapt Linrz_uncorrelated_observe_model to an equivalent
 * Linrz_correlated_observe_model
 */
{
public:
	Adapted_Linrz_correlated_observe_model (Linrz_uncorrelated_observe_model& adapt);
	const FM::Vec& h(const FM::Vec& x) const
	{
		return unc.h(x);
	}
	inline void normalise (FM::Vec& z_denorm, const FM::Vec& z_from) const
	{
		unc.normalise (z_denorm, z_from);
	};
protected:
	Linrz_uncorrelated_observe_model& unc;
};

/*
 * Generalised Models: generalise a model so it include properties of more then one model.
 */

// General Linearised Uncorrelated Additive and Likelihood observe model
class General_LzUnAd_observe_model : public Linrz_uncorrelated_observe_model, public Likelihood_observe_model
{
public:
	General_LzUnAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_uncorrelated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}
private:
	friend class General_LiUnAd_observe_model;
	struct Likelihood_uncorrelated
	{
		Likelihood_uncorrelated(std::size_t z_size) :
			zInnov(z_size), Zv_inv(z_size)
		{	zset = false;
		}
		mutable FM::Vec zInnov;	// Normailised innovation, temporary for L(x)
		FM::Vec Zv_inv;			// Inverse Noise Covariance given zz
		Float logdetZ;			// log(det(Z))
		bool zset;
		Float L(const Uncorrelated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const;
		// Definition of likelihood for additive noise model given zz
		void Lz(const Uncorrelated_additive_observe_model& model);
	};
	Likelihood_uncorrelated li;
};

// General Linear Uncorrelated Additive and Likelihood observe model
class General_LiUnAd_observe_model : public Linear_uncorrelated_observe_model, public Likelihood_observe_model
{
public:
	General_LiUnAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linear_uncorrelated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	General_LzUnAd_observe_model::Likelihood_uncorrelated li;
};

// General Linearised Correlated Additive and Likelihood observe model
class General_LzCoAd_observe_model : public Linrz_correlated_observe_model, public Likelihood_observe_model
{
public:
	General_LzCoAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_correlated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	friend class General_LiCoAd_observe_model;
	struct Likelihood_correlated
	{
		Likelihood_correlated(std::size_t z_size) :
			zInnov(z_size), Z_inv(z_size,z_size)
		{	zset = false;
		}
		mutable FM::Vec zInnov;	// Normalised innovation, temporary for L(x)
		FM::SymMatrix Z_inv;	// Inverse Noise Covariance
		Float logdetZ;			// log(det(Z)
		bool zset;
		static Float scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V);
		Float L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const;
		// Definition of likelihood for additive noise model given zz
		void Lz(const Correlated_additive_observe_model& model);
	};
	Likelihood_correlated li;
};

// General Linear Correlated Additive and Likelihood observe model
class General_LiCoAd_observe_model : public Linear_correlated_observe_model, public Likelihood_observe_model
{
public:
	General_LiCoAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linear_correlated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	General_LzCoAd_observe_model::Likelihood_correlated li;
};

}// namespace

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * 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.                                            *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/opencvutils.h>

using namespace Eigen;
using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Consolidate redundant/overlapping detections.
 * \author Brendan Klare \cite bklare
 */
class ConsolidateDetectionsTransform : public UntrainableMetadataTransform
{
    Q_OBJECT

    void projectMetadata(const File &src, File &dst) const
    {
        dst = src;
        if (!dst.contains("Confidences"))
            return;

        //Compute overlap between rectangles and create discrete Laplacian matrix
        QList<Rect> rects = OpenCVUtils::toRects(src.rects());
        int n = rects.size();
        if (n == 0)
            return;
        MatrixXf laplace(n,n);
        for (int i = 0; i < n; i++) {
            laplace(i,i) = 0;
        }
        for (int i = 0; i < n; i++){
            for (int j = i + 1; j < n; j++) {
                float overlap = (float)((rects[i] & rects[j]).area()) / (float)max(rects[i].area(), rects[j].area());
                if (overlap > 0.5) {
                    laplace(i,j) = -1.0;
                    laplace(j,i) = -1.0;
                    laplace(i,i) = laplace(i,i) + 1.0;
                    laplace(j,j) = laplace(j,j) + 1.0;
                } else {
                    laplace(i,j) = 0;
                    laplace(j,i) = 0;
                }
            }
        }

        // Compute eigendecomposition
        SelfAdjointEigenSolver<Eigen::MatrixXf> eSolver(laplace);
        MatrixXf allEVals = eSolver.eigenvalues();
        MatrixXf allEVecs = eSolver.eigenvectors();

        //Keep eigenvectors with zero eigenvalues
        int nRegions = 0;
        for (int i = 0; i < n; i++) {
            if (fabs(allEVals(i)) < 1e-4) {
                nRegions++;
            }
        }
        MatrixXf regionVecs(n, nRegions);
        for (int i = 0, cnt = 0; i < n; i++) {
            if (fabs(allEVals(i)) < 1e-4)
                regionVecs.col(cnt++) = allEVecs.col(i);
        }

        //Determine membership for each consolidated location
        // and compute average of regions. This is determined by
        // finding which eigenvector has the highest magnitude for
        // each input dimension. Each input dimension corresponds to
        // one of the input rect region. Thus, each eigenvector represents
        // a set of overlaping regions.
        float *midX = new float[nRegions];
        float *midY = new float[nRegions];
        float *avgWidth = new float[nRegions];
        float *avgHeight = new float[nRegions];
        float *confs = new float[nRegions];
        int *cnts = new int[nRegions];
        int mx;
        int mxIdx;
        for (int i = 0 ; i < nRegions; i++) {
            midX[i] = 0;
            midY[i] = 0;
            avgWidth[i] = 0;
            avgHeight[i] = 0;
            confs[i] = 0;
            cnts[i] = 0;
        }

        QList<float> confidences = dst.getList<float>("Confidences");
        for (int i = 0; i < n; i++) {
            mx = 0.0;
            mxIdx = -1;

            for (int j = 0; j < nRegions; j++) {
                if (fabs(regionVecs(i,j)) > mx) {
                    mx = fabs(regionVecs(i,j));
                    mxIdx = j;
                }
            }

            Rect curRect = rects[i];
            midX[mxIdx] += ((float)curRect.x + (float)curRect.width  / 2.0);
            midY[mxIdx] += ((float)curRect.y + (float)curRect.height / 2.0);
            avgWidth[mxIdx]  += (float) curRect.width;
            avgHeight[mxIdx] += (float) curRect.height;
            confs[mxIdx] += confidences[i];
            cnts[mxIdx]++;
        }

        QList<Rect> consolidatedRects;
        QList<float> consolidatedConfidences;
        for (int i = 0; i < nRegions; i++) {
            float cntF = (float) cnts[i];
            if (cntF > 0) {
                int x = qRound((midX[i] / cntF) - (avgWidth[i] / cntF) / 2.0);
                int y = qRound((midY[i] / cntF) - (avgHeight[i] / cntF) / 2.0);
                int w = qRound(avgWidth[i] / cntF);
                int h = qRound(avgHeight[i] / cntF);
                consolidatedRects.append(Rect(x,y,w,h));
                consolidatedConfidences.append(confs[i] / cntF);
            }
        }

        delete [] midX;
        delete [] midY;
        delete [] avgWidth;
        delete [] avgHeight;
        delete [] confs;
        delete [] cnts;

        dst.setRects(consolidatedRects);
        dst.setList<float>("Confidences", consolidatedConfidences);
    }
};

BR_REGISTER(Transform, ConsolidateDetectionsTransform)

} // namespace br

#include "metadata/consolidatedetections.moc"


#include <k52/optimization/hleborodov_rosenbrock_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <fstream>
#include <iostream>

#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;

namespace
{
const double kArgumentsIncreaseValue = 1.5;
const double kArgumentsDecreaseValue = -0.5;
}

namespace k52
{
namespace optimization
{

HleborodovRosenbrockMethod::HleborodovRosenbrockMethod(double precision, size_t max_iteration_number,
    double first_step, double max_step)
    : precision_(precision)
    , arguments_increase_(kArgumentsIncreaseValue)
    , arguments_decrease_(kArgumentsDecreaseValue)
    , first_step_(first_step)
    , max_step_(max_step)
    , max_iteration_number_(max_iteration_number)
{
}

HleborodovRosenbrockMethod* HleborodovRosenbrockMethod::Clone() const
{
    return new HleborodovRosenbrockMethod(precision_, max_iteration_number_, first_step_, max_step_);
}

std::string HleborodovRosenbrockMethod::get_name() const
{
    return "Hleborodov Rosenbrock Method";
}

#ifdef BUILD_WITH_MPI
void HleborodovRosenbrockMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, first_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
}

void HleborodovRosenbrockMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, first_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
}
#endif

vector<double> HleborodovRosenbrockMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> arguments = initial_parameters;
    dimension_ = arguments.size();
    vector<double> next_step_arguments(arguments);

    InitializeBasisAndSteps();

    for (size_t i = 1; i <= max_iteration_number_; i++)
    {
        // Research of next arguments, that minimize function
        MakeStep(next_step_arguments);

        if (IsExitCriteriaFulfilled(next_step_arguments, arguments))
        {
            break;
        }

        // Check number of iterations
        if (i == max_iteration_number_)
        {
            std::cout << " Minimun has not found";
        }

        // Make new basis by Gramm-Shmidt method
        CreateNewBasis();
        arguments = next_step_arguments;
    }

    return next_step_arguments;
}

void HleborodovRosenbrockMethod::InitializeBasisAndSteps()
{
    steps_array_ = vector<double> (dimension_,first_step_);

    basis_ = vector< vector<double> > (dimension_, vector<double> (dimension_, 0));

    // Initialize basis is Decart coordinate system
    for(size_t i = 0; i<dimension_; i++)
    {
        basis_[i][i] = 1;
    }
}

bool HleborodovRosenbrockMethod::IsExitCriteriaFulfilled(
    const vector<double>& arguments,
    const vector<double>& previous_step_arguments)
{
    bool escape = true;
    // check arguments increases to escape
    for(size_t j = 0; j<dimension_; j++)
    {
        if(std::abs(arguments[j] - previous_step_arguments[j]) > precision_)
        {
            escape = false;
        }
    }
    return escape;
}

void HleborodovRosenbrockMethod::MakeStep(vector<double> &arguments)
{
    double start_function_value = CountObjectiveFunctionValueToMinimize(arguments);

    vector<double> tested_arguments(arguments);
    vector<double> tested_arguments2(arguments);

    /// Sort steps_array_ by descending for beter method speed
    std::sort(steps_array_.begin(), steps_array_.end(), std::greater<double>());

    for(size_t i = 0; i<dimension_; i++)
    {
        // make test step to minimize function in current direction
        // make it by each of coordinates
        MakeStepPerCoordinate(i, &tested_arguments);

        double counted_value = CountObjectiveFunctionValueToMinimize(tested_arguments);

        if(start_function_value > counted_value)
        {
            steps_array_[i] *= arguments_increase_;
        }
        else
        {
            steps_array_[i] *= arguments_decrease_;
        }

        CorrectStep(i);

        MakeStepPerCoordinate(i, &tested_arguments2);

        tested_arguments = arguments;
    }

    arguments = tested_arguments2;
}

void HleborodovRosenbrockMethod::MakeStepPerCoordinate(int basis_index, vector<double>* arguments)
{
    for(size_t j = 0; j<dimension_; j++)
    {
        (*arguments)[j] += steps_array_[basis_index]*basis_[basis_index][j];
    }
}

void HleborodovRosenbrockMethod::CreateNewBasis()
{
    vector< vector<double> > matrix_of_coeff(dimension_, vector<double> (dimension_, 0));
    vector< vector<double> > unnormalized_basis(dimension_, vector<double> (dimension_, 0));

    for(size_t j = 0; j<dimension_; j++)
    {
        for(size_t i = j; i<dimension_; i++)
        {
            for(size_t k = 0; k<dimension_; k++)
            {
                matrix_of_coeff[j][k] += steps_array_[i]*basis_[i][k];
            }
        }
    }

    for(size_t i = 0; i<dimension_; i++)
    {
        for(size_t j = 0; j<dimension_; j++)
        {
            unnormalized_basis[i][j] = matrix_of_coeff[i][j];
        }
        basis_[0][i] = matrix_of_coeff[0][i];
    }

    NormalizeVector(basis_[0]);
    double temp;

    for(size_t j = 1; j<dimension_; j++)
    {
        for(size_t i = 0; i<j; i++)
        {
            temp = ScalarComposition(matrix_of_coeff[j],basis_[i]);
            for(size_t k = 0; k<dimension_; k++)
            {
                unnormalized_basis[j][k] -= temp*basis_[i][k];
            }
        }

        for(size_t k = 0; k<dimension_; k++)
        {
            basis_[j][k] = unnormalized_basis[j][k];
        }
        NormalizeVector(basis_[j]);
    }
}

void HleborodovRosenbrockMethod::NormalizeVector(vector<double> &target_vector)
{
    double temp = 0;
    for(size_t i = 0; i<dimension_; i++)
    {
        temp += target_vector[i]*target_vector[i];
    }
    temp = sqrt(temp);
    for(size_t i = 0; i<dimension_; i++)
    {
        if(temp != 0)
        {
            target_vector[i] /= temp;
        }
    }
}

double HleborodovRosenbrockMethod::ScalarComposition(
    const vector<double>& first_vector,
    const vector<double>& second_vector)
{
    double scalar_composition = 0;
    for(size_t i = 0; i<dimension_; i++)
    {
        scalar_composition += first_vector[i]*second_vector[i];
    }
    return scalar_composition;
}

void HleborodovRosenbrockMethod::CorrectStep(size_t step_index)
{
    if( std::abs(steps_array_[step_index]) > max_step_)
    {
        steps_array_[step_index] =
            max_step_*steps_array_[step_index]/std::abs(steps_array_[step_index]);
    }
}

}/* namespace optimization */
}/* namespace k52 */


#include <boost/multiprecision/cpp_int.hpp>

int main()
{
  boost::multiprecision::uint128_t a(123);
  int b = 123;
  if (a != b) {
    return 1;
  }

  return 0;
}


/*
 * simplification.cpp
 *
 *  Created on: Nov 22, 2010
 *      Author: stewie
 */

#include <iostream>
#include <utility>
#include <vector>
#include <map>
#include <deque>
#include <set>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/algorithm/find.hpp>

#include <BnSimulator/core/MutableBooleanNetwork.hpp>
#include <BnSimulator/core/simplification.hpp>

using namespace std;
using namespace boost;

namespace bn {

typedef adjacency_list<listS, listS, bidirectionalS, BooleanFunction> TempGraph;
typedef TempGraph::vertex_descriptor vertex;

void simplify_node(const vertex v, TempGraph& g, set<vertex>& q, set<vertex>& toRemove) {
	if (toRemove.count(v)) {
		//cerr << "Vertex " << v << " should be removed" << endl;
		return;
	}
	//cerr << "Examining node " << v << " [" << g[v] << "]" << endl;
	if (out_degree(v, g) == 0) {
		//cerr << "has no outputs" << endl;
		TempGraph::inv_adjacency_iterator vi, vend;
		for (tie(vi, vend) = inv_adjacent_vertices(v, g); vi != vend; ++vi)
			q.insert(*vi);
		clear_vertex(v, g);
		//remove_vertex(v, g);
		toRemove.insert(v);
	} else {
		BooleanFunction& f = g[v];
		pair<bool, bool> constant = f.isConstant();
		if (constant.first) {
			//cerr << "is constantly " << constant.second << endl;
			TempGraph::inv_adjacency_iterator vi, vend;
			for (tie(vi, vend) = inv_adjacent_vertices(v, g); vi != vend; ++vi)
				if (*vi != v) // no self-edges
					q.insert(*vi);
			TempGraph::adjacency_iterator ai, aend;
			for (tie(ai, aend) = adjacent_vertices(v, g); ai != aend; ++ai) {
				if (*ai != v) { // no self-edges
					/*size_t xi = 0;
					for (tie(vi, vend) = inv_adjacent_vertices(*ai, g); vi
							!= vend; ++vi, ++xi) {
						if (*vi == v)
							break;
					}*/
					const size_t xi = find(inv_adjacent_vertices(*ai, g) | adaptors::indexed(0), v).index();
					g[*ai] = g[*ai].clamped(xi, constant.second);
					q.insert(*ai);
				}
			}
			clear_vertex(v, g);
			//remove_vertex(v, g);
			toRemove.insert(v);
		} else {
			size_t xi = 0;
			TempGraph::in_edge_iterator ei, eend;
			// inputs are ordered from LSB to MSB
			for (tie(ei, eend) = in_edges(v, g); ei != eend;){
				if (g[v].isInfluent(xi)) {
					++ei;
					++xi;
				} else {
					//cerr << "Variable " << xi << " for " << g[v] << " is not influent...";
					g[v] = g[v].clamped(xi, true); // true or false doesn't matter
					//cerr << g[v] << endl;
					TempGraph::in_edge_iterator temp = ei++; // avoid iterator invalidation
					q.insert(opposite(*temp, v, g)); // will later check if parent has out degree > 0
					remove_edge(*temp, g);
				}
			}
		}
	}
}

struct MakeFunction {
	const MutableBooleanNetwork::Network& from;
	TempGraph& to;
	MakeFunction(const MutableBooleanNetwork::Network& from, TempGraph& to) :
		from(from), to(to) {
	}

	void operator()(MutableBooleanNetwork::Network::vertex_descriptor u,
			TempGraph::vertex_descriptor v) const {
		to[v] = BooleanFunction(from[u]);
	}
};

struct GetTruthTable {
	const TempGraph& from;
	MutableBooleanNetwork::Network& to;
	GetTruthTable(const TempGraph& from, MutableBooleanNetwork::Network& to) :
		from(from), to(to) {
	}

	void operator()(TempGraph::vertex_descriptor u,
			MutableBooleanNetwork::Network::vertex_descriptor v) const {
		to[v] = from[u].truthTable();
	}
};

MutableBooleanNetwork::Network simplify(const MutableBooleanNetwork& net) {
	TempGraph g;
	copy_graph(net.topology(), g, vertex_copy(MakeFunction(net.topology(), g)));

	TempGraph::vertex_iterator vi, vend;
	set<vertex> q, toRemove;
	for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
		simplify_node(*vi, g, q, toRemove);
	}
	while (!q.empty()) {
		set<vertex>::const_iterator it = q.begin();
		//vertex v = *q.begin();
		simplify_node(*it, g, q, toRemove);
		q.erase(it);
	}
	for(set<vertex>::const_iterator it = toRemove.begin(); it != toRemove.end(); ++it)
		remove_vertex(*it, g);

	map<vertex, size_t> vertexMap;
	size_t id = 0;
	for (tie(vi, vend) = vertices(g); vi != vend; ++vi, ++id) {
		vertexMap[*vi] = id;
	}
	MutableBooleanNetwork::Network topology;
	copy_graph(
			g,
			topology,
			vertex_copy(GetTruthTable(g, topology)). vertex_index_map(
					make_assoc_property_map(vertexMap)));
	return topology;
}

} // namespace bn


// Copyright 2016-2020 Michel Koenen (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * cObjectPath.hpp
 *
 *  Created on: Feb 18, 2016
 *      Author: Michel Koenen
 */

#ifndef COBJECTPATH_HPP_
#define COBJECTPATH_HPP_

#include "falconsCommon.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

//#include "int/types/cPositionTypes.hpp"
//#include "int/types/cRobotLocationTypes.hpp"
//#include "int/types/cBallLocationTypes.hpp"

typedef boost::geometry::model::point<float, 2, boost::geometry::cs::cartesian> Point;
typedef boost::geometry::model::segment<Point> Segment;

class cObjectPath
{
	public:
		cObjectPath(Point3D &location, Vector3D &speedvector);
		bool intersectCheck( Point2D lineStart, Point2D lineEnd , Point2D &intersectCoord);
		bool intersectCheck( Point2D lineStart, Point2D lineEnd );
		Point2D getPerpendicularInterceptionPoint( Point2D otherObject, bool &valid);
		float calcY(float forX);
		float calcX(float forY);
		float getSlope();
		float getYIntercept();
		Point2D getTowardsCoordinate();
		float getX();
		float getY();
		float getSpeedX();
		float getSpeedY();

	private:
		float slope;
		float intercept;
		Point2D myPosition;
		Point2D mySpeed;
};

#endif /* COBJECTPATH_HPP_ */


#pragma once

#include <Eigen/Eigen>

namespace py4dgeo {

/* Declare the most important types used in py4dgeo */

/** @brief The C++ type for a point cloud
 *
 * Point clouds are represented as (nx3) matrices from the Eigen library.
 *
 * The choice of this type allows us both very efficient implementation
 * of numeric algorithms using the Eigen library, as well as no-copy
 * interoperability with numpy's multidimensional arrays.
 */
using EigenPointCloud =
  Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>;

/** @brief A non-const reference type for passing around @ref EigenPointCloud
 *
 * You should use this in function signatures that accept a point cloud
 * as a parameter and need to modify the point cloud.
 */
using EigenPointCloudRef = Eigen::Ref<EigenPointCloud>;

/** @brief A const reference type for passing around @ref EigenPointCloud
 *
 * You should use this in function signatures that accept a read-only
 * point cloud.
 */
using EigenPointCloudConstRef = const Eigen::Ref<const EigenPointCloud>&;

/** @brief The C++ type to represent a set of normal vectors on a point cloud
 *
 * In contrast to point clouds, these use double precision.
 */
using EigenNormalSet =
  Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;

/** @brief A mutable reference to a set of normal vectors on a point cloud */
using EigenNormalSetRef = Eigen::Ref<EigenNormalSet>;

/** @brief An immutable reference to a set of normal vectors on a point cloud */
using EigenNormalSetConstRef = const Eigen::Ref<const EigenNormalSet>&;

/** @brief The type used for point cloud indices */
using IndexType = Eigen::Index;

/** @brief Return structure for the uncertainty of the distance computation
 *
 * This structured type is used to describe the uncertainty of point cloud
 * distance at a single corepoint. It contains the level of detection,
 * the standard deviations within both point clouds and the number of sampled
 * points.
 */
struct DistanceUncertainty
{
  double lodetection;
  double stddev1;
  IndexType num_samples1;
  double stddev2;
  IndexType num_samples2;
};

/** @brief The variable-sized vector type used for distances */
using DistanceVector = std::vector<double>;

/** @brief The variable-sized vector type used for uncertainties */
using UncertaintyVector = std::vector<DistanceUncertainty>;

/** @brief An enumerator for py4dgeo's memory policy
 *
 * This is used and documented through its Python binding equivalent.
 */
enum class MemoryPolicy
{
  STRICT = 0,
  MINIMAL = 1,
  COREPOINTS = 2,
  RELAXED = 3
};

}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

#include "arlib/arlib.hpp"

#include <iostream>
#include <string>
#include <vector>

// Create type-aliases for the Graph type
using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
                                    boost::bidirectionalS, boost::no_property,
                                    boost::property<boost::edge_weight_t, int>>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

std::vector<arlib::Path<Graph>> get_alternative_routes(Graph const &G, Vertex s,
                                                       Vertex t) {
  // Make output MultiPredecessorMap
  auto predecessors = arlib::multi_predecessor_map<Vertex>{};

  int k = 3;                                       // Nb alternative routes
  double theta = 0.5;                              // Overlapping threshold
  auto weight = boost::get(boost::edge_weight, G); // Get Edge WeightMap

  arlib::onepass_plus(G, weight, predecessors, s, t, k, theta);
  auto alt_routes = arlib::to_paths(G, predecessors, weight, s, t);
  return alt_routes;
}

void print_path(arlib::Path<Graph> const &path,
                std::vector<std::string> const &name) {
  using namespace boost;

  for (auto [v_it, v_end] = vertices(path); v_it != v_end; ++v_it) {
    for (auto [e_it, e_end] = out_edges(*v_it, path); e_it != e_end; ++e_it) {
      std::cout << name[source(*e_it, path)] << " -- "
                << name[target(*e_it, path)] << "\n";
    }
  }
}

int main() {
  // Make convenient labels for the vertices
  enum { S, N1, N2, N3, N4, N5, T };
  const long unsigned num_vertices = T;
  const auto name =
      std::vector<std::string>{"s", "n1", "n2", "n3", "n4", "n5", "t"};

  // Writing out the edges in the graph
  const auto edges = std::vector<std::pair<int, int>>{
      {S, N1},  {N1, S},  {S, N2},  {N2, S},  {S, N3},  {N3, S},
      {N1, T},  {T, N1},  {N3, N1}, {N1, N3}, {N3, N5}, {N5, N3},
      {N3, N2}, {N2, N3}, {N3, N4}, {N4, N3}, {N2, N4}, {N4, N2},
      {N5, T},  {T, N5},  {N5, N4}, {N4, N5}, {N4, T},  {T, N4}};

  const auto weights = std::vector<int>{6, 6, 4, 4, 3, 3, 6, 6, 2, 2, 3, 3,
                                        3, 3, 5, 5, 5, 5, 2, 2, 1, 1, 2, 2};
  auto G = Graph{edges.begin(), edges.end(), weights.begin(), num_vertices};

  //=-------------------------------------------------------------------------=

  auto alt_routes = get_alternative_routes(G, S, T);

  for (auto const &route : alt_routes) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  return 0;
}


#include <stdio.h>
#include <sys/time.h>
#include <iostream>

#include <Eigen/Dense>

#include "hw/car/stm32rs232.h"
#include "hw/imu/imu.h"
#include "hw/input/js.h"
#include "inih/cpp/INIReader.h"

#include "controlloop/fit.h"
#include "controlloop/pid.h"
#include "controlloop/motorcontrol.h"

const int STEER_LIMIT_LOW = -85;
const int STEER_LIMIT_HIGH = 127;

const float WHEEL_DIAMETER = 0.065;
const float DRIVE_RATIO = 84./25. * 2.1;  // 84t spur, 25t pinion, 2.1 final drive
const float MOTOR_POLES = 3;  // brushless sensor counts per motor revolution
const float V_SCALE = WHEEL_DIAMETER*M_PI / DRIVE_RATIO / MOTOR_POLES;

using Eigen::Vector3f;
using Eigen::Matrix3f;

class CFIR : public JoystickListener, public ControlListener {
 public:
  CFIR(IMU *imu, JoystickInput *js) {
    imu_ = imu;
    js_ = js;
    js_throttle_ = 0;
    js_steering_ = 0;
    exit_ = false;
    recording_ = NULL;
    mode_ = Control;
  }
  virtual ~CFIR() {}

  virtual void OnButtonPress(char button) {
    switch (button) {
      case '+':  // plus button; start recording
        StartRecording();
        break;
      case '-':  // minus button; stop recording
        StopRecording();
        break;
      case 'L':
        mode_ = SysId;
        break;
      case 'R':
        mode_ = Control;
        break;
    }
  }

  virtual void OnAxisMove(int axis, int16_t value) {
    switch (axis) {
      case 1:  // left stick y axis
        js_throttle_ = -value;
        break;
      case 2:  // right stick x axis
        js_steering_ = value;
        break;
    }
  }

  void StartRecording() {
    if (recording_) {
      StopRecording();
    }
    char fnamebuf[256];
    snprintf(fnamebuf, sizeof(fnamebuf), "log-%ld.txt", time(NULL));
    recording_ = fopen(fnamebuf, "w");
    if (recording_ == NULL) {
      perror(fnamebuf);
      return;
    }
    // setlinebuf(recording_);
    fprintf(recording_, "# dt u_esc u_steer dw wperiod gx gy gz ax ay az\n");
    fprintf(stderr, "recording %s\n", fnamebuf);
  }

  void StopRecording() {
    if (recording_ == NULL) {
      return;
    }
    fclose(recording_);
    recording_ = NULL;
    fprintf(stderr, "stopped recording\n");
  }

  int16_t Throttle() { return js_throttle_; }
  int16_t Steering() { return js_steering_; }

  int16_t js_throttle_;
  int16_t js_steering_;
  bool exit_;
  FILE *recording_;

  bool was_learning = false;

  IMU *imu_;
  JoystickInput *js_;

  enum {
    SysId = 0,
    Control = 1
  } mode_;

  int n = 0;

  virtual bool OnControlFrame(CarHW *car, float dt) {
    Vector3f accel, gyro;

    js_->ReadInput(this);
    imu_->ReadIMU(&accel, &gyro);

    float ds, v;
    car->GetWheelMotion(&ds, &v);

    float u_esc = 0;
    float u_steer = 0;
    switch (mode_) {
      case SysId: {
        if (!was_learning) {
          fprintf(stderr, "starting system identification\n");
        }
#if 0
          if (v > 0) {
            // separate brake_id is unsafe for now
            motor_id.AddObservation(v, 1, u_esc, dt);
            steer_id.AddObservation(gyro[2], v, u_steer, dt);
          }
#endif
        // hold PID loops in reset while learning
        // motor_pi.Reset();
        // steer_pi.Reset();
        was_learning = true;
        break;
      }
      case Control: {
        float v_target = 8 * Throttle() / 32767.0;
        if (v_target <= 0) {
          // don't wind up the integrator! just stop the car.
          v_target = 0;
        }
        // turn left -> positive gyro rate, hence negative sign
        float k_target = -Steering() / 32768.0;  // +-1m turning radius

        if (was_learning) {
          /*
          auto km = motor_id.Solve();
          fprintf(stderr, "motor sysid: %f %f %f %f %f\n", km[0], km[1], km[2],
                  km[3], km[4]);
          auto k = steer_id.Solve();
          fprintf(stderr, "steer sysid: %f %f %f %f\n", k[0], k[1], k[2], k[3]);
          */
          was_learning = false;
        }

#if 0
        auto k = motor_id.Solve();
        motor_pi.SetK(1, 0, 0);
        u_esc = motor_pi.Control(v_target - v, dt);
        k = steer_id.Solve();
        steer_pi.SetK(1, 0, 0);
        float kerr = 0;
        if (v > 0.5) {  // must be going at least 0.5 m/s for closed loop yaw
                        // control
          kerr = k_target - gyro[2] / v;
        } else {
          // reset wind-up at low speeds
          steer_pi.Reset();
        }
        u_steer = clipi16(k_target * k[0] + k[3] + steer_pi.Control(kerr, dt),
                          STEER_LIMIT_LOW, STEER_LIMIT_HIGH);
#endif
      } break;
    }

    if (!car->SetControls(n >> 4, u_esc, u_steer)) {
      fprintf(stderr, "SetControls returned false?\n");
    }

    if (recording_) {
      fprintf(recording_, "%0.3f %d %d %f %f %f %f %f %f %f %f %f %f\n",
          dt, Throttle(), Steering(), u_esc, u_steer, ds, v,
          gyro[0], gyro[1], gyro[2],
          accel[0], accel[1], accel[2]);
    }

    n++;
    return !exit_;
  }
};

/*
void controltest() {
  float target_v = (n & 256) ? 200 : 400;

  // observe effect of previous input and update model
  float v = 0;
  if (wperiod != 0) {
    v = 1.0e6 / wperiod;
  }

  // loop bandwidth
  const float Wc = 0.5 * 6.28;

  // initial PID constants just to kick-start it; they will quickly be overwritten
  float Kp = 0.05;
  float Ki = 10*0.05;
  float Kd = 0;

  if (v != 0 && last_v != 0) {
    float dvdt = (v - last_v) / dt;
    float d2vdt2 = (dvdt - last_dvdt) / dt;
    //Vector3f X(last_u[0], last_u[0]*v, v, 1);
    Vector3f X(v, dvdt, d2vdt2);
    //XTY *= 0.97;
    //XTX *= 0.97;

    XTY += X * last_u[0];
    XTX += X * X.transpose();

    Eigen::LDLT<Matrix3f> ALDLT = XTX.ldlt();
    Vector3f k = ALDLT.solve(XTY);
    Ki = Wc * k[0];
    Kp = Wc * k[1];
    Kd = Wc * k[2];
    printf("c1=%f c2=%f c3=%f Ki=%f Kp=%f Kd=%f\n", k[0], k[1], k[2], Ki, Kp, Kd);

    last_dvdt = dvdt;
  }

#if 0
  const float Kp = 5;
  float target_vdot = Kp * (target_v - v);
  if (k[0] < 1) k[0] = 1;
  if (k[1] > 0) k[1] = 0;
  if (k[2] > 0) k[2] = 0;
  if (k[3] > 0) k[3] = 0;
  float uu = (target_vdot - k[2]*v - k[3]) / (k[0] + k[1]*v);
#if 0
  float uu = 0;
  if (n & 32) {
    uu += 32 - (n & 31);
  } else {
    uu += n & 31;
  }
#endif
#endif

  float err = target_v - v;
  float derr = (err - last_err) / dt;
  ierr += err * dt;
  last_err = err;
  float uu = Kp*err + Ki*ierr + Kd*derr;

  int8_t u = clipf(uu, -127, 127);
  // if (target_vdot > 0 && u < 10) u = 10;
  if (!car.SetControls(n >> 4, u, 0)) {
    fprintf(stderr, "SetControls returned false?\n");
  }
  // printf("%0.3f wpos:%d wperiod:%d k:%f %f %f %f v:%f tv:%f vdot:%f uu:%f u:%d lu:%d\n", dt, wpos, wperiod, k[0], k[1], k[2], k[3], v, target_v, target_vdot, uu, u, last_u[0]);
  printf("%0.3f wpos:%d wperiod:%d v:%f tv:%f uu:%f ierr:%f u:%d lu:%d\n", dt, wpos, wperiod, v, target_v, uu, ierr, u, last_u[0]);

  n++;
  // printf("   %d %d %d %d\n", last_u[0], last_u[1], last_u[2], last_u[3]);
  for (int i = 0; i < ESC_DELAY-1; i++) {
    last_u[i] = last_u[i+1];
  }
  last_v = v;
#endif
}
*/

int main(int argc, char *argv[]) {
  JoystickInput js;
  INIReader ini("cycloid.ini");
  I2C i2c;
  IMU *imu = IMU::GetI2CIMU(i2c, ini);
  CarHW *car = CarHW::GetCar(&i2c, ini);

  if (!i2c.Open()) {
    fprintf(stderr, "need to enable i2c in raspi-config, probably\n");
    return 1;
  }

  if (!js.Open(ini)) {
    return 1;
  }
  if (!car->Init()) {
    return 1;
  }
  if (!imu->Init()) {
    return 1;
  }

  // for fwd, use model dv/dt = [k1 k2 k3] . [u v 1]
  // prior [1 0.1 -1]

  // for brake, use different [k1 k2 k3] but same model

  uint16_t wpos, wperiod;
  uint16_t last_wpos = 0;

  CFIR ir(imu, &js);

  setlinebuf(stdout);

  car->RunMainLoop(&ir);
}


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

  @copyright 2015 NumScale SAS

  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_OPERATOR_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-trigonometric Trigonometric functions

    Algorithms for computing scalar and SIMD version of
    trigonometric and inverse trigonometric functions.
  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-trigonometric Trigonometric Callable Objects
    Callable objects version of @ref group-trigonometric

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-trigonometric section.
  **/
} }

#include <boost/simd/function/acosd.hpp>
#include <boost/simd/function/acos.hpp>
#include <boost/simd/function/acospi.hpp>
#include <boost/simd/function/acotd.hpp>
#include <boost/simd/function/acot.hpp>
#include <boost/simd/function/acscd.hpp>
#include <boost/simd/function/acsc.hpp>
#include <boost/simd/function/asecd.hpp>
#include <boost/simd/function/asec.hpp>
#include <boost/simd/function/asind.hpp>
#include <boost/simd/function/asin.hpp>
#include <boost/simd/function/asinpi.hpp>
#include <boost/simd/function/atan2d.hpp>
#include <boost/simd/function/atan2.hpp>
#include <boost/simd/function/atand.hpp>
#include <boost/simd/function/atan.hpp>
#include <boost/simd/function/atanpi.hpp>
#include <boost/simd/function/cosd.hpp>
#include <boost/simd/function/cos.hpp>
#include <boost/simd/function/cospi.hpp>
#include <boost/simd/function/cotd.hpp>
#include <boost/simd/function/cot.hpp>
#include <boost/simd/function/cotpi.hpp>
#include <boost/simd/function/cscd.hpp>
#include <boost/simd/function/csc.hpp>
#include <boost/simd/function/cscpi.hpp>
#include <boost/simd/function/indeg.hpp>
#include <boost/simd/function/inrad.hpp>
#include <boost/simd/function/rem_2pi.hpp>
#include <boost/simd/function/rem_pio2_cephes.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/rem_pio2_straight.hpp>
#include <boost/simd/function/secd.hpp>
#include <boost/simd/function/sec.hpp>
#include <boost/simd/function/secpi.hpp>
#include <boost/simd/function/sinc.hpp>
#include <boost/simd/function/sincosd.hpp>
#include <boost/simd/function/sincos.hpp>
#include <boost/simd/function/sincospi.hpp>
#include <boost/simd/function/sincpi.hpp>
#include <boost/simd/function/sind.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/function/sinpi.hpp>
#include <boost/simd/function/tand.hpp>
#include <boost/simd/function/tan.hpp>
#include <boost/simd/function/tanpi.hpp>

#endif


#ifndef get_feasible_steps_hpp
#define get_feasible_steps_hpp

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

double getSmallestPositiveRealQuadRoot(double a, double b, double c,
    double tol);

void computeInjectiveStepSize_2d(const MatrixXi& F,
    const MatrixXd& x,
    const VectorXd& p,
    double tol,
    double slackness,
    double* output);

double getSmallestPositiveRealCubicRoot(double a, double b, double c, double d,
    double tol);

void computeInjectiveStepSize_3d(const MatrixXi& F,
    const MatrixXd& x,
    const VectorXd& p,
    double tol,
    double slackness,
    double* output);

void computeInjectiveStepSize_3d(const RowVector3d& xx1,
    const RowVector3d& xx2,
    const RowVector3d& xx3,
    const RowVector3d& xx4,
    const Vector3d& pp1,
    const Vector3d& pp2,
    const Vector3d& pp3,
    const Vector3d& pp4,
    double tol,
    double& output);

#endif