// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "nrosy.h"

#include <igl/copyleft/comiso/nrosy.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/edge_topology.h>
#include <igl/per_face_normals.h>

#include <iostream>
#include <fstream>

#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <queue>

#include <gmm/gmm.h>
#include <CoMISo/Solver/ConstrainedSolver.hh>
#include <CoMISo/Solver/MISolver.hh>
#include <CoMISo/Solver/GMM_Tools.hh>

namespace igl
{
namespace copyleft
{

namespace comiso
{
class NRosyField
{
public:
  // Init
  IGL_INLINE NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F);

  // Generate the N-rosy field
  // N degree of the rosy field
  // roundseparately: round the integer variables one at a time, slower but higher quality
  IGL_INLINE void solve(const int N = 4);

  // Set a hard constraint on fid
  // fid: face id
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintHard(const int fid, const Eigen::Vector3d& v);

  // Set a soft constraint on fid
  // fid: face id
  // w: weight of the soft constraint, clipped between 0 and 1
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintSoft(const int fid, const double w, const Eigen::Vector3d& v);

  // Set the ratio between smoothness and soft constraints (0 -> smoothness only, 1 -> soft constr only)
  IGL_INLINE void setSoftAlpha(double alpha);

  // Reset constraints (at least one constraint must be present or solve will fail)
  IGL_INLINE void resetConstraints();

  // Return the current field
  IGL_INLINE Eigen::MatrixXd getFieldPerFace();

  // Return the current field (in Ahish's ffield format)
  IGL_INLINE Eigen::MatrixXd getFFieldPerFace();

  // Compute singularity indexes
  IGL_INLINE void findCones(int N);

  // Return the singularities
  IGL_INLINE Eigen::VectorXd getSingularityIndexPerVertex();

private:

  // Compute angle differences between reference frames
  IGL_INLINE void computek();

  // Remove useless matchings
  IGL_INLINE void reduceSpace();

  // Prepare the system matrix
  IGL_INLINE void prepareSystemMatrix(const int N);

  // Solve without roundings
  IGL_INLINE void solveNoRoundings();

  // Solve with roundings using CoMIso
  IGL_INLINE void solveRoundings();

  // Round all p to 0 and fix
  IGL_INLINE void roundAndFixToZero();

  // Round all p and fix
  IGL_INLINE void roundAndFix();

  // Convert a vector in 3d to an angle wrt the local reference system
  IGL_INLINE double convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v);

  // Convert an angle wrt the local reference system to a 3d vector
  IGL_INLINE Eigen::Vector3d convertLocalto3D(unsigned fid, double a);

  // Compute the per vertex angle defect
  IGL_INLINE Eigen::VectorXd angleDefect();

  // Temporary variable for the field
  Eigen::VectorXd angles;

  // Hard constraints
  Eigen::VectorXd hard;
  std::vector<bool> isHard;

  // Soft constraints
  Eigen::VectorXd soft;
  Eigen::VectorXd wSoft;
  double          softAlpha;

  // Face Topology
  Eigen::MatrixXi TT, TTi;

  // Edge Topology
  Eigen::MatrixXi EV, FE, EF;
  std::vector<bool> isBorderEdge;

  // Per Edge information
  // Angle between two reference frames
  Eigen::VectorXd k;

  // Jumps
  Eigen::VectorXi p;
  std::vector<bool> pFixed;

  // Mesh
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;

  // Normals per face
  Eigen::MatrixXd N;

  // Singularity index
  Eigen::VectorXd singularityIndex;

  // Reference frame per triangle
  std::vector<Eigen::MatrixXd> TPs;

  // System stuff
  Eigen::SparseMatrix<double> A;
  Eigen::VectorXd b;
  Eigen::VectorXi tag_t;
  Eigen::VectorXi tag_p;

};

} // NAMESPACE COMISO
} // NAMESPACE COPYLEFT
} // NAMESPACE IGL

igl::copyleft::comiso::NRosyField::NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F)
{
  using namespace std;
  using namespace Eigen;

  V = _V;
  F = _F;

  assert(V.rows() > 0);
  assert(F.rows() > 0);

  // Generate topological relations
  igl::triangle_triangle_adjacency(F,TT,TTi);
  igl::edge_topology(V,F, EV, FE, EF);

  // Flag border edges
  isBorderEdge.resize(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    isBorderEdge[i] = (EF(i,0) == -1) || ((EF(i,1) == -1));

  // Generate normals per face
  igl::per_face_normals(V, F, N);

  // Generate reference frames
  for(unsigned fid=0; fid<F.rows(); ++fid)
  {
    // First edge
    Vector3d e1 = V.row(F(fid,1)) - V.row(F(fid,0));
    e1.normalize();
    Vector3d e2 = N.row(fid);
    e2 = e2.cross(e1);
    e2.normalize();

    MatrixXd TP(2,3);
    TP << e1.transpose(), e2.transpose();
    TPs.push_back(TP);
  }

  // Alloc internal variables
  angles = VectorXd::Zero(F.rows());
  p = VectorXi::Zero(EV.rows());
  pFixed.resize(EV.rows());
  k = VectorXd::Zero(EV.rows());
  singularityIndex = VectorXd::Zero(V.rows());

  // Reset the constraints
  resetConstraints();

  // Compute k, differences between reference frames
  computek();

  softAlpha = 0.5;
}

void igl::copyleft::comiso::NRosyField::setSoftAlpha(double alpha)
{
  assert(alpha >= 0 && alpha < 1);
  softAlpha = alpha;
}

void igl::copyleft::comiso::NRosyField::prepareSystemMatrix(const int N)
{
  using namespace std;
  using namespace Eigen;

  double Nd = N;

  // Minimize the MIQ energy
  // Energy on edge ij is
  //     (t_i - t_j + kij + pij*(2*pi/N))^2
  // Partial derivatives:
  //   t_i: 2     ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //   t_j: 2     (-t_i + t_j - kij - pij*(2*pi/N)) = 0
  //   pij: 4pi/N ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //
  //          t_i      t_j         pij       kij
  // t_i [     2       -2           4pi/N      2    ]
  // t_j [    -2        2          -4pi/N     -2    ]
  // pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]

  // Count and tag the variables
  tag_t = VectorXi::Constant(F.rows(),-1);
  vector<int> id_t;
  int count = 0;
  for(unsigned i=0; i<F.rows(); ++i)
    if (!isHard[i])
    {
      tag_t(i) = count++;
      id_t.push_back(i);
    }

  unsigned count_t = id_t.size();

  tag_p = VectorXi::Constant(EF.rows(),-1);
  vector<int> id_p;
  for(unsigned i=0; i<EF.rows(); ++i)
  {
    if (!pFixed[i])
    {
      // if it is not fixed then it is a variable
      tag_p(i) = count++;
    }

    // if it is not a border edge,
    if (!isBorderEdge[i])
    {
      // and it is not between two fixed faces
      if (!(isHard[EF(i,0)] && isHard[EF(i,1)]))
      {
          // then it participates in the energy!
          id_p.push_back(i);
      }
    }
  }

  unsigned count_p = count - count_t;
  // System sizes: A (count_t + count_p) x (count_t + count_p)
  //               b (count_t + count_p)

  b = VectorXd::Zero(count_t + count_p);

  std::vector<Eigen::Triplet<double> > T;
  T.reserve(3 * 4 * count_p);

  for(unsigned r=0; r<id_p.size(); ++r)
  {
    int eid = id_p[r];
    int i = EF(eid,0);
    int j = EF(eid,1);
    bool isFixed_i = isHard[i];
    bool isFixed_j = isHard[j];
    bool isFixed_p = pFixed[eid];
    int row;
    // (i)-th row: t_i [     2       -2           4pi/N      2    ]
    if (!isFixed_i)
    {
      row = tag_t[i];
      if (isFixed_i) b(row) += -2               * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i]  , 2             ));
      if (isFixed_j) b(row) +=  2               * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j]  ,-2             ));
      if (isFixed_p) b(row) += -((4 * igl::PI)/Nd) * p[eid] ; else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],((4 * igl::PI)/Nd)));
      b(row) += -2 * k[eid];
      assert(hard[i] == hard[i]);
      assert(hard[j] == hard[j]);
      assert(p[eid] == p[eid]);
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (j)+1 -th row: t_j [    -2        2          -4pi/N     -2    ]
    if (!isFixed_j)
    {
      row = tag_t[j];
      if (isFixed_i) b(row) += 2               * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i]  , -2             ));
      if (isFixed_j) b(row) += -2              * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j] ,  2              ));
      if (isFixed_p) b(row) += ((4 * igl::PI)/Nd) * p[eid] ; else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],-((4 * igl::PI)/Nd)));
      b(row) += 2 * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (r*3)+2 -th row: pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]
    if (!isFixed_p)
    {
      row = tag_p[eid];
      if (isFixed_i) b(row) += -(4 * igl::PI)/Nd              * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i] ,   (4 * igl::PI)/Nd             ));
      if (isFixed_j) b(row) +=  (4 * igl::PI)/Nd              * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j] ,  -(4 * igl::PI)/Nd             ));
      if (isFixed_p) b(row) += -(2 * pow(((2*igl::PI)/Nd),2)) * p[eid] ;  else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],  (2 * pow(((2*igl::PI)/Nd),2))));
      b(row) += - (4 * igl::PI)/Nd * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }

  }

  A = SparseMatrix<double>(count_t + count_p, count_t + count_p);
  A.setFromTriplets(T.begin(), T.end());

  // Soft constraints
  bool addSoft = false;

  for(unsigned i=0; i<wSoft.size();++i)
    if (wSoft[i] != 0)
      addSoft = true;

  if (addSoft)
  {
    cerr << " Adding soft here: " << endl;
    cerr << " softAplha: " << softAlpha << endl;
    VectorXd bSoft = VectorXd::Zero(count_t + count_p);

    std::vector<Eigen::Triplet<double> > TSoft;
    TSoft.reserve(2 * count_p);

    for(unsigned i=0; i<F.rows(); ++i)
    {
      int varid = tag_t[i];
      if (varid != -1) // if it is a variable in the system
      {
        TSoft.push_back(Eigen::Triplet<double>(varid,varid,wSoft[i]));
        bSoft[varid] += wSoft[i] * soft[i];
      }
    }
    SparseMatrix<double> ASoft(count_t + count_p, count_t + count_p);
    ASoft.setFromTriplets(TSoft.begin(), TSoft.end());

//    ofstream s("/Users/daniele/As.txt");
//    for(unsigned i=0; i<TSoft.size(); ++i)
//      s << TSoft[i].row() << " " << TSoft[i].col() << " " << TSoft[i].value() << endl;
//    s.close();

//    ofstream s2("/Users/daniele/bs.txt");
//    for(unsigned i=0; i<bSoft.rows(); ++i)
//      s2 << bSoft(i) << endl;
//    s2.close();

    // Stupid Eigen bug
    SparseMatrix<double> Atmp (count_t + count_p, count_t + count_p);
    SparseMatrix<double> Atmp2(count_t + count_p, count_t + count_p);
    SparseMatrix<double> Atmp3(count_t + count_p, count_t + count_p);

    // Merge the two part of the energy
    Atmp = (1.0 - softAlpha)*A;
    Atmp2 = softAlpha * ASoft;
    Atmp3 = Atmp+Atmp2;

    A = Atmp3;
    b = b*(1.0 - softAlpha) + bSoft * softAlpha;
  }

//  ofstream s("/Users/daniele/A.txt");
//  for (int k=0; k<A.outerSize(); ++k)
//    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
//    {
//      s << it.row() << " " << it.col() << " " << it.value() << endl;
//    }
//  s.close();
//
//  ofstream s2("/Users/daniele/b.txt");
//  for(unsigned i=0; i<b.rows(); ++i)
//    s2 << b(i) << endl;
//  s2.close();
}

void igl::copyleft::comiso::NRosyField::solveNoRoundings()
{
  using namespace std;
  using namespace Eigen;

  // Solve the linear system
  SimplicialLDLT<SparseMatrix<double> > solver;
  solver.compute(A);
  VectorXd x = solver.solve(b);

  // Copy the result back
  for(unsigned i=0; i<F.rows(); ++i)
    if (tag_t[i] != -1)
      angles[i] = x(tag_t[i]);
    else
      angles[i] = hard[i];

  for(unsigned i=0; i<EF.rows(); ++i)
    if(tag_p[i]  != -1)
      p[i] = roundl(x[tag_p[i]]);
}

void igl::copyleft::comiso::NRosyField::solveRoundings()
{
  using namespace std;
  using namespace Eigen;

  unsigned n = A.rows();

  gmm::col_matrix< gmm::wsvector< double > > gmm_A;
  std::vector<double> gmm_b;
  std::vector<int> ids_to_round;
  std::vector<double> x;

  gmm_A.resize(n,n);
  gmm_b.resize(n);
  x.resize(n);

  // Copy A
  for (int k=0; k<A.outerSize(); ++k)
    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
    {
      gmm_A(it.row(),it.col()) += it.value();
    }

  // Copy b
  for(unsigned i=0; i<n;++i)
    gmm_b[i] = b[i];

  // Set variables to round
  ids_to_round.clear();
  for(unsigned i=0; i<tag_p.size();++i)
    if(tag_p[i] != -1)
      ids_to_round.push_back(tag_p[i]);

  // Empty constraints
  gmm::row_matrix< gmm::wsvector< double > > gmm_C(0, n);

  COMISO::ConstrainedSolver cs;
  //print_miso_settings(cs.misolver());
  cs.solve(gmm_C, gmm_A, x, gmm_b, ids_to_round, 0.0, false, true);

  // Copy the result back
  for(unsigned i=0; i<F.rows(); ++i)
    if (tag_t[i] != -1)
      angles[i] = x[tag_t[i]];
    else
      angles[i] = hard[i];

  for(unsigned i=0; i<EF.rows(); ++i)
    if(tag_p[i]  != -1)
      p[i] = roundl(x[tag_p[i]]);

}

void igl::copyleft::comiso::NRosyField::roundAndFix()
{
  for(unsigned i=0; i<p.rows(); ++i)
    pFixed[i] = true;
}

void igl::copyleft::comiso::NRosyField::roundAndFixToZero()
{
  for(unsigned i=0; i<p.rows(); ++i)
  {
    pFixed[i] = true;
    p[i] = 0;
  }
}

void igl::copyleft::comiso::NRosyField::solve(const int N)
{
  // Reduce the search space by fixing matchings
  reduceSpace();

  // Build the system
  prepareSystemMatrix(N);

  // Solve with integer roundings
  solveRoundings();

  // This is a very greedy solving strategy
  // // Solve with no roundings
  // solveNoRoundings();
  //
  // // Round all p and fix them
  // roundAndFix();
  //
  // // Build the system
  // prepareSystemMatrix(N);
  //
  // // Solve with no roundings (they are all fixed)
  // solveNoRoundings();

  // Find the cones
  findCones(N);
}

void igl::copyleft::comiso::NRosyField::setConstraintHard(const int fid, const Eigen::Vector3d& v)
{
  isHard[fid] = true;
  hard(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::setConstraintSoft(const int fid, const double w, const Eigen::Vector3d& v)
{
  wSoft(fid) = w;
  soft(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::resetConstraints()
{
  using namespace std;
  using namespace Eigen;

  isHard.resize(F.rows());
  for(unsigned i=0; i<F.rows(); ++i)
    isHard[i] = false;
  hard   = VectorXd::Zero(F.rows());

  wSoft  = VectorXd::Zero(F.rows());
  soft   = VectorXd::Zero(F.rows());
}

Eigen::MatrixXd igl::copyleft::comiso::NRosyField::getFieldPerFace()
{
  using namespace std;
  using namespace Eigen;

  MatrixXd result(F.rows(),3);
  for(unsigned i=0; i<F.rows(); ++i)
    result.row(i) = convertLocalto3D(i, angles(i));
  return result;
}

Eigen::MatrixXd igl::copyleft::comiso::NRosyField::getFFieldPerFace()
{
  using namespace std;
  using namespace Eigen;

  MatrixXd result(F.rows(),6);
  for(unsigned i=0; i<F.rows(); ++i)
  {
      Vector3d v1 = convertLocalto3D(i, angles(i));
      Vector3d n = N.row(i);
      Vector3d v2 = n.cross(v1);
      v1.normalize();
      v2.normalize();

      result.block(i,0,1,3) = v1.transpose();
      result.block(i,3,1,3) = v2.transpose();
  }
  return result;
}

void igl::copyleft::comiso::NRosyField::computek()
{
  using namespace std;
  using namespace Eigen;

  // For every non-border edge
  for (unsigned eid=0; eid<EF.rows(); ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = EF(eid,0);
      int fid1 = EF(eid,1);

      Vector3d N0 = N.row(fid0);
      Vector3d N1 = N.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (EV(eid,0) == F(fid0,i))
          fid0_vc = i;
        if (EV(eid,1) == F(fid1,i))
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Vector3d common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      MatrixXd P(3,3);
      VectorXd o = V.row(F(fid0,fid0_vc));
      VectorXd tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      P.transposeInPlace();

      MatrixXd V0(3,3);
      V0.row(0) = V.row(F(fid0,0)).transpose() -o;
      V0.row(1) = V.row(F(fid0,1)).transpose() -o;
      V0.row(2) = V.row(F(fid0,2)).transpose() -o;

      V0 = (P*V0.transpose()).transpose();

      assert(V0(0,2) < 10e-10);
      assert(V0(1,2) < 10e-10);
      assert(V0(2,2) < 10e-10);

      MatrixXd V1(3,3);
      V1.row(0) = V.row(F(fid1,0)).transpose() -o;
      V1.row(1) = V.row(F(fid1,1)).transpose() -o;
      V1.row(2) = V.row(F(fid1,2)).transpose() -o;
      V1 = (P*V1.transpose()).transpose();

      assert(V1(fid1_vc,2) < 10e-10);
      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      MatrixXd R(3,3);
      R << 1,          0,            0,
           0, cos(alpha), -sin(alpha) ,
           0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      assert(V1(0,2) < 10e-10);
      assert(V1(1,2) < 10e-10);
      assert(V1(2,2) < 10e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      VectorXd ref0 = V0.row(1) - V0.row(0);
      VectorXd ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      MatrixXd R2(2,2);
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

      tmp = R2*ref0.head<2>();

      assert(tmp(0) - ref1(0) < 10^10);
      assert(tmp(1) - ref1(1) < 10^10);

      k[eid] = ktemp;
    }
  }

}

void igl::copyleft::comiso::NRosyField::reduceSpace()
{
  using namespace std;
  using namespace Eigen;

  // All variables are free in the beginning
  for(unsigned i=0; i<EV.rows(); ++i)
    pFixed[i] = false;

  vector<VectorXd> debug;

  // debug
//  MatrixXd B(F.rows(),3);
//  for(unsigned i=0; i<F.rows(); ++i)
//    B.row(i) = 1./3. * (V.row(F(i,0)) + V.row(F(i,1)) + V.row(F(i,2)));

  vector<bool> visited(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    visited[i] = false;

  vector<bool> starting(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    starting[i] = false;

  queue<int> q;
  for(unsigned i=0; i<F.rows(); ++i)
    if (isHard[i] || wSoft[i] != 0)
    {
      q.push(i);
      starting[i] = true;
    }

  // Reduce the search space (see MI paper)
  while (!q.empty())
  {
    int c = q.front();
    q.pop();

    visited[c] = true;
    for(int i=0; i<3; ++i)
    {
      int eid = FE(c,i);
      int fid = TT(c,i);

      // skip borders
      if (fid != -1)
      {
        assert((EF(eid,0) == c && EF(eid,1) == fid) || (EF(eid,1) == c && EF(eid,0) == fid));
        // for every neighbouring face
        if (!visited[fid] && !starting[fid])
        {
          pFixed[eid] = true;
          p[eid] = 0;
          visited[fid] = true;
          q.push(fid);

        }
      }
      else
      {
        // fix borders
        pFixed[eid] = true;
        p[eid] = 0;
      }
    }

  }

  // Force matchings between fixed faces
  for(unsigned i=0; i<F.rows();++i)
  {
    if (isHard[i])
    {
      for(unsigned int j=0; j<3; ++j)
      {
        int fid = TT(i,j);
        if ((fid!=-1) && (isHard[fid]))
        {
          // i and fid are adjacent and fixed
          int eid = FE(i,j);
          int fid0 = EF(eid,0);
          int fid1 = EF(eid,1);

          pFixed[eid] = true;
          p[eid] = roundl(2.0/igl::PI*(hard(fid1) - hard(fid0) - k(eid)));
        }
      }
    }
  }

//  std::ofstream s("/Users/daniele/debug.txt");
//  for(unsigned i=0; i<debug.size(); i += 2)
//    s << debug[i].transpose() << " " << debug[i+1].transpose() << endl;
//  s.close();

}

double igl::copyleft::comiso::NRosyField::convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v)
{
  using namespace std;
  using namespace Eigen;

  // Project onto the tangent plane
  Vector2d vp = TPs[fid] * v;

  // Convert to angle
  return atan2(vp(1),vp(0));
}

Eigen::Vector3d igl::copyleft::comiso::NRosyField::convertLocalto3D(unsigned fid, double a)
{
  using namespace std;
  using namespace Eigen;

  Vector2d vp(cos(a),sin(a));
  return vp.transpose() * TPs[fid];
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::angleDefect()
{
  Eigen::VectorXd A = Eigen::VectorXd::Constant(V.rows(),-2*igl::PI);

  for (unsigned i=0; i < F.rows(); ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      Eigen::VectorXd a = V.row(F(i,(j+1)%3)) - V.row(F(i,j));
      Eigen::VectorXd b = V.row(F(i,(j+2)%3)) - V.row(F(i,j));
      double t = a.transpose()*b;
      t /= (a.norm() * b.norm());
      A(F(i,j)) += acos(t);
    }
  }

  return A;
}

void igl::copyleft::comiso::NRosyField::findCones(int N)
{
  // Compute I0, see http://www.graphics.rwth-aachen.de/media/papers/bommes_zimmer_2009_siggraph_011.pdf for details

  Eigen::VectorXd I0 = Eigen::VectorXd::Zero(V.rows());

  // first the k
  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      I0(EV(i,0)) -= k(i);
      I0(EV(i,1)) += k(i);
    }
  }

  // then the A
  Eigen::VectorXd A = angleDefect();

  I0 = I0 + A;

  // normalize
  I0 = I0 / (2*igl::PI);

  // round to integer (remove numerical noise)
  for (unsigned i=0; i < I0.size(); ++i)
    I0(i) = round(I0(i));

  // compute I
  Eigen::VectorXd I = I0;

  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      I(EV(i,0)) -= double(p(i))/double(N);
      I(EV(i,1)) += double(p(i))/double(N);
    }
  }

  // Clear the vertices on the edges
  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (isBorderEdge[i])
    {
      I0(EV(i,0)) = 0;
      I0(EV(i,1)) = 0;
      I(EV(i,0)) = 0;
      I(EV(i,1)) = 0;
      A(EV(i,0)) = 0;
      A(EV(i,1)) = 0;
    }
  }

  singularityIndex = I;
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::getSingularityIndexPerVertex()
{
  return singularityIndex;
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::VectorXi& b,
  const Eigen::MatrixXd& bc,
  const Eigen::VectorXi& b_soft,
  const Eigen::VectorXd& w_soft,
  const Eigen::MatrixXd& bc_soft,
  const int N,
  const double soft,
  Eigen::MatrixXd& R,
  Eigen::VectorXd& S
  )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V,F);

  // Add hard constraints
  for (unsigned i=0; i<b.size();++i)
    solver.setConstraintHard(b(i),bc.row(i));

  // Add soft constraints
  for (unsigned i=0; i<b_soft.size();++i)
    solver.setConstraintSoft(b_soft(i),w_soft(i),bc_soft.row(i));

  // Set the soft constraints global weight
  solver.setSoftAlpha(soft);

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
                           const Eigen::MatrixXd& V,
                           const Eigen::MatrixXi& F,
                           const Eigen::VectorXi& b,
                           const Eigen::MatrixXd& bc,
                           const int N,
                           Eigen::MatrixXd& R,
                           Eigen::VectorXd& S
                           )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V,F);

  // Add hard constraints
  for (unsigned i=0; i<b.size();++i)
    solver.setConstraintHard(b(i),bc.row(i));

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}


/**
 * @file
 * @author Denise Ratasich
 * @date 11.10.2013
 *
 * @brief Implementation of the SIR particle filter.
 */

#include <ctime>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
// uniform_real gets depricated with newer versions, change to
// uniform_real_distribution with newer version of boost! (see also
// ParticleFilterSIR::resample)
//#include <boost/random/uniform_real_distribution.hpp>

#include "estimation/ParticleFilterSIR.h"
#include "probability/pdfs.h"
#include "probability/sampling.h"

namespace estimation
{
  ParticleFilterSIR::ParticleFilterSIR (unsigned int N)
    : AbstractParticleFilter(N)
  {
    rng.seed((unsigned int)clock());
  }

  ParticleFilterSIR::~ParticleFilterSIR ()
  {
    // nothing to do
  }

  void ParticleFilterSIR::log(std::ostream& os, Log type, int index) const
  {
    switch (type)
    {
    case PARTICLES:
      for (int i = 0; i < particles.size(); i++)
	os << particles[i][index] << " ";
      os << std::endl;
      break;
    case WEIGHTS:
      for (int i = 0; i < particles.size(); i++)
	os << weights[i] << " ";
      os << std::endl;
      break;
    case NEFF:
      os << Neff << std::endl;
      break;
    }
  }

  // -----------------------------------------
  // getters and setters
  // -----------------------------------------
  void ParticleFilterSIR::setProcessNoiseCovariance (MatrixXd& Q)
  {
    this->Q = Q;
    validated = false;
  }

  void ParticleFilterSIR::setMeasurementNoiseCovariance (MatrixXd& R)
  {
    this->R = R;
    validated = false;
  }

  void ParticleFilterSIR::validate (void)
  {
    try
    {
      // check for minimal initialization -----------------------
      if (!f)			// callback empty
	throw std::runtime_error("State transition model missing.");
      if (!h)			// callback empty
	throw std::runtime_error("Observation model missing.");
      if (Q.rows() == 0)
	throw std::runtime_error("Process noise covariance missing.");
      if (R.rows() == 0)
	throw std::runtime_error("Measurement noise covariance missing.");
      if (particles[0].size() == 0)
	throw std::runtime_error("Particles not initialized.");

      // take sizes from required parameters
      int n = particles[0].size();
      int m = R.rows();

      // create other parameters if missing ---------------------
      // none

      // check appropriate sizes of matrices and vectors --------
      if (Q.rows() != n  ||  Q.cols() != n)
	throw std::runtime_error("Process noise covariance has invalid size.");
      if (R.rows() != R.cols())
	throw std::runtime_error("Measurement noise covariance must be a square matrix.");

      // validation finished successfully -----------------------
      validated = true;

      // further things to initialize ---------------------------
      // create output state
      if (out.size() != n) {	// not initialized till now or invalid size from reinit
	out.clear();
	for (int i = 0; i < n; i++)
	  out.add(OutputValue());
      }
    }
    catch(std::exception& e)
    {
      std::string additionalInfo = "ParticleFilterSIR: Validation failed. ";
      throw estimator_error(additionalInfo + e.what());
    }
  }

  // -----------------------------------------
  // Overrides of ParticleFilterSIR's IEstimator implementation
  // -----------------------------------------

  void ParticleFilterSIR::serialize(std::ostream& os) const
  {
    os << "SIR Particle Filter";
  }

  // -----------------------------------------
  // Particle Filtering
  // -----------------------------------------
  void ParticleFilterSIR::sample (void)
  {
    // estimate next state of particles
    for (int i = 0; i < particles.size(); i++)
    {
      // sample from process noise: zero mean gaussian with covariance Q
      VectorXd w = probability::sampleNormalDistribution(VectorXd::Zero(Q.rows()), Q);

      f(particles[i], u);		// time update
      particles[i] = particles[i] + w;  // add noise
    }
  }

  void ParticleFilterSIR::weight (Input measurements)
  {
    double sumWeights = 0, sumWeightsSquare = 0;

    // measurement vector z
    VectorXd z(R.rows());

    if (measurements.size() != z.size())
      throw std::runtime_error("Number of measurements invalid.");

    // calculate weight
    for (int i = 0; i < particles.size(); i++)
    {
      // weight = likelihood of the measurement
      // estimate expected measurement when in state i
      VectorXd z_expected = VectorXd::Zero(z.size());
      h(z_expected, particles[i]);

      // get probability of the measurement (mean = z_expected,
      // covariance = measurement noise covariance)
      prepareMeasurements(z, measurements, z_expected);	// fills z
      double weight = probability::pdfNormalDistribution(z, z_expected, R);

      weights[i] = weights[i] * weight;	// set weight (consider old weight!)
      sumWeights += weights[i];		// add to sum for normalization
    }

    // normalize weights
    for (int i = 0; i < particles.size(); i++)
    {
      weights[i] /= sumWeights;

      // accumulate for Neff-calculation
      sumWeightsSquare += weights[i] * weights[i];
    }

    // evaluate effective number of particles
    Neff = 1 / sumWeightsSquare;
  }

  void ParticleFilterSIR::resample (void)
  {
    int N = particles.size();

    if (Neff > 0.8*N)
      return;

    // copy current particles to the buffer 'partices_old' where the
    // new ones will be chosen from; the array 'particles' will be
    // filled with these drawn ones
    std::vector<VectorXd> particles_old(particles);

    // 1. generate CDF
    std::vector<double> cdf(N);
    cdf[0] = weights[0];
    for (int i = 1; i < N; i++)
      cdf[i] = cdf[i-1] + weights[i];

    // 2. draw a starting point
    boost::uniform_real<> dist(0, 1.0 / N);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
      random(rng, dist);
    double u0 = random();	// u0 is then betw 0..1/N
    int i = 0;

    for (int j = 0; j < N; j++)
    {
      // 3. move along the CDF
      double uj = u0 + ((double)j)/N;

      // check where the random number in the CDF fits -> this is the
      // sample to choose; a sample with higher weight has a higher
      // span in the CDF, hence will be chosen more often
      while (uj > cdf[i])
      	i++;

      particles[j] = particles_old[i];
      weights[j] = 1.0 / N;
    }
  }
}


#include "format_helper.h"

#include <iostream>
#include <iomanip>
#include <dirent.h>
#include <sys/stat.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include "math_helper.h"

namespace core {

using namespace std;
using namespace cv;

namespace {
void GetCalibParams(const cv::Mat& P_left, const cv::Mat& P_right, double* calib) {
  //std::cout << P_left << "\n" << P_right;
  calib[0] = P_left.at<double>(0,0);
  calib[1] = P_left.at<double>(1,1);
  calib[2] = P_left.at<double>(0,2);
  calib[3] = P_left.at<double>(1,2);
  //calib[4] = std::abs(P_left.at<double>(0,3) - P_right.at<double>(0,3)) / calib[0];
  double x_diff = P_left.at<double>(0,3) - P_right.at<double>(0,3);
  double y_diff = P_left.at<double>(1,3) - P_right.at<double>(1,3);
  double z_diff = P_left.at<double>(2,3) - P_right.at<double>(2,3);
  calib[4] = std::sqrt(x_diff*x_diff + y_diff*y_diff + z_diff*z_diff) / calib[0];
  //for (int i = 0; i < 5; i++)
  //  std::cout << calib[i] << "\n";
}
}

void FormatHelper::ParseKITTIDatasetConfig(
    const std::string config_file, std::string& cam_params_file, std::string& dataset_name,
    std::string& left_folder, std::string& right_folder, std::string& gt_filepath,
    std::size_t& start_num, std::size_t& end_num, std::size_t& num_width) {
  try {
    po::variables_map vm;
    po::options_description config("Config file options");
    config.add_options()
      ("dataset_name", po::value<std::string>(&dataset_name)->required(), "name")
      ("camera_params,p", po::value<std::string>(&cam_params_file)->required(), "camera params file")
      ("left_folder", po::value<std::string>(&left_folder)->required(), "folder")
      ("right_folder", po::value<std::string>(&right_folder)->required(), "folder")
      ("start_num", po::value<std::size_t>(&start_num)->required(), "start number")
      ("end_num", po::value<std::size_t>(&end_num)->required(), "end number")
      ("num_width", po::value<std::size_t>(&num_width)->required(), "padding")
      ("groundtruth,g", po::value<std::string>(&gt_filepath)->required(), "file with motion GT");

    std::ifstream ifs(config_file);
    if (!ifs) {
      throw "can not open config file: " + config_file + "\n";
    }
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    throw 1;
  }

  left_folder += "/";
  right_folder += "/";
}

void FormatHelper::ReadCalibKitti(const std::string path, double* grey_cam, double* color_cam)
{
  std::ifstream calib_file(path);
  std::vector<cv::Mat> P;
  P.resize(4);

  std::string word;
  for (int i = 0; i < 4; i++) {
    P[i].create(3, 4, CV_64F);
    calib_file >> word;
    for (int j = 0; j < 12; j++) {
      calib_file >> word;
      int row = j / 4;
      int col = j % 4;
      P[i].at<double>(row,col) = std::stod(word);
    }
    //std::cout << P[i] << "\n";
  }
  //std::cout << "\nCalib mono:\n";
  GetCalibParams(P[0], P[1], grey_cam);
  //std::cout << "\nCalib color:\n";
  GetCalibParams(P[2], P[3], color_cam);
}

// reads next matrix in a row
void FormatHelper::ReadNextRtMatrix(std::ifstream& file, cv::Mat& Rt)
{
   std::string line;
   getline(file, line);
   stringstream stream(line);
   string val;
   //cout << "LINE: " << line << endl;
   Rt = Mat::eye(4, 4, CV_64F);
   for(int i = 0; i < 3; i++) {
      for(int j = 0; j < 4; j++) {
         stream >> val;
         Rt.at<double>(i,j) = std::stod(val);
         //cout << "READ: " << motion_params[i] << endl << endl;
      }
   }
}

void FormatHelper::Read2FrameMotionFromAccCameraMotion(const std::string filename, const int num_of_motions,
                                                       std::vector<cv::Mat>& world_motion,
                                                       std::vector<cv::Mat>& camera_motion) {
  // wrt - world motion
  // crt - camera motion
  std::ifstream file(filename);
  cv::Mat wrt, wrt_prev, wrt_curr;
  // read first (usually identety) matrix
  core::FormatHelper::ReadNextRtMatrix(file, wrt_curr);
  core::MathHelper::invTrans(wrt_curr, wrt_prev);

  for (int i = 0; i < num_of_motions; i++) {
    core::FormatHelper::ReadNextRtMatrix(file, wrt_curr);
    cv::Mat crt = wrt_prev * wrt_curr;
    core::MathHelper::invTrans(wrt_curr, wrt_prev);
    camera_motion.push_back(crt.clone());
    core::MathHelper::invTrans(crt, wrt);
    world_motion.push_back(wrt.clone());
    //std::cout << crt << "\n\n";
    //std::cout << wrt << "\n\n";
  }
}

void FormatHelper::WriteMatRt(const cv::Mat& Rt, std::ofstream& fp) {
   //std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++) {
        double val = Rt.at<double>(i,j);
        fp << std::scientific << val << " ";
      }
   }
   fp << endl;
}

void FormatHelper::WriteMotionToFile(const Eigen::Matrix4d& Rt, std::ofstream& file) {
  //std::setprecision(6);
  for(int i = 0; i < Rt.rows() - 1; i++)
    for(int j = 0; j < Rt.cols(); j++)
      file << std::scientific << Rt(i,j) << " ";
  file << std::endl;
}

void FormatHelper::WriteMatRt(const cv::Mat& Rt, std::ofstream& fp, bool convert_to_cm)
{
   //std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++) {
        double val = Rt.at<double>(i,j);
        if(convert_to_cm && j == 3)
          val *= 100.0;
        fp << std::scientific << val << " ";
      }
   }
   fp << endl;
}

// [fx fy cx cy tx=baseline]
void FormatHelper::readCameraParams(const std::string& filepath, double (&cam_params)[5])
{
   ifstream file(filepath);
   std::string line;
   getline(file, line);
   stringstream stream(line);
   string val;
   //cout << "[FormatHelper]: Using camera params: ";
   for(int i = 0; i < 5; i++) {
      stream >> val;
      cam_params[i] = std::stod(val);
      //cout << cam_params[i] << "  ";
   }
   cout << endl;
   file.close();
}

void FormatHelper::readGpsPoints(string filename, vector<Mat> points)
{
   ifstream file(filename);
   string val;
   points.clear();
   Mat pt(2, 1, CV_64F);
   while(!file.eof()) {
      file >> val;
      pt.at<double>(0,0) = std::stod(val);
      file >> val;
      pt.at<double>(1,0) = std::stod(val);
      cout << pt << endl;
      points.push_back(pt.clone());
      for(int i = 0; i < 9; i++) file >> val;
   }
}

void FormatHelper::getCalibParams(std::string& intrinsic_filename, std::string& extrinsic_filename, cv::Mat& P_left,
                    cv::Mat& P_right, cv::Mat& Q, cv::Mat& C_left, cv::Mat& D_left, cv::Mat& C_right, cv::Mat& D_right)
{
   // reading intrinsic parameters
   //FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
   FileStorage fs(intrinsic_filename, FileStorage::READ);
   if(!fs.isOpened())
   {
      cout << "Failed to open file " << intrinsic_filename << endl;
      return;
   }

   //Mat M1, D1, M2, D2;
   fs["M1"] >> C_left;
   fs["D1"] >> D_left;
   fs["M2"] >> C_right;
   fs["D2"] >> D_right;
   Mat R, T, R1, R2;
   fs["R"] >> R;
   fs["T"] >> T;

   // TODO - read this also from file
   //cv::Size imageSize(CALIB_WIDTH, CALIB_HEIGHT);
   //stereoRectify(C_left, D_left, C_right, D_right, imageSize, R, T, R1, R2, P_left, P_right, Q, CALIB_ZERO_DISPARITY,
   //      -1, imageSize);

   return;
}

bool FormatHelper::readStringList(const std::string& filename, std::vector<std::string>& l)
{
   l.resize(0);
   FileStorage fs(filename, FileStorage::READ);
   if( !fs.isOpened() )
      return false;
   FileNode n = fs.getFirstTopLevelNode();
   if( n.type() != FileNode::SEQ )
      return false;
   FileNodeIterator it = n.begin(), it_end = n.end();
   for( ; it != it_end; ++it )
      l.push_back((string)*it);
   return true;
}

/* Returns a list of files in a directory (except the ones that begin with a dot) */
void FormatHelper::GetFilesInFolder(const std::string& dir_path, std::vector<std::string>& files,
                                    bool get_full_path = false) {
  DIR *dir;
  struct dirent *ent;
  struct stat st;
  dir = opendir(dir_path.c_str());
  while ((ent = readdir(dir)) != NULL) {
    const std::string file_name = ent->d_name;
    const std::string full_file_name = dir_path + "/" + file_name;
    if (file_name[0] == '.')
      continue;
    if (stat(full_file_name.c_str(), &st) == -1)
      continue;
    const bool is_directory = (st.st_mode & S_IFDIR) != 0;
    if (is_directory)
      continue;

    if (get_full_path)
      files.push_back(full_file_name);
    else
      files.push_back(file_name);
  }
  closedir(dir);
  std::sort(files.begin(), files.end());
}

}


#include <iostream>
#include <thread>
#include <chrono>
#include <memory>
#include <array>
#include <vector>
#include <random>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

#include "sfmMyFunctions.h"
#include "sfmExceptionMacro.h"
#include "sfmBasicTypes.h"
#include "sfmPedestrianSpawner.h"

//this always seems to be highlighted but it complies fine
#include "sfmVisualiser.h"

int main(int argc, char** argv)
{
    //variables needed throughout the function
    double world_width_x = 50.0;
    double world_height_y = 10.0;
    int no_pedestrians = 100;
    int choice;
    double dt = 0.1;
    double finish_time_s = 100;
    double v_max = 1.3;
    std::vector<std::shared_ptr<sfm::Forces> >pedestrians;

    // Create viewer and initialise with required number of pedestrians
    //left here as its easier to reposition the viewer to where you would like it before
    //entering the selection, i left a second commented later incase someone didnt like
    //this, it is a little jarring at first.
    sfm::Visualiser viewer(no_pedestrians, world_width_x, world_height_y);

    // neatened up the interface to easily pick from three demos
    std::cout   << "Please Pick Demo of 100 pedestrians over 50 seconds; \n"
                << "1(Directional),2(Targeted) or 3(All random);) " << std::endl;
    std::cin >> choice;

    //this has all the pedestrians moving straight along the x axis to their opposite side
    if(choice == 1){

        //set up variables for directional functions
        sfm::dir2d left_side_x(1,2);
        sfm::dir2d left_side_y(0.1,9.9);
        sfm::dir2d right_side_x(48,49);
        sfm::dir2d right_side_y(0.1,9.9);
        sfm::dir2d direc1(1,0);
        sfm::dir2d direc2(-1,0);

        //creates 2 sets of pedestrians in the same format as in the open_mp file
        //essencially we create 2 sets of pedestrians and append them to the total.
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;
        pedestrians1 = sfm::Factory::Distributed(pedestrians1,"Directional",no_pedestrians/2,direc1,direc1,left_side_x,left_side_y);
        for(int point = 0; point < pedestrians1.size();++point){
            pedestrians.emplace_back(pedestrians1[point]);
        }
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians2;
        pedestrians2 = sfm::Factory::Distributed(pedestrians2,"Directional",no_pedestrians/2,direc2,direc2,right_side_x,right_side_y);
        for(int point = 0; point < pedestrians2.size();++point){
            pedestrians.emplace_back(pedestrians2[point]);
        }
    }

    //this has all the pedestrians moving straight along to a point on the opposite side
    else if(choice == 2){

        //variables for targeted pedestrians
        sfm::dir2d left_side_x(1,2);
        sfm::dir2d left_side_y(0.1,9.9);
        sfm::dir2d dest_left_x(48,49);
        sfm::dir2d dest_left_y(0.1,9.9);
        sfm::dir2d right_side_x(48,49);
        sfm::dir2d right_side_y(0.1,9.9);
        sfm::dir2d dest_right_x(1,2);
        sfm::dir2d dest_right_y(0.1,9.9);

        // this uses the distributed fixed class method
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;
        pedestrians1 = sfm::Factory::Distributed(pedestrians1,"Targeted",no_pedestrians/2,dest_left_x,dest_left_y,left_side_x,left_side_y);
        for(int point = 0; point < pedestrians1.size();++point){
            pedestrians.emplace_back(pedestrians1[point]);
        }
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians2;
        pedestrians2 = sfm::Factory::Distributed(pedestrians2,"Targeted",no_pedestrians/2,dest_right_x,dest_right_y,right_side_x,right_side_y);
        for(int point = 0; point < pedestrians2.size();++point){
            pedestrians.emplace_back(pedestrians2[point]);
        }

    }

    // this will randomly spawn 100 pedestrains at 100 randompoints
    else if(choice == 3){

        //very simple to spawn random pedestrians
        pedestrians = sfm::Factory::Spawner(pedestrians,no_pedestrians);
    }
    else{
        std::cout << "Pick one of the three demos" <<std::endl;
    }

    // Create viewer and initialise with required number of pedestrians
    //sfm::Visualiser viewer(no_pedestrians, world_width_x, world_height_y);

    //for loop over time
    for(int t=0; t<(finish_time_s/dt);++t){

        //next a for loop that goes through each pedestrian and calculates its resultant force and then updates the new qualities
        //detailed explanation in open mp
        for(int j=0; j<pedestrians.size();++j){
            viewer.SetPedestrian(j, pedestrians[j]->Return_Current_Position()[1],
                                    pedestrians[j]->Return_Current_Position()[0],
                                    pedestrians[j]->Return_Velocity()[1],
                                    pedestrians[j]->Return_Velocity()[0]);
            sfm::dir2d temp_force = pedestrians[j]->Resultant_force(pedestrians,temp_force, dt);
            sfm::dir2d  new_velocity = (temp_force*dt) + pedestrians[j]->Return_Velocity();
            if(new_velocity.length() > v_max*pedestrians[j]->Return_Speed()){
                new_velocity = new_velocity*(v_max*pedestrians[j]->Return_Speed()/new_velocity.length());
                }
            sfm::dir2d position(pedestrians[j]->Return_Current_Position()[1],pedestrians[j]->Return_Current_Position()[0]);
            sfm::pos2d new_position = {position[1]+(new_velocity[1]*dt),(position[0]+new_velocity[0]*dt)};
            pedestrians[j]->Update_Velocity(new_velocity);
            pedestrians[j]->Update_Current_Position(new_position);
        }

        // Tell viewer to redraw scene
        viewer.UpdateScene();

        // Sleep for a bit so can see visualiser updating, i left it as 10 miliseconds at the
        // actual sleep time of 100 ms seemed a bit too slow (0.1s dt)
        std::this_thread::sleep_for (std::chrono::milliseconds(100));

    }

    return 0;
}


/*
 * SVD_pvfmm.hpp
 *
 *  Created on: Feb 20, 2017
 *      Author: wyan
 */

#ifndef SVD_PVFMM_HPP_
#define SVD_PVFMM_HPP_

#include <cassert>
#include <cmath>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

template <class T>
inline void gemm(char TransA, char TransB, int M, int N, int K, T alpha, T *A,
                 int lda, T *B, int ldb, T beta, T *C, int ldc) {
    if ((TransA == 'N' || TransA == 'n') && (TransB == 'N' || TransB == 'n')) {
#pragma omp parallel for
        for (auto n = 0; n < N; n++) {     // Columns of C
            for (auto m = 0; m < M; m++) { // Rows of C
                T AxB = 0;
                for (auto k = 0; k < K; k++) {
                    AxB += A[m + lda * k] * B[k + ldb * n];
                }
                C[m + ldc * n] =
                    alpha * AxB + (beta == 0 ? 0 : beta * C[m + ldc * n]);
            }
        }
    } else if (TransA == 'N' || TransA == 'n') {
#pragma omp parallel for
        for (auto n = 0; n < N; n++) {     // Columns of C
            for (auto m = 0; m < M; m++) { // Rows of C
                T AxB = 0;
                for (auto k = 0; k < K; k++) {
                    AxB += A[m + lda * k] * B[n + ldb * k];
                }
                C[m + ldc * n] =
                    alpha * AxB + (beta == 0 ? 0 : beta * C[m + ldc * n]);
            }
        }
    } else if (TransB == 'N' || TransB == 'n') {
#pragma omp parallel for
        for (auto n = 0; n < N; n++) {     // Columns of C
            for (auto m = 0; m < M; m++) { // Rows of C
                T AxB = 0;
                for (auto k = 0; k < K; k++) {
                    AxB += A[k + lda * m] * B[k + ldb * n];
                }
                C[m + ldc * n] =
                    alpha * AxB + (beta == 0 ? 0 : beta * C[m + ldc * n]);
            }
        }
    } else {
#pragma omp parallel for
        for (auto n = 0; n < N; n++) {     // Columns of C
            for (auto m = 0; m < M; m++) { // Rows of C
                T AxB = 0;
                for (auto k = 0; k < K; k++) {
                    AxB += A[k + lda * m] * B[n + ldb * k];
                }
                C[m + ldc * n] =
                    alpha * AxB + (beta == 0 ? 0 : beta * C[m + ldc * n]);
            }
        }
    }
}

#define U(i, j) U_[(i)*dim[0] + (j)]
#define S(i, j) S_[(i)*dim[1] + (j)]
#define V(i, j) V_[(i)*dim[1] + (j)]

template <class T>
void GivensL(T *S_, const size_t dim[2], size_t m, T a, T b) {
    T r = sqrt(a * a + b * b);
    T c = a / r;
    T s = -b / r;

#pragma omp parallel for
    for (size_t i = 0; i < dim[1]; i++) {
        T S0 = S(m + 0, i);
        T S1 = S(m + 1, i);
        S(m, i) += S0 * (c - 1);
        S(m, i) += S1 * (-s);

        S(m + 1, i) += S0 * (s);
        S(m + 1, i) += S1 * (c - 1);
    }
}

template <class T>
void GivensR(T *S_, const size_t dim[2], size_t m, T a, T b) {
    T r = sqrt(a * a + b * b);
    T c = a / r;
    T s = -b / r;

#pragma omp parallel for
    for (size_t i = 0; i < dim[0]; i++) {
        T S0 = S(i, m + 0);
        T S1 = S(i, m + 1);
        S(i, m) += S0 * (c - 1);
        S(i, m) += S1 * (-s);

        S(i, m + 1) += S0 * (s);
        S(i, m + 1) += S1 * (c - 1);
    }
}

template <class T>
void SVD(const size_t dim[2], T *U_, T *S_, T *V_, T eps = -1) {
    assert(dim[0] >= dim[1]);
#ifdef SVD_DEBUG
    Matrix<T> M0(dim[0], dim[1], S_);
#endif

    { // Bi-diagonalization
        size_t n = std::min(dim[0], dim[1]);
        std::vector<T> house_vec(std::max(dim[0], dim[1]));
        for (size_t i = 0; i < n; i++) {
            // Column Householder
            {
                T x1 = S(i, i);
                if (x1 < 0)
                    x1 = -x1;

                T x_inv_norm = 0;
                for (size_t j = i; j < dim[0]; j++) {
                    x_inv_norm += S(j, i) * S(j, i);
                }
                if (x_inv_norm > 0)
                    x_inv_norm = 1 / sqrt(x_inv_norm);

                T alpha = sqrt(1 + x1 * x_inv_norm);
                T beta = x_inv_norm / alpha;

                house_vec[i] = -alpha;
                for (size_t j = i + 1; j < dim[0]; j++) {
                    house_vec[j] = -beta * S(j, i);
                }
                if (S(i, i) < 0)
                    for (size_t j = i + 1; j < dim[0]; j++) {
                        house_vec[j] = -house_vec[j];
                    }
            }
#pragma omp parallel for
            for (size_t k = i; k < dim[1]; k++) {
                T dot_prod = 0;
                for (size_t j = i; j < dim[0]; j++) {
                    dot_prod += S(j, k) * house_vec[j];
                }
                for (size_t j = i; j < dim[0]; j++) {
                    S(j, k) -= dot_prod * house_vec[j];
                }
            }
#pragma omp parallel for
            for (size_t k = 0; k < dim[0]; k++) {
                T dot_prod = 0;
                for (size_t j = i; j < dim[0]; j++) {
                    dot_prod += U(k, j) * house_vec[j];
                }
                for (size_t j = i; j < dim[0]; j++) {
                    U(k, j) -= dot_prod * house_vec[j];
                }
            }

            // Row Householder
            if (i >= n - 1)
                continue;
            {
                T x1 = S(i, i + 1);
                if (x1 < 0)
                    x1 = -x1;

                T x_inv_norm = 0;
                for (size_t j = i + 1; j < dim[1]; j++) {
                    x_inv_norm += S(i, j) * S(i, j);
                }
                if (x_inv_norm > 0)
                    x_inv_norm = 1 / sqrt(x_inv_norm);

                T alpha = sqrt(1 + x1 * x_inv_norm);
                T beta = x_inv_norm / alpha;

                house_vec[i + 1] = -alpha;
                for (size_t j = i + 2; j < dim[1]; j++) {
                    house_vec[j] = -beta * S(i, j);
                }
                if (S(i, i + 1) < 0)
                    for (size_t j = i + 2; j < dim[1]; j++) {
                        house_vec[j] = -house_vec[j];
                    }
            }
#pragma omp parallel for
            for (size_t k = i; k < dim[0]; k++) {
                T dot_prod = 0;
                for (size_t j = i + 1; j < dim[1]; j++) {
                    dot_prod += S(k, j) * house_vec[j];
                }
                for (size_t j = i + 1; j < dim[1]; j++) {
                    S(k, j) -= dot_prod * house_vec[j];
                }
            }
#pragma omp parallel for
            for (size_t k = 0; k < dim[1]; k++) {
                T dot_prod = 0;
                for (size_t j = i + 1; j < dim[1]; j++) {
                    dot_prod += V(j, k) * house_vec[j];
                }
                for (size_t j = i + 1; j < dim[1]; j++) {
                    V(j, k) -= dot_prod * house_vec[j];
                }
            }
        }
    }

    size_t k0 = 0;
    size_t iter = 0;
    if (eps < 0) {
        eps = 1.0;
        while (eps + (T)1.0 > 1.0)
            eps *= 0.5;
        eps *= 64.0;
    }
    while (k0 < dim[1] - 1) { // Diagonalization
        iter++;

        T S_max = 0.0;
        for (size_t i = 0; i < dim[1]; i++)
            S_max = (S_max > S(i, i) ? S_max : S(i, i));

        while (k0 < dim[1] - 1 && std::abs<T>(S(k0, k0 + 1)) <= eps * S_max)
            k0++;
        if (k0 == dim[1] - 1)
            continue;

        size_t n = k0 + 2;
        while (n < dim[1] && std::abs<T>(S(n - 1, n)) > eps * S_max)
            n++;

        T alpha = 0;
        T beta = 0;
        { // Compute mu
            T C[2][2];
            C[0][0] = S(n - 2, n - 2) * S(n - 2, n - 2);
            if (n - k0 > 2)
                C[0][0] += S(n - 3, n - 2) * S(n - 3, n - 2);
            C[0][1] = S(n - 2, n - 2) * S(n - 2, n - 1);
            C[1][0] = S(n - 2, n - 2) * S(n - 2, n - 1);
            C[1][1] = S(n - 1, n - 1) * S(n - 1, n - 1) +
                      S(n - 2, n - 1) * S(n - 2, n - 1);

            T b = -(C[0][0] + C[1][1]) / 2;
            T c = C[0][0] * C[1][1] - C[0][1] * C[1][0];
            T d = 0;
            if (b * b - c > 0)
                d = sqrt(b * b - c);
            else {
                T b = (C[0][0] - C[1][1]) / 2;
                T c = -C[0][1] * C[1][0];
                if (b * b - c > 0)
                    d = sqrt(b * b - c);
            }

            T lambda1 = -b + d;
            T lambda2 = -b - d;

            T d1 = lambda1 - C[1][1];
            d1 = (d1 < 0 ? -d1 : d1);
            T d2 = lambda2 - C[1][1];
            d2 = (d2 < 0 ? -d2 : d2);
            T mu = (d1 < d2 ? lambda1 : lambda2);

            alpha = S(k0, k0) * S(k0, k0) - mu;
            beta = S(k0, k0) * S(k0, k0 + 1);
        }

        for (size_t k = k0; k < n - 1; k++) {
            size_t dimU[2] = {dim[0], dim[0]};
            size_t dimV[2] = {dim[1], dim[1]};
            GivensR(S_, dim, k, alpha, beta);
            GivensL(V_, dimV, k, alpha, beta);

            alpha = S(k, k);
            beta = S(k + 1, k);
            GivensL(S_, dim, k, alpha, beta);
            GivensR(U_, dimU, k, alpha, beta);

            alpha = S(k, k + 1);
            beta = S(k, k + 2);
        }

        { // Make S bi-diagonal again
            for (size_t i0 = k0; i0 < n - 1; i0++) {
                for (size_t i1 = 0; i1 < dim[1]; i1++) {
                    if (i0 > i1 || i0 + 1 < i1)
                        S(i0, i1) = 0;
                }
            }
            for (size_t i0 = 0; i0 < dim[0]; i0++) {
                for (size_t i1 = k0; i1 < n - 1; i1++) {
                    if (i0 > i1 || i0 + 1 < i1)
                        S(i0, i1) = 0;
                }
            }
            for (size_t i = 0; i < dim[1] - 1; i++) {
                if (std::abs<T>(S(i, i + 1)) <= eps * S_max) {
                    S(i, i + 1) = 0;
                }
            }
        }
        // std::cout<<iter<<' '<<k0<<' '<<n<<'\n';
    }

    { // Check Error
#ifdef SVD_DEBUG
        Matrix<T> U0(dim[0], dim[0], U_);
        Matrix<T> S0(dim[0], dim[1], S_);
        Matrix<T> V0(dim[1], dim[1], V_);
        Matrix<T> E = M0 - U0 * S0 * V0;
        T max_err = 0;
        T max_nondiag0 = 0;
        T max_nondiag1 = 0;
        for (size_t i = 0; i < E.Dim(0); i++)
            for (size_t j = 0; j < E.Dim(1); j++) {
                if (max_err < pvfmm::fabs<T>(E[i][j]))
                    max_err = pvfmm::fabs<T>(E[i][j]);
                if ((i > j + 0 || i + 0 < j) &&
                    max_nondiag0 < pvfmm::fabs<T>(S0[i][j]))
                    max_nondiag0 = pvfmm::fabs<T>(S0[i][j]);
                if ((i > j + 1 || i + 1 < j) &&
                    max_nondiag1 < pvfmm::fabs<T>(S0[i][j]))
                    max_nondiag1 = pvfmm::fabs<T>(S0[i][j]);
            }
        std::cout << max_err << '\n';
        std::cout << max_nondiag0 << '\n';
        std::cout << max_nondiag1 << '\n';
#endif
    }
}

#undef U
#undef S
#undef V
#undef SVD_DEBUG

template <class T>
inline void svd(char *JOBU, char *JOBVT, int *M, int *N, T *A, int *LDA, T *S,
                T *U, int *LDU, T *VT, int *LDVT, T *WORK, int *LWORK,
                int *INFO) {

    const size_t dim[2] = {static_cast<size_t>(std::max(*N, *M)),
                           static_cast<size_t>(std::min(*N, *M))};

    std::vector<T> Udata(dim[0] * dim[0], 0);
    std::vector<T> Vdata(dim[1] * dim[1], 0);
    std::vector<T> Sdata(dim[0] * dim[1], 0);
    T *U_ = Udata.data();
    T *V_ = Vdata.data();
    T *S_ = Sdata.data();
    //	T* U_ = mem::aligned_new < T > (dim[0] * dim[0]);
    //	memset(U_, 0, dim[0] * dim[0] * sizeof(T));
    //	T* V_ = mem::aligned_new < T > (dim[1] * dim[1]);
    //	memset(V_, 0, dim[1] * dim[1] * sizeof(T));
    //	T* S_ = mem::aligned_new < T > (dim[0] * dim[1]);

    const size_t lda = *LDA;
    const size_t ldu = *LDU;
    const size_t ldv = *LDVT;

    if (dim[1] == static_cast<size_t>(*M)) {
        for (size_t i = 0; i < dim[0]; i++) {
            for (size_t j = 0; j < dim[1]; j++) {
                S_[i * dim[1] + j] = A[i * lda + j];
            }
        }
    } else {
        for (size_t i = 0; i < dim[0]; i++) {
            for (size_t j = 0; j < dim[1]; j++) {
                S_[i * dim[1] + j] = A[j * lda + i];
            }
        }
    }
    for (size_t i = 0; i < dim[0]; i++) {
        U_[i * dim[0] + i] = 1;
    }
    for (size_t i = 0; i < dim[1]; i++) {
        V_[i * dim[1] + i] = 1;
    }

    SVD<T>(dim, U_, S_, V_, (T)-1);

    for (size_t i = 0; i < dim[1]; i++) { // Set S
        S[i] = S_[i * dim[1] + i];
    }
    if (dim[1] == static_cast<size_t>(*M)) { // Set U
        for (size_t i = 0; i < dim[1]; i++)
            for (int j = 0; j < *M; j++) {
                U[j + ldu * i] = V_[j + i * dim[1]] * (S[i] < 0.0 ? -1.0 : 1.0);
            }
    } else {
        for (size_t i = 0; i < dim[1]; i++)
            for (int j = 0; j < *M; j++) {
                U[j + ldu * i] = U_[i + j * dim[0]] * (S[i] < 0.0 ? -1.0 : 1.0);
            }
    }
    if (dim[0] == static_cast<size_t>(*N)) { // Set V
        for (int i = 0; i < *N; i++)
            for (size_t j = 0; j < dim[1]; j++) {
                VT[j + ldv * i] = U_[j + i * dim[0]];
            }
    } else {
        for (int i = 0; i < *N; i++)
            for (size_t j = 0; j < dim[1]; j++) {
                VT[j + ldv * i] = V_[i + j * dim[1]];
            }
    }
    for (size_t i = 0; i < dim[1]; i++) {
        S[i] = S[i] * (S[i] < 0.0 ? -1.0 : 1.0);
    }

    //	mem::aligned_delete < T > (U_);
    //	mem::aligned_delete < T > (S_);
    //	mem::aligned_delete < T > (V_);

    if (0) { // Verify
        const size_t dim[2] = {static_cast<size_t>(std::max(*N, *M)),
                               static_cast<size_t>(std::min(*N, *M))};
        const size_t lda = *LDA;
        const size_t ldu = *LDU;
        const size_t ldv = *LDVT;

        Eigen::MatrixXd A1(*M, *N);
        Eigen::MatrixXd S1(dim[1], dim[1]);
        Eigen::MatrixXd U1(*M, dim[1]);
        Eigen::MatrixXd V1(dim[1], *N);
        for (size_t i = 0; i < *N; i++)
            for (size_t j = 0; j < *M; j++) {
                //				A1[j][i] = A[j + i * lda];
                A1(j, i) = A[j + i * lda];
            }
        S1.setZero();
        for (size_t i = 0; i < dim[1]; i++) { // Set S
                                              //			S1[i][i] = S[i];
            S1(i, i) = S[i];
        }
        for (size_t i = 0; i < dim[1]; i++)
            for (size_t j = 0; j < *M; j++) {
                //				U1[j][i] = U[j + ldu * i];
                U1(j, i) = U[j + ldu * i];
            }
        for (size_t i = 0; i < *N; i++)
            for (size_t j = 0; j < dim[1]; j++) {
                //				V1[j][i] = VT[j + ldv * i];
                V1(j, i) = VT[j + ldv * i];
            }
        std::cout << U1 * S1 * V1 - A1 << '\n';
    }
}

template <class T>
void pinv_pvfmm(T *M, int n1, int n2, T eps, T *M_) {
    if (n1 * n2 == 0)
        return;
    int m = n2;
    int n = n1;
    int k = (m < n ? m : n);

    std::vector<T> Udata(m * k);
    std::vector<T> Sdata(k);
    std::vector<T> VTdata(k * n);

    //	T* tU = mem::aligned_new < T > (m * k);
    //	T* tS = mem::aligned_new < T > (k);
    //	T* tVT = mem::aligned_new < T > (k * n);

    T *tU = Udata.data();
    T *tS = Sdata.data();
    T *tVT = VTdata.data();

    // SVD
    int INFO = 0;
    char JOBU = 'S';
    char JOBVT = 'S';

    // int wssize = max(3*min(m,n)+max(m,n), 5*min(m,n));
    int wssize = 3 * (m < n ? m : n) + (m > n ? m : n);
    int wssize1 = 5 * (m < n ? m : n);
    wssize = (wssize > wssize1 ? wssize : wssize1);

    //	T* wsbuf = mem::aligned_new < T > (wssize);
    std::vector<T> wsbufdata(wssize);
    T *wsbuf = wsbufdata.data();

    svd(&JOBU, &JOBVT, &m, &n, &M[0], &m, &tS[0], &tU[0], &m, &tVT[0], &k,
        wsbuf, &wssize, &INFO);
    if (INFO != 0)
        std::cout << INFO << '\n';
    assert(INFO == 0);
    //	mem::aligned_delete < T > (wsbuf);

    T eps_ = tS[0] * eps;
    for (int i = 0; i < k; i++)
        if (tS[i] < eps_) {
            tS[i] = 0;
        } else {
            tS[i] = 1.0 / tS[i];
            //			std::cout << tS[i] << std::endl;
        }
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < k; j++) {
            tU[i + j * m] *= tS[j];
        }
    }

    gemm<T>('T', 'T', n, m, k, 1.0, &tVT[0], k, &tU[0], m, 0.0, M_, n);
    //	mem::aligned_delete < T > (tU);
    //	mem::aligned_delete < T > (tS);
    //	mem::aligned_delete < T > (tVT);
}

inline void pinv(const Eigen::MatrixXd &Mat, Eigen::MatrixXd &MatPinv) {
    double eps = 1;
    while (eps + 1.0 > 1.0) {
        eps *= 0.5;
    }
    eps = sqrt(eps);

    std::vector<double> M(Mat.cols() * Mat.rows());
    std::vector<double> Mpinv(M.size());

    // row major
    for (int i = 0; i < Mat.rows(); i++) {
        for (int j = 0; j < Mat.cols(); j++) {
            M[i * Mat.cols() + j] = Mat(i, j);
        }
    }

    pinv_pvfmm<double>(M.data(), Mat.rows(), Mat.cols(), eps, Mpinv.data());

    //#define U(i,j) U_[(i)*dim[0]+(j)]
    // row major
    for (int i = 0; i < Mat.cols(); i++) {
        for (int j = 0; j < Mat.rows(); j++) {
            MatPinv(i, j) = Mpinv[i * Mat.rows() + j];
        }
    }
}

inline void pinv(const Eigen::MatrixXd &Mat, Eigen::MatrixXd &MatPinvU,
                 Eigen::MatrixXd &MatPinvVT) {
    // this is the really backward stable SVD used by pvfmm

    double eps = 1;
    while (eps * (double)0.5 + (double)1.0 > 1.0) {
        eps *= 0.5;
    }

    std::vector<double> M(Mat.cols() * Mat.rows());
    std::vector<double> Mpinv(M.size());

    // row major
    for (int i = 0; i < Mat.rows(); i++) {
        for (int j = 0; j < Mat.cols(); j++) {
            M[i * Mat.cols() + j] = Mat(i, j);
        }
    }

    const int n1 = Mat.rows();
    const int n2 = Mat.cols();

    if (n1 * n2 == 0)
        return;
    int m = n2;
    int n = n1;
    int k = (m < n ? m : n);

    std::vector<double> Udata(m * k);
    std::vector<double> Sdata(k);
    std::vector<double> VTdata(k * n);

    //	T* tU = mem::aligned_new < T > (m * k);
    //	T* tS = mem::aligned_new < T > (k);
    //	T* tVT = mem::aligned_new < T > (k * n);

    double *tU = Udata.data();
    double *tS = Sdata.data();
    double *tVT = VTdata.data();

    // SVD
    int INFO = 0;
    char JOBU = 'S';
    char JOBVT = 'S';

    // int wssize = max(3*min(m,n)+max(m,n), 5*min(m,n));
    int wssize = 3 * (m < n ? m : n) + (m > n ? m : n);
    int wssize1 = 5 * (m < n ? m : n);
    wssize = (wssize > wssize1 ? wssize : wssize1);

    //	T* wsbuf = mem::aligned_new < T > (wssize);
    std::vector<double> wsbufdata(wssize);
    double *wsbuf = wsbufdata.data();

    svd(&JOBU, &JOBVT, &m, &n, &M[0], &m, &tS[0], &tU[0], &m, &tVT[0], &k,
        wsbuf, &wssize, &INFO);
    if (INFO != 0) {
        std::cout << INFO << '\n';
    }
    assert(INFO == 0);
    //	mem::aligned_delete < T > (wsbuf);

    double eps_ = tS[0] * eps;

    for (int i = 0; i < k; i++) {
        tS[i] = (tS[i] > eps_ * 4 ? 1.0 / tS[i] : 0.0);
    }

    //	for (int i = 0; i < k; i++)
    //		if (tS[i] < eps_) {
    //			tS[i] = 0;
    //		} else {
    //			tS[i] = 1.0 / tS[i];
    ////			std::cout << tS[i] << std::endl;
    //		}
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < k; j++) {
            tU[i + j * m] *= tS[j];
        }
    }

    MatPinvU.resize(m, k);
    MatPinvVT.resize(n, k);
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < k; j++) {
            MatPinvU(i, j) = tU[i * k + j];
        }
    }

    for (int i = 0; i < n; i++) {
        for (int j = 0; j < k; j++) {
            MatPinvVT(i, j) = tVT[i * k + j];
        }
    }

    //#define U(i,j) U_[(i)*dim[0]+(j)]
    // row major
}

#endif /* M2LLAPLACE_1D3D_SVD_PVFMM_HPP_ */


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

#include <iostream>

//[policy_eg_2

#include <boost/math/special_functions/gamma.hpp>

int main()
{
   using namespace boost::math::policies;
   errno = 0;
   std::cout << "Result of tgamma(30000) is: "
      << boost::math::tgamma(
         30000,
         make_policy(
            domain_error<errno_on_error>(),
            pole_error<errno_on_error>(),
            overflow_error<errno_on_error>(),
            evaluation_error<errno_on_error>()
         )
      ) << std::endl;
   // Check errno was set:
   std::cout << "errno = " << errno << std::endl;
   // and again with evaluation at a pole:
   std::cout << "Result of tgamma(-10) is: "
      << boost::math::tgamma(
         -10,
         make_policy(
            domain_error<errno_on_error>(),
            pole_error<errno_on_error>(),
            overflow_error<errno_on_error>(),
            evaluation_error<errno_on_error>()
         )
      ) << std::endl;
   // Check errno was set:
   std::cout << "errno = " << errno << std::endl;
}

//]


/*! @file mean.cc
 *  @brief Arithmetic and geometric means calculation.
 *  @author Markovtsev Vadim <v.markovtsev@samsung.com>
 *  @version 1.0
 *
 *  @section Notes
 *  This code partially conforms to Google C++ Style Guide.
 *
 *  @section Copyright
 *  Copyright © 2013 Samsung R&D Institute Russia
 *
 *  @section License
 *  Licensed to the Apache Software Foundation (ASF) under one
 *  or more contributor license agreements.  See the NOTICE file
 *  distributed with this work for additional information
 *  regarding copyright ownership.  The ASF licenses this file
 *  to you 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 "src/transforms/mean.h"
#include <algorithm>
#include <limits>
#include <simd/arithmetic-inl.h>
#include <simd/mathfun.h>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop

namespace sound_feature_extraction {
namespace transforms {

using formats::FixedArray;

std::set<MeanType> Parse(const std::string& value,
                         identity<std::set<MeanType>>) {
  static const std::unordered_map<std::string, MeanType> map {
    { internal::kMeanTypeArithmeticStr, kMeanTypeArithmetic },
    { internal::kMeanTypeGeometricStr, kMeanTypeGeometric },
  };

  static const boost::regex all_regex("^\\s*(\\w+\\s*(\\s|$))+");
  boost::smatch match;
  if (!boost::regex_match(value, match, all_regex)) {
    throw InvalidParameterValueException();
  }

  std::set<MeanType> ret;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                            boost::regex("\\s*(\\w+)\\s*"),
                                            1),
                 boost::sregex_token_iterator(),
                 std::inserter(ret, ret.begin()),
                 [](const std::string& subval) {
    auto mtypeit = map.find(subval);
    if (mtypeit == map.end()) {
      throw InvalidParameterValueException();
    }
    return mtypeit->second;
  });

  return ret;
}

Mean::Mean() : types_(kDefaultMeanTypes()) {
}

ALWAYS_VALID_TP(Mean, types)

void Mean::Do(const float* in,
            FixedArray<kMeanTypeCount>* out) const noexcept {
  for (int j = 0; j < kMeanTypeCount; j++) {
    auto mt = static_cast<MeanType>(j);
    if (types_.find(mt) != types_.end()) {
      (*out)[j] = Do(use_simd(), in, input_format_->Size(), mt);
    } else {
      (*out)[j] = 0;
    }
  }
}

float Mean::Do(bool simd, const float* input, size_t length,
               MeanType type) noexcept {
  int ilength = static_cast<int>(length);
  switch (type) {
    case kMeanTypeArithmetic: {
      float res;
      if (simd) {
#if defined(__AVX__) || defined(__ARM_NEON__)
        res = sum_elements(input, length);
      } else {
#else
      } {
#endif
        res = sum_elements_na(input, length);
      }
      res /= length;
      return res;
    }
    case kMeanTypeGeometric: {
      const float power = 1.f / ilength;
      if (simd) {
#ifdef __AVX__
        __m256 res = _mm256_set1_ps(1.f), tmp = _mm256_set1_ps(1.f);
        const __m256 powvec = _mm256_set1_ps(power);
        const __m256 infvec = _mm256_set1_ps(
            std::numeric_limits<float>::infinity());
        for (int j = 0; j < ilength - 7; j += 8) {
          __m256 vec = _mm256_load_ps(input + j);
          __m256 mulvec = _mm256_mul_ps(tmp, vec);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
          __m256 cmpvec = _mm256_cmp_ps(mulvec, infvec, _CMP_EQ_UQ);
          int check = _mm256_movemask_ps(cmpvec);
          if (check != 0) {
            // Taking a power from 0 can lead to unexpected results...
            // Apply the mask to workaround zeros in tmp
            cmpvec = _mm256_cmp_ps(tmp, _mm256_set1_ps(0.f), _CMP_EQ_UQ);
            tmp = pow256_ps(tmp, powvec);
            tmp = _mm256_blendv_ps(tmp, _mm256_set1_ps(0.f), cmpvec);
            res = _mm256_mul_ps(res, tmp);
            tmp = vec;
          } else {
            tmp = mulvec;
          }
#pragma GCC diagnostic pop
        }
        for (int i = 0; i < 8; i++) {
          if (_mm256_get_ps(tmp, i) == 0) {
            return 0;
          }
        }
        tmp = pow256_ps(tmp, powvec);
        res = _mm256_mul_ps(res, tmp);
        float sctmp = 1.f;
        for (int j = (ilength & ~0x7); j < ilength; j++) {
          sctmp *= input[j];
        }
        float scres = powf(sctmp, power);
        for (int j = 0; j < 8; j++) {
          scres *= _mm256_get_ps(res, j);
        }
        return scres;
      } else {
#elif defined(__ARM_NEON__)
        float32x4_t res = vdupq_n_f32(1.0f), tmp = vdupq_n_f32(1.0f),
            powvec = vdupq_n_f32(power),
            infvec = vdupq_n_f32(std::numeric_limits<float>::infinity());

        for (int j = 0; j < ilength - 3; j += 4) {
          float32x4_t vec = vld1q_f32(input + j);
          float32x4_t mulvec = vmulq_f32(tmp, vec);
          uint32x4_t cmpvec = vceqq_f32(mulvec, infvec);
          uint64x2_t cmpvec2 = vpaddlq_u32(cmpvec);
          if (vgetq_lane_u64(cmpvec2, 0) != 0 ||
              vgetq_lane_u64(cmpvec2, 1) != 0) {
            tmp = pow_ps(tmp, powvec);
            res = vmulq_f32(res, tmp);
            tmp = vec;
          } else {
            tmp = mulvec;
          }
        }
        tmp = pow_ps(tmp, powvec);
        res = vmulq_f32(res, tmp);
        float sctmp = 1.f;
        for (int j = (ilength & ~0x3); j < ilength; j++) {
          sctmp *= input[j];
        }
        float scres = powf(sctmp, power);
        scres *= vgetq_lane_f32(res, 0) * vgetq_lane_f32(res, 1) *
            vgetq_lane_f32(res, 2) * vgetq_lane_f32(res, 3);
        return scres;
      } else {
#else
      } {
#endif
        float res = 1.f, tmp = 1.f;
        for (int j = 0; j < ilength; j++) {
          float val = input[j];
          float multmp = tmp * val;
          if (multmp == std::numeric_limits<float>::infinity()) {
            res *= powf(tmp, power);
            tmp = val;
          } else {
            tmp = multmp;
          }
        }
        if (tmp != 1.f) {
          res *= powf(tmp, power);
        }
        return res;
      }
    }
    default:
      break;
  }
  return 0.f;
}

RTP(Mean, types)
REGISTER_TRANSFORM(Mean);

}  // namespace transforms
}  // namespace sound_feature_extraction


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <Eigen/Core>
#include <example-robot-data/path.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/solvers/ddp.hpp"
#include "crocoddyl/core/utils/callbacks.hpp"
#include "crocoddyl/core/utils/timer.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

using boost::make_shared;
using boost::shared_ptr;
using namespace crocoddyl;

std::tuple<shared_ptr<ActionModelAbstractTpl<double>>, shared_ptr<ActionModelAbstractTpl<double>>>
build_arm_action_models()
{
    typedef typename MathBaseTpl<double>::Vector3s Vector3s;
    typedef typename MathBaseTpl<double>::Matrix3s Matrix3s;

    // because urdf is not supported with all double types.
    pinocchio::ModelTpl<double> modeld;
    pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf", modeld);
    pinocchio::srdf::loadReferenceConfigurations(modeld, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf",
                                                 false);

    pinocchio::ModelTpl<double> model_full(modeld.cast<double>()), model;
    std::vector<pinocchio::JointIndex> locked_joints;
    locked_joints.push_back(5);
    locked_joints.push_back(6);
    locked_joints.push_back(7);
    pinocchio::buildReducedModel(model_full, locked_joints, Eigen::VectorXd::Zero(model_full.nq), model);

    shared_ptr<StateMultibodyTpl<double>> state =
        make_shared<StateMultibodyTpl<double>>(make_shared<pinocchio::ModelTpl<double>>(model));

    auto goalTrackingCost = make_shared<CostModelResidualTpl<double>>(
        state, make_shared<ResidualModelFramePlacementTpl<double>>(
                   state, model.getFrameId("gripper_left_joint"),
                   pinocchio::SE3Tpl<double>(Matrix3s::Identity(), Vector3s(double(0), double(0), double(.4)))));
    auto xRegCost = make_shared<CostModelResidualTpl<double>>(state, make_shared<ResidualModelStateTpl<double>>(state));
    auto uRegCost =
        make_shared<CostModelResidualTpl<double>>(state, make_shared<ResidualModelControlTpl<double>>(state));

    // Create a cost model per the running and terminal action model.
    auto runningCostModel = make_shared<CostModelSumTpl<double>>(state);
    auto terminalCostModel = make_shared<CostModelSumTpl<double>>(state);

    // Then let's added the running and terminal cost functions
    runningCostModel->addCost("gripperPose", goalTrackingCost, double(1));
    runningCostModel->addCost("xReg", xRegCost, double(1e-4));
    runningCostModel->addCost("uReg", uRegCost, double(1e-4));
    terminalCostModel->addCost("gripperPose", goalTrackingCost, double(1));

    // We define an actuation model
    auto actuation = make_shared<ActuationModelFullTpl<double>>(state);

    // Next, we need to create an action model for running and terminal knots. The
    // forward dynamics (computed using ABA) are implemented
    // inside DifferentialActionModelFullyActuated.
    auto runningDAM =
        make_shared<DifferentialActionModelFreeFwdDynamicsTpl<double>>(state, actuation, runningCostModel);

    auto runningModel = make_shared<IntegratedActionModelEulerTpl<double>>(runningDAM, double(1e-3));
    auto terminalModel = make_shared<IntegratedActionModelEulerTpl<double>>(runningDAM, double(0.));

    return {runningModel, terminalModel};
}

int main(int argc, char *argv[])
{
    unsigned int N = 100;  // number of nodes
    unsigned int T = 5e3;  // number of trials
    unsigned int MAXITER = 1;

    if (argc > 1)
    {
        T = atoi(argv[1]);
    }

    // Building the running and terminal models
    auto [runningModel, terminalModel] = build_arm_action_models();

    // Get the initial state
    shared_ptr<StateMultibody> state = boost::static_pointer_cast<StateMultibody>(runningModel->get_state());

    std::cout << "NQ: " << state->get_nq() << std::endl;
    std::cout << "Number of nodes: " << N << std::endl << std::endl;

    Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
    Eigen::VectorXd x0(state->get_nx());
    x0 << q0, Eigen::VectorXd::Random(state->get_nv());

    // For this optimal control problem, we define 100 knots (or running action
    // models) plus a terminal knot
    std::vector<shared_ptr<ActionModelAbstract>> runningModels(N, runningModel);
    ShootingProblem problem(x0, runningModels, terminalModel);
    std::vector<Eigen::VectorXd> xs(N + 1, x0);
    std::vector<Eigen::VectorXd> us(N, Eigen::VectorXd::Zero(runningModel->get_nu()));
    for (unsigned int i = 0; i < N; ++i)
    {
        const shared_ptr<ActionModelAbstract> &model = problem.get_runningModels()[i];
        const shared_ptr<ActionDataAbstract> &data = problem.get_runningDatas()[i];
        model->quasiStatic(data, us[i], x0);
    }

    // Formulating the optimal control problem
    SolverDDP ddp(problem);

    // Solving the optimal control problem
    Eigen::ArrayXd duration(T);
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        ddp.solve(xs, us, MAXITER, false, 0.1);
        duration[i] = timer.get_duration();
    }

    double avrg_duration = duration.sum() / T;
    double min_duration = duration.minCoeff();
    double max_duration = duration.maxCoeff();
    std::cout << "  DDP.solve [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
              << std::endl;

    // Running calc
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        problem.calc(xs, us);
        duration[i] = timer.get_duration();
    }

    avrg_duration = duration.sum() / T;
    min_duration = duration.minCoeff();
    max_duration = duration.maxCoeff();
    std::cout << "  ShootingProblem.calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
              << std::endl;

    // Running calcDiff
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        problem.calcDiff(xs, us);
        duration[i] = timer.get_duration();
    }

    avrg_duration = duration.sum() / T;
    min_duration = duration.minCoeff();
    max_duration = duration.maxCoeff();
    std::cout << "  ShootingProblem.calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration
              << ")" << std::endl;
}

// import sys

// import crocoddyl
// import numpy as np
// import example_robot_data
// import pinocchio

// # Load robot
// robot = example_robot_data.load("talos")

// # Create data structures
// rdata = robot.model.createData()
// state = crocoddyl.StateMultibody(robot.model)
// actuation = crocoddyl.ActuationModelFloatingBase(state)

// # Set integration time
// DT = 5e-2
// T = 60
// target = np.array([0.5, 0, 1.8])

// # Initialize reference state, target and reference CoM
// rightFoot = "right_sole_link"
// leftFoot = "left_sole_link"
// endEffector = "gripper_left_joint"
// endEffectorId = robot.model.getFrameId(endEffector)
// rightFootId = robot.model.getFrameId(rightFoot)
// leftFootId = robot.model.getFrameId(leftFoot)
// q0 = robot.model.referenceConfigurations["half_sitting"]
// x0 = np.concatenate([q0, np.zeros(robot.model.nv)])
// pinocchio.forwardKinematics(robot.model, rdata, q0)
// pinocchio.updateFramePlacements(robot.model, rdata)

// # Initialize Gepetto viewer
// display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
// display.robot.viewer.gui.addSphere("world/point", 0.05, [1.0, 0.0, 0.0, 1.0])
// display.robot.viewer.gui.applyConfiguration("world/point", target.tolist() + [0.0, 0.0, 0.0, 1.0])

// # Add contact to the model
// contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
// supportContactModelLeft = crocoddyl.ContactModel6D(
//     state, leftFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
// )
// contactModel.addContact(leftFoot + "_contact", supportContactModelLeft)
// supportContactModelRight = crocoddyl.ContactModel6D(
//     state, rightFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
// )
// contactModel.addContact(rightFoot + "_contact", supportContactModelRight)

// contactModelLeft = crocoddyl.ContactModelMultiple(state, actuation.nu)
// contactModelLeft.addContact(
//     "contact",
//     crocoddyl.ContactModel6D(
//         state, leftFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
//     ),
// )

// contactModelRight = crocoddyl.ContactModelMultiple(state, actuation.nu)
// contactModelRight.addContact(
//     "contact",
//     crocoddyl.ContactModel6D(
//         state, rightFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
//     ),
// )

// # Cost for self-collision
// maxfloat = sys.float_info.max
// xlb = np.concatenate(
//     [
//         -maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.lowerPositionLimit[7:],
//         -maxfloat * np.ones(state.nv),
//     ]
// )
// xub = np.concatenate(
//     [
//         maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.upperPositionLimit[7:],
//         maxfloat * np.ones(state.nv),
//     ]
// )
// bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
// xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
// limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)

// # Cost for state and control
// xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) ** 2
// )
// uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
// xTActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) ** 2
// )
// xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
// uRegCost = crocoddyl.CostModelResidual(state, uResidual)
// xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)

// # Cost for target reaching
// framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
//     state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu
// )
// framePlacementActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([1] * 3 + [0.0001] * 3) ** 2
// )
// goalTrackingCost = crocoddyl.CostModelResidual(
//     state, framePlacementActivation, framePlacementResidual
// )

// # Create cost model per each action model
// runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
// terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)

// # Then let's added the running and terminal cost functions
// runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// runningCostModel.addCost("stateReg", xRegCost, 1e-3)
// runningCostModel.addCost("ctrlReg", uRegCost, 1e-4)
// runningCostModel.addCost("limitCost", limitCost, 1e3)

// terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// terminalCostModel.addCost("stateReg", xRegTermCost, 1e-3)
// terminalCostModel.addCost("limitCost", limitCost, 1e3)

// # Create the action model
// dmodelRunningLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, runningCostModel
// )
// dmodelTerminalLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, terminalCostModel
// )

// dmodelRunning = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, runningCostModel
// )
// dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, terminalCostModel
// )
// runningModelLeft = crocoddyl.IntegratedActionModelEuler(dmodelRunningLeft, DT)
// runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT)
// terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)

// # Problem definition
// x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
// problem = crocoddyl.ShootingProblem(x0, [runningModel] * T + [runningModelLeft] * T, terminalModel)

// # Creating the DDP solver for this OC problem, defining a logger
// solver = crocoddyl.SolverFDDP(problem)
// solver.setCallbacks(
//     [
//         crocoddyl.CallbackVerbose(),
//         crocoddyl.CallbackDisplay(
//             crocoddyl.GepettoDisplay(robot, 4, 4, frameNames=[rightFoot, leftFoot])
//         ),
//     ]
// )

// # Solving it with the FDDP algorithm
// xs = [x0] * (solver.problem.T + 1)
// us = solver.problem.quasiStatic([x0] * solver.problem.T)
// solver.solve(xs, us, 500, False, 0.1)

// # Visualizing the solution in gepetto-viewer
// display.displayFromSolver(solver)

// # Get final state and end effector position
// xT = solver.xs[-1]
// pinocchio.forwardKinematics(robot.model, rdata, xT[: state.nq])
// pinocchio.updateFramePlacements(robot.model, rdata)
// com = pinocchio.centerOfMass(robot.model, rdata, xT[: state.nq])
// finalPosEff = np.array(rdata.oMf[robot.model.getFrameId("gripper_left_joint")].translation.T.flat)

// print("Finally reached = ", finalPosEff)
// print("Distance between hand and target = ", np.linalg.norm(finalPosEff - target))
// print("Distance to default state = ", np.linalg.norm(x0 - np.array(xT.flat)))
// # print("XY distance to CoM reference = ", np.linalg.norm(com[:2] - comRef[:2]))