// Author: Marc Comino 2018

#include "./mesh_io.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <math.h>

#include "./triangle_mesh.h"

namespace data_representation {

namespace {
int countF,countInv,countOutcell=0;

template <typename T>
void Add3Items(T i1, T i2, T i3, int index, std::vector<T> *vector) {
  (*vector)[index] = i1;
  (*vector)[index + 1] = i2;
  (*vector)[index + 2] = i3;
}

bool ReadPlyHeader(std::ifstream *fin, int *vertices, int *faces) {
  char line[100];

  fin->getline(line, 100);
  if (strncmp(line, "ply", 3) != 0) return false;

  *vertices = 0;
  fin->getline(line, 100);
  while (strncmp(line, "end_header", 10) != 0) {
    if (strncmp(line, "element vertex", 14) == 0) *vertices = atoi(&line[15]);
    fin->getline(line, 100);
    if (strncmp(line, "element face", 12) == 0) *faces = atoi(&line[13]);
  }

  if (*vertices <= 0) return false;

  std::cout << "Loading triangle mesh" << std::endl;
  std::cout << "\tVertices = " << *vertices << std::endl;
  std::cout << "\tFaces = " << *faces << std::endl;

  return true;
}

void ReadPlyVertices(std::ifstream *fin, TriangleMesh *mesh) {
  const int kVertices = mesh->vertices_.size() / 3;
  for (int i = 0; i < kVertices; ++i) {
    float x, y, z;
    fin->read(reinterpret_cast<char *>(&x), sizeof(float));
    fin->read(reinterpret_cast<char *>(&y), sizeof(float));
    fin->read(reinterpret_cast<char *>(&z), sizeof(float));

    Add3Items(x, y, z, i * 3, &(mesh->vertices_));
  }
}

void ReadPlyFaces(std::ifstream *fin, TriangleMesh *mesh) {
  unsigned char vertex_per_face;

  const int kFaces = mesh->faces_.size() / 3;
  for (int i = 0; i < kFaces; ++i) {
    int v1, v2, v3;
    fin->read(reinterpret_cast<char *>(&vertex_per_face),
              sizeof(unsigned char));
    assert(vertex_per_face == 3);

    fin->read(reinterpret_cast<char *>(&v1), sizeof(int));
    fin->read(reinterpret_cast<char *>(&v2), sizeof(int));
    fin->read(reinterpret_cast<char *>(&v3), sizeof(int));
    Add3Items(v1, v2, v3, i * 3, &(mesh->faces_));
  }
}
void ReadPlyVerticesM(std::ifstream *fin, std::vector<float> *vertices_) {
  const int kVertices = vertices_->size() / 3;
  for (int i = 0; i < kVertices; ++i) {
    float x, y, z;
    fin->read(reinterpret_cast<char *>(&x), sizeof(float));
    fin->read(reinterpret_cast<char *>(&y), sizeof(float));
    fin->read(reinterpret_cast<char *>(&z), sizeof(float));

    Add3Items(x, y, z, i * 3, (vertices_));
  }
}

void ReadPlyFacesM(std::ifstream *fin, std::vector<int> *faces_) {
  unsigned char vertex_per_face;

  const int kFaces = faces_->size() / 3;
  for (int i = 0; i < kFaces; ++i) {
    int v1, v2, v3;
    fin->read(reinterpret_cast<char *>(&vertex_per_face),
              sizeof(unsigned char));
    assert(vertex_per_face == 3);

    fin->read(reinterpret_cast<char *>(&v1), sizeof(int));
    fin->read(reinterpret_cast<char *>(&v2), sizeof(int));
    fin->read(reinterpret_cast<char *>(&v3), sizeof(int));
    Add3Items(v1, v2, v3, i * 3, (faces_));
  }
}

void ComputeVertexNormals(const std::vector<float> &vertices,
                          const std::vector<int> &faces,
                          std::vector<float> *normals) {
  const int kFaces = faces.size();
  std::vector<float> face_normals(kFaces, 0);

  for (int i = 0; i < kFaces; i += 3) {

    Eigen::Vector3d v1(vertices[faces[i] * 3], vertices[faces[i] * 3 + 1],
                       vertices[faces[i] * 3 + 2]);
    Eigen::Vector3d v2(vertices[faces[i + 1] * 3],
                       vertices[faces[i + 1] * 3 + 1],
                       vertices[faces[i + 1] * 3 + 2]);
    Eigen::Vector3d v3(vertices[faces[i + 2] * 3],
                       vertices[faces[i + 2] * 3 + 1],
                       vertices[faces[i + 2] * 3 + 2]);
    Eigen::Vector3d v1v2 = v2 - v1;
    Eigen::Vector3d v1v3 = v3 - v1;
    Eigen::Vector3d normal = v1v2.cross(v1v3);

    if (normal.norm() > 0) {
      normal.normalize();
    } else {
      normal = Eigen::Vector3d(0.0, 0.0, 0.0);
    }

    for (int j = 0; j < 3; ++j) face_normals[i + j] = normal[j];
  }

  const int kVertices = vertices.size();
  normals->resize(kVertices, 0);
  for (int i = 0; i < kFaces; i += 3) {
    for (int j = 0; j < 3; ++j) {
      int idx = faces[i + j];
      Eigen::Vector3d v1(vertices[faces[i + j] * 3],
                         vertices[faces[i + j] * 3 + 1],
                         vertices[faces[i + j] * 3 + 2]);
      Eigen::Vector3d v2(vertices[faces[i + (j + 1) % 3] * 3],
                         vertices[faces[i + (j + 1) % 3] * 3 + 1],
                         vertices[faces[i + (j + 1) % 3] * 3 + 2]);
      Eigen::Vector3d v3(vertices[faces[i + (j + 2) % 3] * 3],
                         vertices[faces[i + (j + 2) % 3] * 3 + 1],
                         vertices[faces[i + (j + 2) % 3] * 3 + 2]);

      Eigen::Vector3d v1v2 = v2 - v1;
      Eigen::Vector3d v1v3 = v3 - v1;
      double angle = acos(v1v2.dot(v1v3) / (v1v2.norm() * v1v3.norm()));

      if (angle == angle) {
        for (int k = 0; k < 3; ++k) {
          (*normals)[idx * 3 + k] += face_normals[i + k] * angle;
        }
      }
    }
  }

  const int kNormals = normals->size();
  for (int i = 0; i < kNormals; i += 3) {
    Eigen::Vector3d normal((*normals)[i], (*normals)[i + 1], (*normals)[i + 2]);
    if (normal.norm() > 0) {
      normal.normalize();
    } else {
      normal = Eigen::Vector3d(0, 0, 0);
    }

    for (int j = 0; j < 3; ++j) (*normals)[i + j] = normal[j];
  }
}

void ComputeBoundingBox(const std::vector<float> vertices, TriangleMesh *mesh) {
  const int kVertices = vertices.size() / 3;
  for (int i = 0; i < kVertices; ++i) {
    mesh->min_[0] = std::min(mesh->min_[0], vertices[i * 3]);
    mesh->min_[1] = std::min(mesh->min_[1], vertices[i * 3 + 1]);
    mesh->min_[2] = std::min(mesh->min_[2], vertices[i * 3 + 2]);

    mesh->max_[0] = std::max(mesh->max_[0], vertices[i * 3]);
    mesh->max_[1] = std::max(mesh->max_[1], vertices[i * 3 + 1]);
    mesh->max_[2] = std::max(mesh->max_[2], vertices[i * 3 + 2]);
  }
}

//**************Quadratic Error***********///////
std::vector<float> quadraticErrorMetrics(std::vector<int> vertex_i,TriangleMesh *mesh, int cellID, int N){

    std::vector<float> res_vert;
    Eigen::Vector4f res;
    Eigen::Matrix4f Q;
    Q << 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;

    // For each vertex in the cluster, we compute the Q matrix given by the vertex p*pt, where the 3 first components
    // are the x y z of the normal, and the forth component is the negative of the dot product of the vertex position and its normal
    // We add all the Q matrices together.
    for(int i=0;i<(int)vertex_i.size();i++){
        Eigen::Vector3f n1,v1;
        Eigen::Vector4f p1;
        Eigen::Matrix4f Qx;
        n1<<mesh->normals_[3*vertex_i[i]],mesh->normals_[3*vertex_i[i]+1],mesh->normals_[3*vertex_i[i]+2];
        v1<<mesh->vertices_[3*vertex_i[i]],mesh->vertices_[3*vertex_i[i]+1],mesh->vertices_[3*vertex_i[i]+2];
        p1<< n1[0],n1[1],n1[2],-n1.dot(v1);
        Qx=p1*p1.transpose();
        Q=Q+Qx;
    }

    //We compute the mean of all the vertices
    float vertex_mean[3]={0,0,0};
    for(int v=0;v<(int)vertex_i.size();v++){
        vertex_mean[0]=vertex_mean[0]+mesh->vertices_[3*vertex_i[v]];
        vertex_mean[1]=vertex_mean[1]+mesh->vertices_[3*vertex_i[v]+1];
        vertex_mean[2]=vertex_mean[2]+mesh->vertices_[3*vertex_i[v]+2];
    }

    vertex_mean[0]=vertex_mean[0]/vertex_i.size();
    vertex_mean[1]=vertex_mean[1]/vertex_i.size();
    vertex_mean[2]=vertex_mean[2]/vertex_i.size();

    //In order to make Q more invertible
    // We put the last row to 0 0 0 1
    Q(3,0)=0;
    Q(3,1)=0;
    Q(3,2)=0;
    Q(3,3)=1;
    // add 1 to the diagonal of the 3x3 matrix
    Q(0,0)+=1;
    Q(1,1)+=1;
    Q(2,2)+=1;
    // we substract the mean to the last column
    Q(0,3)-=vertex_mean[0];
    Q(1,3)-=vertex_mean[1];
    Q(2,3)-=vertex_mean[2];

    Eigen::Vector4f v_ (0.0,0.0,0.0,1.0);
    Eigen::Matrix4f Q_inv;
    bool inv=false;
    bool out_cell=false;
    // We try to invert Q
    Q.computeInverseWithCheck(Q_inv,inv,0.1);

    //We compute the distance between center of cell and one of the corners;
    float seg_len = sqrt(pow((mesh->max_[0]-mesh->min_[0])/(2*N),2)+pow((mesh->max_[1]-mesh->min_[1])/(2*N),2)+pow((mesh->max_[2]-mesh->min_[2])/(2*N),2));

    if(inv){ // It Q invertible
        // We compute the vertex
        res=Q_inv*v_;
        cellID=floor(cellID/6);// due to the  shape preservation algorithm
        // We check if the new vertex is within a certain distance of the cell
        int k= fmod(cellID,N);
        int j = (fmod(cellID,pow(N,2))-k)/N;
        int i =(cellID -j*N-k)/pow(N,2);

         //****CERTAIN DISTANCE FROM THE CENTER OF THE CELL DISCRIMINATION
        float minL=mesh->min_[0]+i*(mesh->max_[0]-mesh->min_[0])/N;
        float maxL=mesh->min_[0]+(i+1)*(mesh->max_[0]-mesh->min_[0])/N;
        float centerX=(minL+maxL)/2;
         minL=mesh->min_[1]+j*(mesh->max_[1]-mesh->min_[1])/N;
         maxL=mesh->min_[1]+(j+1)*(mesh->max_[1]-mesh->min_[1])/N;
        float centerY=(minL+maxL)/2;
         minL=mesh->min_[2]+k*(mesh->max_[2]-mesh->min_[2])/N;
         maxL=mesh->min_[2]+(k+1)*(mesh->max_[2]-mesh->min_[2])/N;
        float centerZ=(minL+maxL)/2;

        //distance from vertex to center of cell
        float d_r_c=sqrt(pow(centerX-res[0],2)+pow(centerY-res[1],2)+pow(centerZ-res[2],2));

        //We compare distances and decide if we keep the vertex or not, the factor is 2;
        if (d_r_c>16*seg_len/N)out_cell=true;

    }
    if(!inv) countInv++;
    if(out_cell)countOutcell++;
    if(!inv || out_cell){ // If Q not invertible or the vertex is too far from the cell we take the mean
        countF++;
        res<<vertex_mean[0],vertex_mean[1],vertex_mean[2],0;
    }

    res_vert.push_back(res(0));
    res_vert.push_back(res(1));
    res_vert.push_back(res(2));

    return res_vert;
}

int shapePreservation(int i,int j,int k, int v,TriangleMesh *mesh,int N){

    // We look at what direction is the vertex normal pointing at, and assign it to a cell subset of the original cell
    //x+ 0  // x- 1 // y+ 2 // y- 3 // z+ 4 // z- 5
    int index =6*(i*pow(N,2)+j*N+k);
    Eigen::Vector3f normal (mesh->normals_[3*v],mesh->normals_[3*v+1],mesh->normals_[3*v+2]);
    int pos=0;
    float min_angle= FLT_MAX;
    // X+
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(1.0,0.0,0.0))/normal.norm()))!=min_angle){
        pos=0;
    }
    // X-
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(-1.0,0.0,0.0))/normal.norm()))!=min_angle){
        pos=1;
    }
    // Y+
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(0.0,1.0,0.0))/normal.norm()))!=min_angle){
        pos=2;
    }
    // Y-
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(0.0,-1.0,0.0))/normal.norm()))!=min_angle){
        pos=3;
    }
    // Z+
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(0.0,0.0,1.0))/normal.norm()))!=min_angle){
        pos=4;
    }
    // Z-
    if(std::min(min_angle,(float)acos(normal.dot(Eigen::Vector3f(0.0,0.0,-1.0))/normal.norm()))!=min_angle){
        pos=5;
    }

    return index+pos;
}

void vertexCluster(int N,TriangleMesh *mesh){

    std::vector<ce_vert> cells2;
    std::vector<int> vertex_cell;
    std::vector<int> faces_simp;
    std::vector<int> faces_temp;
    std::vector<float> vertex_simp;
    cells2.clear();
    vertex_cell.clear();
    bool in=false;
    int one_per_cell=0;
    for(int v=0;v< (int) mesh->vertices_.size()/3;v++){ //Size would be different for shape preservation
        in=false;
        //We calculate in what segment of each axis of the grid the vertex is found
        int i= fmod((N*(mesh->vertices_[3*v]-mesh->min_[0])/(mesh->max_[0]-mesh->min_[0])),N);
        int j =fmod((N*(mesh->vertices_[3*v+1]-mesh->min_[1])/(mesh->max_[1]-mesh->min_[1])),N);
        int k =fmod((N*(mesh->vertices_[3*v+2]-mesh->min_[2])/(mesh->max_[2]-mesh->min_[2])),N);
        // We calculate the index of the cell where the vertex is found
       // int index_cell = i*pow(N,2)+j*N+k; // when we don't use shape preservation
        //
        int index_cell = shapePreservation(i,j,k,v,mesh,N);// We use Shape Preservation
        int ind=0;
        //We look if the cell exists, if it does, we add the vertex index, if it doesnt, we add the new cell to the vector
        for(int s=0;s<(int)cells2.size();s++){
            if(cells2[s].cell==index_cell){cells2[s].inices_vertex.push_back(v);
            in=true;
            ind=s;}
        }
        if(!in){
            ce_vert cv;
            cv.inices_vertex.push_back(v);
            cv.cell=index_cell;
            cells2.push_back(cv);
            ind=cells2.size()-1;
        }
        vertex_cell.push_back(ind);
    }
    faces_temp.clear();
    faces_temp = mesh->faces_;
    //We modify the vertex index from the faces vector with the new vertex index (cell index)
    for(int f = 0; f<(int)faces_temp.size();f++){
        faces_temp.at(f)=vertex_cell[mesh->faces_[f]];
     }
    // We create the final faces vector by keeping all the faces that have vertices in 3 different cells
    faces_simp.clear();
    for (int f=0;f<(int)faces_temp.size()/3;f++){
        if((faces_temp[3*f]!=faces_temp[3*f+1]) && (faces_temp[3*f+2]!=faces_temp[3*f+1])&&(faces_temp[3*f+2]!=faces_temp[3*f])){

                faces_simp.push_back(faces_temp[3*f]);
                faces_simp.push_back(faces_temp[3*f+1]);
                faces_simp.push_back(faces_temp[3*f+2]);
        }

    }
    //We create the new vertices
    vertex_simp.clear();
    for(int c=0;c<(int)cells2.size();c++){

        //****************Vertex Clustering*****SESSION 2*********///

        //We mean all the vertices in each cell
 /*      float vertex[3]={0,0,0};
        for(int v=0;v<(int)cells2[c].inices_vertex.size();v++){
            vertex[0]=vertex[0]+mesh->vertices_[3*cells2[c].inices_vertex[v]];
            vertex[1]=vertex[1]+mesh->vertices_[3*cells2[c].inices_vertex[v]+1];
            vertex[2]=vertex[2]+mesh->vertices_[3*cells2[c].inices_vertex[v]+2];
        }
        float h = vertex[0]/cells2[c].inices_vertex.size();
        vertex_simp.push_back(h);
        h =vertex[1]/cells2[c].inices_vertex.size();
        vertex_simp.push_back(h);
        h=vertex[2]/cells2[c].inices_vertex.size();
        vertex_simp.push_back(h);  */

        //****************Vertex Clustering: QuadraticErrorMetrics*****SESSION 3*********///

        //We pick the vertex representative using quadric error metrics for cells with more than one vertix,
        //otherwise the vertex representative is the only vertex in the cell
        std::vector<float> vert;
        vert.clear();
        if(cells2[c].inices_vertex.size()>1){
            vert= quadraticErrorMetrics(cells2[c].inices_vertex,mesh,cells2[c].cell,N);
        }
        else  if(cells2[c].inices_vertex.size()==1){
            one_per_cell++;
            vert.push_back(mesh->vertices_[3*cells2[c].inices_vertex[0]]);
            vert.push_back(mesh->vertices_[3*cells2[c].inices_vertex[0]+1]);
            vert.push_back(mesh->vertices_[3*cells2[c].inices_vertex[0]+2]);
        }
        vertex_simp.push_back(vert[0]);
        vertex_simp.push_back(vert[1]);
        vertex_simp.push_back(vert[2]); /////

    }
    std::cout<<"N: "<<std::to_string(N)<<std::endl;
    std::cout<<"Total cells with vertices: "<<std::to_string(vertex_simp.size()/3)<<std::endl;
    std::cout<<"Cells with one vertex: "<<std::to_string(one_per_cell)<<std::endl;
    std::cout<<"Cells that go to use the quadtratic error metrics: "<<std::to_string(vertex_simp.size()/3-one_per_cell)<<std::endl;
    std::cout<<"In q_error vertex go to mean: "<<std::to_string(countF)<<std::endl;
    std::cout<<"In q_error vertex go to mean due to vertex calcluated out of cell: "<<std::to_string(countOutcell)<<std::endl;
    std::cout<<"In q_error vertex go to mean due to invertible Q: "<<std::to_string(countInv)<<std::endl;
    countF=0;
    countInv=0;
    countOutcell=0;

    std::cout<<"Total vertices: "<<std::to_string(vertex_simp.size()/3)<<std::endl;
    std::cout<<"Total faces: "<<std::to_string(faces_simp.size()/3)<<std::endl;

    // We store for each LoD the faces and the vertices.
    switch(N){
    case 16:
        mesh->faces_16=faces_simp;
        mesh->vertices_16=vertex_simp;
        break;
    case 32:
        mesh->faces_32=faces_simp;
        mesh->vertices_32=vertex_simp;
        break;
    case 64:
        mesh->faces_64=faces_simp;
        mesh->vertices_64=vertex_simp;
        break;
    case 128:
        mesh->faces_128=faces_simp;
        mesh->vertices_128=vertex_simp;
        break;
    }

}

}  // namespace

bool ReadFromPly(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_.resize(vertices * 3);
  ReadPlyVertices(&fin, mesh);

  mesh->faces_.resize(faces * 3);
  ReadPlyFaces(&fin, mesh);

  fin.close();

  ComputeVertexNormals(mesh->vertices_, mesh->faces_, &mesh->normals_);
  ComputeBoundingBox(mesh->vertices_, mesh);

  vertexCluster(128,mesh);
  ComputeVertexNormals(mesh->vertices_128, mesh->faces_128, &mesh->normals_128);
  vertexCluster(64,mesh);
  ComputeVertexNormals(mesh->vertices_64, mesh->faces_64, &mesh->normals_64);
  vertexCluster(32,mesh);
  ComputeVertexNormals(mesh->vertices_32, mesh->faces_32, &mesh->normals_32);
  vertexCluster(16,mesh);
  ComputeVertexNormals(mesh->vertices_16, mesh->faces_16, &mesh->normals_16);
  return true;
}

bool ReadFromPlyMuseum4(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_.resize(vertices * 3);
  ReadPlyVertices(&fin, mesh);

  mesh->faces_.resize(faces * 3);
  ReadPlyFaces(&fin, mesh);

  fin.close();

  ComputeVertexNormals(mesh->vertices_, mesh->faces_, &mesh->normals_);
  ComputeBoundingBox(mesh->vertices_, mesh);
  return true;
}

bool ReadFromPlyMuseum0(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_16.resize(vertices * 3);
  ReadPlyVerticesM(&fin, &mesh->vertices_16);

  mesh->faces_16.resize(faces * 3);
  ReadPlyFacesM(&fin, &mesh->faces_16);

  fin.close();

  ComputeVertexNormals(mesh->vertices_16, mesh->faces_16, &mesh->normals_16);
  return true;
}
bool ReadFromPlyMuseum1(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_32.resize(vertices * 3);
  ReadPlyVerticesM(&fin,&mesh->vertices_32);

  mesh->faces_32.resize(faces * 3);
  ReadPlyFacesM(&fin, &mesh->faces_32);

  fin.close();

  ComputeVertexNormals(mesh->vertices_32, mesh->faces_32, &mesh->normals_32);
  return true;
}
bool ReadFromPlyMuseum2(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_64.resize(vertices * 3);
  ReadPlyVerticesM(&fin, &mesh->vertices_64);

  mesh->faces_64.resize(faces * 3);
  ReadPlyFacesM(&fin, &mesh->faces_64);

  fin.close();

  ComputeVertexNormals(mesh->vertices_64, mesh->faces_64, &mesh->normals_64);
  return true;
}
bool ReadFromPlyMuseum3(const std::string &filename, TriangleMesh *mesh) {
  std::ifstream fin;

  fin.open(filename.c_str(), std::ios_base::in | std::ios_base::binary);
  if (!fin.is_open() || !fin.good()) return false;

  int vertices = 0, faces = 0;
  if (!ReadPlyHeader(&fin, &vertices, &faces)) {
    fin.close();
    return false;
  }

  mesh->vertices_128.resize(vertices * 3);
  ReadPlyVerticesM(&fin, &mesh->vertices_128);

  mesh->faces_128.resize(faces * 3);
  ReadPlyFacesM(&fin, &mesh->faces_128);

  fin.close();

  ComputeVertexNormals(mesh->vertices_128, mesh->faces_128, &mesh->normals_128);
  return true;
}

bool WriteToPly(const std::string &filename, TriangleMesh &mesh,const int lod) {
//  (void)filename;
 // (void)mesh;

    std::ofstream outFile( filename.c_str(), std::ios::binary );

     if ( !outFile )
     {
         std::cerr << "Error opening output file: " << filename << "!" << std::endl;
         exit( 1 );
     }

     ////
     // Header
     ////

     int pointNum =0;
     int triangleNum=0;
     switch(lod){
     case 0:
         pointNum    = ( int ) mesh.vertices_16.size()/3;
         triangleNum = ( int ) mesh.faces_16.size()/3;
         break;
     case 1:
          pointNum    = ( int ) mesh.vertices_32.size()/3;
         triangleNum = ( int ) mesh.faces_32.size()/3;
         break;
     case 2:
         pointNum    = ( int ) mesh.vertices_64.size()/3;
         triangleNum = ( int ) mesh.faces_64.size()/3;
         break;
     case 3:
         pointNum    = ( int ) mesh.vertices_128.size()/3;
         triangleNum = ( int ) mesh.faces_128.size()/3;
         break;
     case 4:
         pointNum    = ( int ) mesh.vertices_.size()/3;
         triangleNum = ( int ) mesh.faces_.size()/3;
         break;

     }

     outFile << "ply" << std::endl;
     outFile << "format ascii 1.0" << std::endl;
     outFile << "element vertex " << pointNum << std::endl;
     outFile << "property float x" << std::endl;
     outFile << "property float y" << std::endl;
     outFile << "property float z" << std::endl;
     outFile << "element face " << triangleNum << std::endl;
     outFile << "property list uchar int vertex_index" << std::endl;
     outFile << "end_header" << std::endl;

     ////
     // Points
     ////

     ///
     // Triangles
     ////

     int fo = 3;
     switch(lod){
     case 0:
         for ( int pi = 0; pi < pointNum; pi++ )
         {
             for ( int vi = 0; vi < 3; ++vi ){
                 float val = mesh.vertices_16[3*pi+vi];
                 outFile.write(reinterpret_cast<char *>(&val ), sizeof( float ) );}
         }
         for ( int ti = 0; ti < triangleNum; ++ti )
         {
             outFile.write(reinterpret_cast<char *>(&fo), sizeof(unsigned char));
             for ( int vi = 0; vi < 3; ++vi )
                 outFile.write(const_cast<char *>(reinterpret_cast<const char *>(&mesh.faces_16[ti*3 + vi])), sizeof( int ) );
         }
         break;
     case 1:
         for ( int pi = 0; pi < pointNum; pi++ )
         {
             for ( int vi = 0; vi < 3; ++vi ){
                 float val = mesh.vertices_32[3*pi+vi];
                 outFile.write(reinterpret_cast<char *>(&val ), sizeof( float ) );}
         }
         for ( int ti = 0; ti < triangleNum; ++ti )
         {
             outFile.write(reinterpret_cast<char *>(&fo), sizeof(unsigned char));
             for ( int vi = 0; vi < 3; ++vi )
                 outFile.write(const_cast<char *>(reinterpret_cast<const char *>(&mesh.faces_32[ti*3 + vi] ) ), sizeof( int ) );
         }
         break;
     case 2:
         for ( int pi = 0; pi < pointNum; pi++ )
         {for ( int vi = 0; vi < 3; ++vi ){
                 float val = mesh.vertices_64[3*pi+vi];
                 outFile.write(reinterpret_cast<char *>(&val ), sizeof( float ) );}
         }
         for ( int ti = 0; ti < triangleNum; ++ti )
         {
             outFile.write(reinterpret_cast<char *>(&fo), sizeof(unsigned char));
             for ( int vi = 0; vi < 3; ++vi )
                 outFile.write(const_cast<char *>(reinterpret_cast<const char *>(&mesh.faces_64[ti*3 + vi] ) ), sizeof( int ) );
         }
         break;
     case 3:
         for ( int pi = 0; pi < pointNum; pi++ )
         {
             for ( int vi = 0; vi < 3; ++vi ){
                 float val = mesh.vertices_128[3*pi+vi];
                 outFile.write(reinterpret_cast<char *>(&val ), sizeof( float ) );}
         }
         for ( int ti = 0; ti < triangleNum; ++ti )
         {
             outFile.write(reinterpret_cast<char *>(&fo), sizeof(unsigned char));
             for ( int vi = 0; vi < 3; ++vi )
                 outFile.write(const_cast<char *>(reinterpret_cast<const char *>(&mesh.faces_128[ti*3 + vi] ) ), sizeof( int ) );
         }
         break;
     case 4:
         for ( int pi = 0; pi < pointNum; pi++ )
         {for ( int vi = 0; vi < 3; ++vi ){
                 float val = mesh.vertices_[3*pi+vi];
                 outFile.write(reinterpret_cast<char *>(&val ), sizeof( float ) );}
         }
         for ( int ti = 0; ti < triangleNum; ++ti )
         {
             outFile.write(reinterpret_cast<char *>(&fo), sizeof(unsigned char));
             for ( int vi = 0; vi < 3; ++vi )
                 outFile.write(const_cast<char *>(reinterpret_cast<const char *>(&mesh.faces_[ti*3 + vi]  )), sizeof( int ) );
         }
         break;

     }

     return true;
    //return false;
}

}  // namespace data_representation


#ifndef ASLAM_BACKEND_OPTIMIZER_HPP
#define ASLAM_BACKEND_OPTIMIZER_HPP

#include <sparse_block_matrix/linear_solver.h>
#include <boost/shared_ptr.hpp>
//#include <boost/function.hpp>
#include <sm/assert_macros.hpp>
#include <Eigen/Core>
#include "OptimizerOptions.hpp"
#include "backend.hpp"
#include "OptimizationProblemBase.hpp"
#include <aslam/Exceptions.hpp>
#include <sm/timing/Timer.hpp>
#include <sm/boost/null_deleter.hpp>
#include <boost/thread.hpp>

namespace aslam {
  namespace backend {
    /**
     * \class Optimizer
     *
     * A sparse Gauss-Newton/LM optimizer.
     *
     * The notation in this file follows Harley and Zisserman, Appendix 6.
     *
     * Some Additions to the standard algorithm:
     * Choice of Lambda:
     * H.B. Nielsen. Damping Parameter in Marquardt�s Method. Technical Report
     * IMM-REP-1999-05, Technical University of Denmark, 1999.
     *
     * Jacobian Normalisation:
     * Jean-Claude Trigeassou, Thierry Poinot & Sandrine Moreau (2003):
     * A Methodology for Estimation of Physical Parameters, Systems Analysis Modelling Simulation, 43:7, 925-943
     *
     * Erik Etien, Damien Halbert, and Thierry Poinot
     * Improved Jiles�Atherton Model for Least Square Identification Using Sensitivity Function Normalization
     * IEEE TRANSACTIONS ON MAGNETICS, VOL. 44, NO. 7, JULY 2008
     *
     */
    class Optimizer {
    public:
        //typedef sm::timing::Timer Timer;
      /// Swapping this to the dummy timer will disable timing
      typedef sm::timing::DummyTimer Timer;

      SM_DEFINE_EXCEPTION(Exception, aslam::Exception);

      typedef sparse_block_matrix::LinearSolver<Eigen::MatrixXd> LinearSolver;

      Optimizer(const OptimizerOptions& options = OptimizerOptions());
      virtual ~Optimizer();

      /// \brief Set up to work on the optimization problem.
      void setProblem(boost::shared_ptr<OptimizationProblemBase> problem);

      /// \brief Set up to work on the optimization problem.
      void setProblem(OptimizationProblemBase * problem, bool optimizerOwnsProblem) { setProblem(optimizerOwnsProblem ? boost::shared_ptr<OptimizationProblemBase>(problem) : boost::shared_ptr<OptimizationProblemBase>(problem, sm::null_deleter())); }

      /// \brief initialize the optimizer to run on an optimization problem.
      void initialize();

      /// \brief initialize the linear solver specified in the optimizer options.
      void initializeLinearSolver();

      /// \brief Run the optimization
      SolutionReturnValue optimize();
#ifndef QRSOLVER_DISABLED
      SolutionReturnValue optimizeDogLeg();
#endif

      /// \brief Get the optimizer options.
      OptimizerOptions& options();

      /// \brief Build the GaussNewton matrices from the optimization problem.
      void buildMatrices();

      /// \brief return the full Hessian
      const SparseBlockMatrix& H() const;

      /// \brief return the full rhs
      const Eigen::VectorXd& rhs() const;
      inline const Eigen::VectorXd& epsilon() const {
        return rhs();
      }

      /// \brief return the reduced system lhs
      const SparseBlockMatrix& A() const;

      /// \brief return the reduced system rhs
      const Eigen::VectorXd& b() const;

      /// \brief return the reduced system dx
      const Eigen::VectorXd& dx() const;

      /// The value of the objective function.
      double J() const;

      /// \brief compute the full covariance matrix. This is expensive.
      void computeCovariances();

      /// \brief compute only the diagonal covariance blocks.
      void computeDiagonalCovariances();

      /// \brief compute only the covariance blocks associated with the block indices passed as an argument
      void computeCovarianceBlocks(const std::vector<std::pair<int, int> >& blockIndices);

      /// \brief get a particular covariance block. If the block has not been computed, this will return NULL.
      const Eigen::MatrixXd* getCovarianceBlock(int blockRow, int blockCol) const;

      /// \brief get the covariance matrix as a sparse matrix. This matrix will only be filled in by what was computed above.
      const SparseBlockMatrix& P() const;

      /// \brief get the covariance matrix as a sparse matrix. This matrix will only be filled in by what was computed above.
      const SparseBlockMatrix& getCovariance() const;

      /// \brief Evaluate the error at the current state.
      double evaluateError();

      /// \brief Get dense design variable i.
      DesignVariable* denseVariable(size_t i);
      /// \brief Get sparse design variable i
      DesignVariable* sparseVariable(size_t i);

      /// \brief how many dense design variables are involved in the problem
      size_t numDenseDesignVariables() const;

      /// \brief how many sparse design variables are involved in the problem
      size_t numSparseDesignVariables() const;

      /// \brief print the internal timing information.
      void printTiming() const;

      /// \brief Do a bunch of checks to see if the problem is well-defined. This includes checking that every error term is
      ///        hooked up to design variables and running finite differences on error terms where this is possible.
      void checkProblemSetup();

      /// \brief Build the Gauss-Newton matrices.
      void buildGnMatrices();

      double applyNormalizedStateUpdate();
      void normalizeGnMatrices();
      void initialiseDesignVariableScales();
      SolutionReturnValue optimizeNormalized();
      const Eigen::VectorXd& getDvScales() const;

    private:

      /// \brief Zero the Gauss-Newton matrices.
      void zeroMatrices();

      /// \brief Revert the last state update.
      void revertLastStateUpdate();

      /// \brief Apply a state update.
      double applyStateUpdate();

      /// \brief Returns Rho for the LM lambda update
      double getLmRho();

      /// \brief Set the initial lambda by looking at the entries of the Hessian matrix.
      void setInitialLambda();

      /// \brief The current value of LM lambda.
      double _lambda;

      /// \brief The full Hessian matrix.
      SparseBlockMatrix _H;

      /// \brief The inverse of the Hessian matrix (the covariance).
      SparseBlockMatrix _invH;

      /// \brief The rhs of Gauss-Newton.
      Eigen::VectorXd _rhs;

      /// \brief The dense update vector.
      Eigen::VectorXd _dx;

      Eigen::VectorXd designVariableScales;

      // These next three items are not necessary as part of the
      // state but we want to use them for debugging/visualization/plotting.
      /// \brief The marginalized dense matrix.
      SparseBlockMatrix _A;

      /// \brief The rhs of the marginalized system of equations.
      Eigen::VectorXd _b;

      /// \brief the inverted blocks of the sparse lhs.
      std::vector<Eigen::MatrixXd> _invVi;

      /// \brief The current value of the cost function.
      double _J;
      /// \brief The previous value of the cost function.
      double _p_J;

      /// \brief The sparse linear solver.
      boost::shared_ptr<LinearSolver> _solver;
      boost::shared_ptr<LinearSolver> _fallbackSolver;

      /// \brief The current optimization problem.
      boost::shared_ptr<OptimizationProblemBase> _problem;

      /// \brief all design variables...first the non-marginalized ones (the dense ones), then the marginalized ones.
      std::vector<DesignVariable*> _designVariables;

      /// \brief all of the error terms involved in this problem
      std::set<ErrorTerm*> _errorTerms;

      /// \brief an index into the _designVariables member that tells where the first marginalized design variable lives.
      int _marginalizedStartingBlock;

      /// \brief the current set of options
      OptimizerOptions _options;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_OPTIMIZER_HPP */


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check mlog2two2nmb behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mlog2two2nmb;
  using boost::simd::Mlog2two2nmb;
  T ref = T(0);
  STF_TYPE_IS(decltype(Mlog2two2nmb<T>()), T);
  STF_EQUAL(Mlog2two2nmb<T>(), ref);
  STF_EQUAL(mlog2two2nmb( as(T{}) ),ref);
}

STF_CASE_TPL( "Check mlog2two2nmb behavior for double"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mlog2two2nmb;
  using boost::simd::Mlog2two2nmb;
  using boost::simd::Nbmantissabits;
  T ref = -T(Nbmantissabits<T>());

  STF_TYPE_IS(decltype(Mlog2two2nmb<T>()), T);
  STF_IEEE_EQUAL(Mlog2two2nmb<T>(), ref);
  STF_IEEE_EQUAL(mlog2two2nmb( as(T{}) ), ref);
}


/*
 * liblager_recognize.cc
 *
 *  Created on: Mar 2, 2015
 *      Author: Andrés Odio
 */

#include <algorithm>  // for std::min_element
using std::min_element;
#include <boost/math/common_factor.hpp>
#include <chrono>
using std::chrono::duration;
using std::chrono::microseconds;
using std::chrono::seconds;
using std::chrono::system_clock;
using std::chrono::time_point;
#include <iostream>
using std::cout;
using std::endl;
using std::string;
using std::stringstream;
using std::fixed;
#include <iomanip>
using std::setprecision;
#include <Python.h>

#include "liblager_connect.h"
#include "liblager_recognize.h"
#include "string_tokenizer.h"

LagerRecognizer* LagerRecognizer::instance_ = NULL;

LagerRecognizer* LagerRecognizer::Instance(vector<struct SubscribedGesture>* subscribed_gestures) {
  if (!instance_) {
    instance_ = new LagerRecognizer(subscribed_gestures);
  }

  return instance_;
}

#define d(i,j) dd[(i) * (m+2) + (j) ]
#define min(x,y) ((x) < (y) ? (x) : (y))
#define min3(a,b,c) ((a)< (b) ? min((a),(c)) : min((b),(c)))
#define min4(a,b,c,d) ((a)< (b) ? min3((a),(c),(d)) : min3((b),(c),(d)))

/* Calculates the Damerau-Levenshtein distance between two strings.
 * Based on implementation at: http://stackoverflow.com/a/10741694
 */
int DLDistance(const char *s, const char* t, int n, int m) {
  int *dd, *DA;
  int i, j, cost, k, i1, j1, DB;
  int infinity = n + m;

  DA = (int*) malloc(256 * sizeof(int));
  dd = (int*) malloc((n + 2) * (m + 2) * sizeof(int));

  d(0,0)= infinity;
  for (i = 0; i < n + 1; i++) {
    d(i+1,1)= i;
    d(i+1,0) = infinity;
  }
  for (j = 0; j < m + 1; j++) {
    d(1,j+1)= j;
    d(0,j+1) = infinity;
  }
  for (k = 0; k < 256; k++)
    DA[k] = 0;
  for (i = 1; i < n + 1; i++) {
    DB = 0;
    for (j = 1; j < m + 1; j++) {
      i1 = DA[t[j - 1]];
      j1 = DB;
      cost = ((s[i - 1] == t[j - 1]) ? 0 : 1);
      if (cost == 0)
        DB = j;
      d(i+1,j+1)=
      min4(d(i,j)+cost,
          d(i+1,j) + 1,
          d(i,j+1)+1,
          d(i1,j1) + (i-i1-1) + 1 + (j-j1-1));
    }
    DA[s[i - 1]] = i;
  }
  cost = d(n + 1, m + 1);
  free(dd);
  return cost;
}

/* New size must be a multiple of the original string size */
string ExpandString(const string& input_string, int new_size) {
  stringstream output_string;
  int length_multiplier = new_size / input_string.length();

  vector<string> movement_pairs;
  TokenizeString(input_string, movement_pairs, ".");

  for (vector<string>::iterator it = movement_pairs.begin();
      it < movement_pairs.end(); ++it) {
    for (int i = 0; i < length_multiplier; i++) {
      output_string << *it << ".";
    }
  }

  return output_string.str();
}

bool GestureEntryLessThan(SubscribedGesture i, SubscribedGesture j) {
  return (i.distance_pct < j.distance_pct);
}

void LagerRecognizer::UpdateSubscribedGestureDistance(
    struct SubscribedGesture& subscribed_gesture, string current_gesture) {
  const string input_gesture = current_gesture;

  int gesture_length_least_common_multiple = boost::math::lcm(
      input_gesture.length(), subscribed_gesture.lager.length());

  string expanded_input_string = ExpandString(
      input_gesture, gesture_length_least_common_multiple);
  subscribed_gesture.expanded_lager = ExpandString(
      subscribed_gesture.lager, gesture_length_least_common_multiple);

  subscribed_gesture.distance = DLDistance(
      expanded_input_string.c_str(), subscribed_gesture.expanded_lager.c_str(),
      expanded_input_string.length(),
      subscribed_gesture.expanded_lager.length());

  subscribed_gesture.distance_pct = (subscribed_gesture.distance * 100.0f)
      / subscribed_gesture.expanded_lager.length();
}

void LagerRecognizer::UpdateSubscribedGestureDistances(string current_gesture) {
  for (vector<SubscribedGesture>::iterator it = subscribed_gestures_->begin();
      it < subscribed_gestures_->end(); ++it) {
    UpdateSubscribedGestureDistance(*it, current_gesture);
  }
}

bool LagerRecognizer::IsSingleSensorGesture(string current_gesture) {
  vector<string> movement_pairs;
  TokenizeString(current_gesture, movement_pairs, ".");
  bool sensor_0_moved = false;
  bool sensor_1_moved = false;

  for (vector<string>::iterator it = movement_pairs.begin();
      it < movement_pairs.end(); ++it) {
    // Check if sensor 0 movement is present
    if ((*it).c_str()[0] != '_') {
      sensor_0_moved = true;
    }

    // Check if sensor 0 movement is present
    if ((*it).c_str()[0] != '_') {
      sensor_1_moved = true;
    }

    // If we already found movement on both sensors, there is no need to keep checking
    if (sensor_0_moved && sensor_1_moved) {
      break;
    }
  }

  return (!sensor_0_moved || !sensor_1_moved);
}

int LagerRecognizer::GetMillisecondsUntilNow(
    const time_point<system_clock> &last_time) {
  time_point<system_clock> now = system_clock::now();
  return duration<double, std::milli>(now - last_time).count();
}

void LagerRecognizer::PrintRecognitionResults(
    struct SubscribedGesture& closest_gesture,
    int gesture_distance_threshold_pct,
    time_point<system_clock> recognition_start_time, bool match_found) {
  unsigned int num_milliseconds_since_recognition_start =
      GetMillisecondsUntilNow(recognition_start_time);

  cout << "Distances" << endl;
  cout << "---------" << endl;

  for (vector<SubscribedGesture>::iterator it = subscribed_gestures_->begin();
       it < subscribed_gestures_->end(); ++it) {
    cout << "  "
         << std::left << std::setw(15)
         << it->name << " : "
         << setprecision (2) << fixed
         << it->distance_pct << " % ("
         << it->distance << " D-L ops)" << endl;
  }

  cout << endl;

  if (match_found) {
    cout << " ________________________________ " << endl;
    cout << "|                                |" << endl;
    cout << "|      DAMERAU-LEVENSHTEIN       |" << endl;
    cout << "|          MATCH FOUND!          |" << endl;
    cout << "|________________________________|" << endl;
    cout << "                                  " << endl;
  } else {
    cout << endl;
    cout << "NO MATCH." << endl;
  }

  cout << endl;
  cout << "Closest gesture:\t" << closest_gesture.name << endl;
  cout << endl;
  cout << "Distance:\t\t" << closest_gesture.distance_pct << " % ("
      << closest_gesture.distance << " D-L ops)" << endl;
  cout << "Threshold:\t\t" << gesture_distance_threshold_pct << " %" << endl;
  cout << endl;
  cout << "Recognition time: \t" << num_milliseconds_since_recognition_start
       << " ms" << endl;
  cout << endl << endl;
}

void LagerRecognizer::PrintMlRecognitionResults(
    struct SubscribedGesture& closest_gesture,
    long recognition_probability,
    int gesture_probability_threshold_pct,
    long recognition_time,
    bool match_found) {

  if (match_found) {
    cout << " ________________________________ " << endl;
    cout << "|                                |" << endl;
    cout << "|         DEEP LEARNING          |" << endl;
    cout << "|          MATCH FOUND!          |" << endl;
    cout << "|________________________________|" << endl;
    cout << "                                  " << endl;
  } else {
    cout << endl;
    cout << "NO MATCH." << endl;
  }

  cout << endl;
  cout << "Closest gesture:\t" << closest_gesture.name << endl;
  cout << endl;
  cout << "Probability:\t\t" << recognition_probability << " %" << endl;
  cout << "Threshold:\t\t" << gesture_probability_threshold_pct << " %" << endl;
  cout << endl;
  cout << "Recognition time: \t" << recognition_time << " ms" << endl;
  cout << endl;
}

struct SubscribedGesture LagerRecognizer::RecognizeGesture(
    bool draw_gestures, string current_gesture,
    bool& match_found) {
  int lowestDistance;

  cout << " ________________________________ " << endl;
  cout << "|                                |" << endl;
  cout << "|      DAMERAU-LEVENSHTEIN       |" << endl;
  cout << "|          RECOGNIZER            |" << endl;
  cout << "|________________________________|" << endl;
  cout << "                                  " << endl;

  time_point<system_clock> recognition_start_time = system_clock::now();
  int gesture_distance_threshold_pct =
      IsSingleSensorGesture(current_gesture) ?
          SINGLE_SENSOR_GESTURE_DISTANCE_THRESHOLD_PCT :
          DUAL_SENSOR_GESTURE_DISTANCE_THRESHOLD_PCT;

  UpdateSubscribedGestureDistances(current_gesture);

  SubscribedGesture closest_gesture = *min_element(subscribed_gestures_->begin(),
                                                   subscribed_gestures_->end(),
                                                   GestureEntryLessThan);
  match_found = closest_gesture.distance_pct <= gesture_distance_threshold_pct;

  PrintRecognitionResults(closest_gesture, gesture_distance_threshold_pct,
                          recognition_start_time, match_found);

  return closest_gesture;
}

struct SubscribedGesture LagerRecognizer::RecognizeGestureML(
    string current_gesture,
    bool& match_found) {

  cout << " ________________________________ " << endl;
  cout << "|                                |" << endl;
  cout << "|         DEEP LEARNING          |" << endl;
  cout << "|           RECOGNIZER           |" << endl;
  cout << "|________________________________|" << endl;
  cout << "                                  " << endl;

  struct PythonClassifierResult result = CallPythonClassifier(ml_classifier_, current_gesture);

  match_found = ((result.gesture_index >= 0) && (result.probability > ML_RECOGNITION_THRESHOLD_PCT));

  if (result.gesture_index < 0) {
    cout << "Error returned from ML classifier. Gesture index: " << result.gesture_index;
    result.gesture_index = 0;
  }

  SubscribedGesture recognized_gesture = (*subscribed_gestures_)[result.gesture_index];

  PrintMlRecognitionResults(recognized_gesture, result.probability, ML_RECOGNITION_THRESHOLD_PCT, result.elapsed_time, match_found);

  return recognized_gesture;
}

PyObject* LagerRecognizer::InitializePythonClassifier() {
  PyObject *pName, *pModule, *pFunc;

  string python_module_name = "ml_recognizer";
  string python_function_name = "main";

  Py_Initialize();
  pName = PyUnicode_DecodeFSDefault(python_module_name.c_str());
  pModule = PyImport_Import(pName);
  Py_DECREF(pName);

  if (pModule != NULL) {
    pFunc = PyObject_GetAttrString(pModule, python_function_name.c_str());
    /* pFunc is a new reference */

    int num_args = 0;
    if (pFunc && PyCallable_Check(pFunc)) {
      return pFunc;
    }
    else {
      if (PyErr_Occurred())
          PyErr_Print();
      fprintf(stderr, "Cannot find function \"%s\"\n", python_function_name.c_str());
      return NULL;
    }
  }
  else {
    PyErr_Print();
    fprintf(stderr, "Failed to load \"%s\"\n", python_module_name.c_str());
    return NULL;
  }

}

struct PythonClassifierResult LagerRecognizer::CallPythonClassifier(
    PyObject* python_classifier,
    string current_gesture) {

  PyObject *pArgs, *pValue;
  PyObject *pReturnValue;
  struct PythonClassifierResult result;
  result.gesture_index = -1;
  result.probability = 0.0;

  if (python_classifier && PyCallable_Check(python_classifier)) {
      pArgs = PyTuple_New(1);

      pValue = PyUnicode_FromString(current_gesture.c_str());
      if (!pValue) {
          Py_DECREF(pArgs);
          fprintf(stderr, "Cannot convert argument\n");
          return result;
      }
      /* pValue reference stolen here: */
      PyTuple_SetItem(pArgs, 0, pValue);

      pValue = PyObject_CallObject(python_classifier, pArgs);
      Py_DECREF(pArgs);

      if ((pValue != NULL) && PyTuple_Check(pValue) && (PyTuple_Size(pValue) == 3)){
          result.gesture_index = PyLong_AsLong(PyTuple_GetItem(pValue, 0));
          result.probability = PyFloat_AsDouble(PyTuple_GetItem(pValue, 1));
          result.elapsed_time = PyLong_AsLong(PyTuple_GetItem(pValue, 2));
          Py_DECREF(pValue);
      }
      else {
          PyErr_Print();
          fprintf(stderr,"Call failed\n");
      }
  }
  else {
      if (PyErr_Occurred())
          PyErr_Print();
      fprintf(stderr, "Cannot find function \n");
  }

  return result;
}


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

#include <cmath>
#include <limits>
#include <sstream>

// Helpful functions from libsm
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

TEST(QuaternionAlgebraTestSuite, testRotation) {
    try {
        using namespace sm::kinematics;
        for (int i = 0; i < 1000; i++) {
            // Create a random quaternion
            Eigen::Vector4d q_a_b = quatRandom();

            // Create a random point in R3
            Eigen::Vector3d v_b;
            v_b.setRandom();

            Eigen::Vector3d v_a1 = quat2r(q_a_b) * v_b;
            Eigen::Vector3d v_a2 = quatRotate(q_a_b, v_b);

            sm::eigen::assertNear(v_a1, v_a2, 1e-10, SM_SOURCE_FILE_POS,
                                  "The rotation matrix And shortcut rotations are not equal");
        }
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

TEST(QuaternionAlgebraTestSuite, testAxisAngle2QuatAndBackOnSpecialValues) {
    const double eps = std::numeric_limits<double>::epsilon();
    const double sq1o2 = sqrt(1.0 / 2);
    try {
        std::pair<Eigen::Vector3d, Eigen::Vector4d> testPairs[] = {
            {{0, 0, 0}, {0, 0, 0, 1}},
            {{6.366197723675814e-10, 0, 0}, {1E-9, 0, 0, 1}},
            {{0.5, 0, 0}, {sq1o2, 0, 0, sq1o2}},
            {{1, 0, 0}, {1, 0, 0, 0}},
            {{1.5, 0, 0}, {sq1o2, 0, 0, -sq1o2}},
            {{2, 0, 0}, {0, 0, 0, -1}},
            {{-1, 0, 0}, {-1, 0, 0, 0}},
            {{-2, 0, 0}, {0, 0, 0, -1}},
            {{0, 0.5, 0}, {0, sq1o2, 0, sq1o2}},
            {{0, 1, 0}, {0, 1, 0, 0}},
            {{0, 1.5, 0}, {0, sq1o2, 0, -sq1o2}},
            {{0, 2, 0}, {0, 0, 0, -1}},
            {{0, -1, 0}, {0, -1, 0, 0}},
            {{0, -2, 0}, {0, 0, 0, -1}},
        };
        using namespace sm::kinematics;
        for (auto p : testPairs) {
            sm::eigen::assertNear(p.second, axisAngle2quat(p.first * M_PI), eps, SM_SOURCE_FILE_POS, "");
            if (p.second(3) != -1.0) {
                sm::eigen::assertNear(p.first, quat2AxisAngle(p.second) / M_PI, eps, SM_SOURCE_FILE_POS, "");
                sm::eigen::assertNear(p.second, axisAngle2quat(quat2AxisAngle(p.second)), eps, SM_SOURCE_FILE_POS, "");
                sm::eigen::assertNear(p.first, quat2AxisAngle(axisAngle2quat(p.first)), 2 * eps, SM_SOURCE_FILE_POS,
                                      "");
            }
        }
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

class Stringer {
  public:
    operator const char*() { return ss.str().c_str(); }
    operator const std::string() { return ss.str(); }

    template <typename T>
    Stringer& operator<<(const T& something) {
        ss << something;
        return *this;
    }

  private:
    std::stringstream ss;
};

TEST(QuaternionAlgebraTestSuite, testAxisAngle2QuatAndBack) {
    const double eps = std::numeric_limits<double>::epsilon();
    try {
        using namespace sm::kinematics;
        for (int i = 0; i < 1000; i++) {
            // Create a random quaternion
            Eigen::Vector4d q = quatRandom();
            sm::eigen::assertNear(q, axisAngle2quat(quat2AxisAngle(q)), q.block<3, 1>(0, 0).norm() * eps * 20,
                                  SM_SOURCE_FILE_POS, "");

            // Create a random point in R3
            Eigen::Vector3d v_b;
            v_b.setRandom();
            double tolerance;
            if (i % 2 == 0) {
                if (i % 4 == 0) {
                    // make use of the special code for quaternions close to identity more likely!
                    v_b *= 1E-20;
                } else {
                    // go very close to full rotation (but less than).
                    v_b *= (M_PI * 2 - 8 * eps) / v_b.norm();
                }
            }
            tolerance = v_b.norm() * eps * 40;
            sm::eigen::assertNear(
                v_b, quat2AxisAngle(axisAngle2quat(v_b)), tolerance, SM_SOURCE_FILE_POS,
                Stringer() << "axisAngle2QuatAndBack is too little accurate : \naA2Q(v_b)=" << axisAngle2quat(v_b)
                           << "\nv_b.norm() - 2Pi=" << (v_b.norm() - M_PI * 2));
        }
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

TEST(QuaternionAlgebraTestSuite, testExpJacobian) {
    Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
    using namespace sm::kinematics;
    for (int i = 0; i < 100; i++) {
        const Eigen::Vector3d v = Eigen::Vector3d::Random();
        sm::eigen::assertNear(quatExpJacobian(v),
                              sm::eigen::numericalDiff<Eigen::Vector4d, Eigen::Vector3d>(
                                  [&v](const Eigen::Vector3d& x) { return axisAngle2quat(v + x); }, zero3),
                              1E-7, SM_SOURCE_FILE_POS, "");
    }
}

TEST(QuaternionAlgebraTestSuite, testLogJacobian) {
    Eigen::Vector4d zero4 = Eigen::Vector4d::Zero();
    using namespace sm::kinematics;
    for (int i = 0; i < 100; i++) {
        const Eigen::Vector4d q = quatRandom();
        sm::eigen::assertNear(quatLogJacobian2(q),
                              sm::eigen::numericalDiff<Eigen::Vector3d, Eigen::Vector4d>(
                                  [&q](const Eigen::Vector4d& x) {
                                      return quat2AxisAngle(qplus(axisAngle2quat(2 * qeps(qplus(x, quatInv(q)))), q));
                                  },
                                  zero4),
                              1E-7, SM_SOURCE_FILE_POS, "");
        // TODO figure out how quatLogJacobian was meant to work and write test (the following does not work)
        // sm::eigen::assertNear(quatLogJacobian(q), sm::eigen::numericalDiff<Eigen::Vector3d, Eigen::Vector4d >(
        // [&q](const Eigen::Vector4d & x) { return  quat2AxisAngle(qplus(axisAngle2quat(2 * qeps(qplus(x,
        // quatInv(q)))), q)); }, zero4), 1E-7, SM_SOURCE_FILE_POS,  "");
    }
}


#include <boost/functional/hash.hpp>
#include "common/time.h"
#include "common/type.h"

#include <algorithm>
#include <vector>
#include <string>

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cassert>
#include <cmath>

using namespace std;

long long cand_num;
long long res_num;

const int JACCARD = 1;
const int COSINE = 2;
const int DICE = 3;

struct category
{
	static int N;
	static int METHOD;
	static double THRESHOLD;

	// [s_len, e_len]
	int s_len;
	int e_len;

	int N1, N2;
	int K, K2;

	int **range_start;
	int **range_end;

	int sig_len;
	vector<vector<int>> subs;
	HashMap<int, vector<int>> *sig_map;

	void init(int len)
	{
		s_len = len;
		if (METHOD == JACCARD)
		{
			e_len = s_len / THRESHOLD;
			K = 2 * (1 - THRESHOLD) / (1 + THRESHOLD) * e_len;
		} else if (METHOD == COSINE)
		{
			e_len = s_len / THRESHOLD / THRESHOLD;
			K = 2 * e_len * (1 - THRESHOLD);
		} else if (METHOD == DICE)
		{
			e_len = s_len * (2 - THRESHOLD) / THRESHOLD;
			K = 2 * e_len * (1 - THRESHOLD);
		}

		N1 = K + 1;
		N2 = 2;

		K2 = (K + 1) / N1 - 1;

		// important fix
		if ((K + 1) % N1 != 0) K2++;

		if (N1 > K + 1 || N1 * N2 <= K + 1)
		{
			exit(-3);
		}

		int n = N2;
		int k = N2 - K2;
		vector<int> sub(k);

		int s;
		for (s = 0; s < k; s++)
			sub[s] = s;
		subs.push_back(sub);

		while (sub[0] < n - k) {
			for (s = 0; s < k; s++)
				if (sub[k - s - 1] < n - s - 1)
					break;
			s = k - s - 1;
			sub[s]++;
			s++;
			for (; s < k; s++)
				sub[s] = sub[s - 1] + 1;
			subs.push_back(sub);
		}

		sig_len = N1 * subs.size();
		sig_map = new HashMap<int, vector<int>>[sig_len];

		int *t = new int[N1 * N2];
		range_start = new int*[N1];
		for (int k = 0; k < N1; k++)
			range_start[k] = t + k * N2;

		t = new int[N1 * N2];
		range_end = new int*[N1];
		for (int k = 0; k < N1; k++)
			range_end[k] = t + k * N2;

		for (int i = 0; i < N1; i++)
			for (int j = 0; j < N2; j++)
			{
				range_start[i][j] = N * (N2 * i + j) / N1 / N2;
				range_end[i][j] = N * (N2 * i + j + 1) / N1 / N2;
			}
	}
};

int category::N;
int category::METHOD;
double category::THRESHOLD;

const int MAX_LEN = 3300;
const int MAX_CATEGORY = 100;
category helper[MAX_CATEGORY];

int check_overlap(vector<int> &a, vector<int> &b, int overlap)
{
	int posa = 0, posb = 0, count = 0;
	while (posa < (int)a.size() && posb < (int)b.size())
	{
		if (count + min((int)a.size() - posa, (int)b.size() - posb) < overlap)
			return -1;
		if (a[posa] == b[posb])
		{
			count++;
			posa++;
			posb++;
		} else if (a[posa] < b[posb])
		{
			posa++;
		} else {
			posb++;
		}
	}
	return count;
}

bool jac(vector<int> &a, vector<int> &b, double threshold)
{
	double factor = category::THRESHOLD / (1 + category::THRESHOLD);
	int require_overlap = ceil(factor * (a.size() + b.size()) - 1e-6);
	int real_overlap = check_overlap(a, b, require_overlap);

	if (real_overlap == -1) return false;
	return (real_overlap / (double)(a.size() + b.size() - real_overlap)) >= threshold;
}

bool cosine(vector<int> &a, vector<int> &b, double threshold)
{
	int require_overlap = ceil(category::THRESHOLD * sqrt(a.size() * b.size()) - 1e-6);
	int real_overlap = check_overlap(a, b, require_overlap);

	if (real_overlap == -1) return false;
	return (real_overlap / sqrt(a.size() * b.size())) >= threshold;
}

bool dice(vector<int> &a, vector<int> &b, double threshold)
{
	int require_overlap = ceil(category::THRESHOLD / 2.0 * (a.size() + b.size()) - 1e-6);
	int real_overlap = check_overlap(a, b, require_overlap);

	if (real_overlap == -1) return false;
	return (2 * real_overlap / (double)(a.size() + b.size())) >= threshold;
}

bool (*verify)(vector<int> &a, vector<int> &b, double threshold);

void perform_join(int k, vector<vector<int>> &token, int id, bool *checked_flag)
{
	static boost::hash<vector<int>> vector_hash;

	int index = -1;
	vector<int> prepare;
	for (int i = 0; i < helper[k].N1; i++)
		for (vector<vector<int>>::iterator sub = helper[k].subs.begin(); sub != helper[k].subs.end(); sub++)
		{
			index++;
			prepare.clear();
			for (vector<int>::iterator j = sub->begin(); j != sub->end(); j++)
			{
				vector<int>::iterator s_pos = lower_bound(token[id].begin(), token[id].end(), helper[k].range_start[i][*j]);
				vector<int>::iterator e_pos = lower_bound(token[id].begin(), token[id].end(), helper[k].range_end[i][*j]);
				while (s_pos != e_pos)
				{
					prepare.push_back(*s_pos);
					s_pos++;
				}
			}

			int hash_value = vector_hash(prepare);
			vector<int> &candidate = helper[k].sig_map[index][hash_value];
			for (vector<int>::iterator cand_id = candidate.begin(); cand_id != candidate.end(); cand_id++)
			{
				if (!checked_flag[*cand_id])
				{
					cand_num++;
					checked_flag[*cand_id] = 1;
					if (verify(token[*cand_id], token[id], category::THRESHOLD))
					{
						res_num++;
					}
				}
			}

			if (candidate.size() == 0 ||
					candidate.back() != id)
			{
				candidate.push_back(id);
			}
		}
}

void convert_to_signature(vector<vector<int>> &token)
{
	bool *checked_flag = new bool[token.size()];
	for (int id = 0; id < (int)token.size(); id++)
	{
		int k;
		for (k = 0; k < MAX_CATEGORY; k++)
			if (helper[k].s_len <= (int)token[id].size()
					&& helper[k].e_len >= (int)token[id].size())
				break;

		assert(k < MAX_CATEGORY);
		memset(checked_flag, 0, id);

		perform_join(k, token, id, checked_flag);
		perform_join(k + 1, token, id, checked_flag);
	}
	delete[] checked_flag;
}

int main(int argc, char **argv)
{
	if (argc != 4) return -1;
	log_start();

	if (argv[1][0] == 'j')
	{
		category::METHOD = JACCARD;
		verify = jac;
	} else if (argv[1][0] == 'c') {
		category::METHOD = COSINE;
		verify = cosine;
	} else if (argv[1][0] == 'd') {
		category::METHOD = DICE;
		verify = dice;
	}

	FILE *in = fopen(argv[2], "r");
	category::THRESHOLD = atof(argv[3]);

	int count;
	vector<vector<int>> token;
	while (fread(&count, sizeof(int), 1, in))
	{
		fread(&count, sizeof(int), 1, in);
		token.push_back(vector<int>(count));
		fread(token.back().data(), sizeof(int), count, in);
		if (token.back().back() + 1 > category::N)
		{
			category::N = token.back().back() + 1;
		}
	}

	int len = 1;
	for (int k = 0; k < MAX_CATEGORY; k++)
	{
		helper[k].init(len);
		len = helper[k].e_len + 1;
		if (len > MAX_LEN) break;
	}

	convert_to_signature(token);
	printf("%lld\n", res_num);
	printf("%lld\n", cand_num);

	return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE cartesian_product
#include <boost/test/unit_test.hpp>

#include <type_traits>
#include <cartesian_product.hpp>

template<typename...> struct List;

BOOST_AUTO_TEST_CASE(empty_list)
{
	BOOST_CHECK( (std::is_same<typename meta::cartesian_product<List<>>::type, List<>>::value == true) );
}

BOOST_AUTO_TEST_CASE(one_list)
{
	BOOST_CHECK( (
		std::is_same<
			typename meta::cartesian_product<
				List<int,float,bool>
			>::type,
			List<
				List<int>,
				List<float>,
				List<bool>
			>
		>::value == true) );
}

BOOST_AUTO_TEST_CASE(single_lists)
{
	BOOST_CHECK( (
		std::is_same<
			typename meta::cartesian_product<
				List<int>,
				List<float>,
				List<bool>
			>::type,
			List<
				List<int, float, bool>
			>
		>::value == true) );
}

template<typename> struct probe;

BOOST_AUTO_TEST_CASE(two_lists)
{
	struct type1 {};
	struct type2 {};
	struct type3 {};

	BOOST_CHECK( (
		std::is_same<
			typename meta::cartesian_product<
				List<type1, type2, type3>,
				List<type1, type2, type3>
			>::type,
			List<
				List<type1, type1>,
				List<type1, type2>,
				List<type1, type3>,
				List<type2, type1>,
				List<type2, type2>,
				List<type2, type3>,
				List<type3, type1>,
				List<type3, type2>,
				List<type3, type3>
			>
		>::value == true) );
}


/*
 * SpiralIterator.hpp
 *
 *  Created on: Jul 7, 2015
 *      Author: Martin Wermelinger
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/GridMap.hpp"

#include <Eigen/Core>
#include <memory>
#include <vector>

namespace grid_map {

/*!
 * Iterator class to iterate through a circular area of the map with a spiral.
 */
class SpiralIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param center the position of the circle center.
   * @param radius the radius of the circle.
   */
  SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  SpiralIterator& operator =(const SpiralIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const SpiralIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Eigen::Array2i& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  SpiralIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

  /*!
   * Gets the radius of current ring that is iterated through.
   * @return radius of the current ring that is used for iteration.
   */
  double getCurrentRadius() const;

private:

  /*!
   * Check if index is inside the circle.
   * @return true if inside, false otherwise.
   */
  bool isInside(const Index index) const;

  /*!
   * Uses the current distance to get the points of a ring
   * around the center.
   */
  void generateRing();

  int signum(const int val) {
      return (0 < val) - (val < 0);
  }

  //! Position of the circle center;
  Position center_;
  Index indexCenter_;

  //! Radius of the circle.
  double radius_;

  //! Square of the radius for efficiency.
  double radiusSquare_;

  //! Number of rings into the circle is divided.
  unsigned int nRings_;
  unsigned int distance_;
  std::vector<Index> pointsRing_;

  //! Map information needed to get position from iterator.
  Length mapLength_;
  Position mapPosition_;
  double resolution_;
  Size bufferSize_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace */


//==================================================================================================
/*!
  @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_FUNCTION_DIVIDES_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIVIDES_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
    @ingroup group-operator
    Function object implementing divides capabilities

    Perform the quotient of two parameters of the same type.

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    auto r = divides(a,b);
    @endcode

    or

    @code
    auto r = a/b;
    @endcode

    returns the quotient of @c a by @c b

    @Note

    As usual the simd division is often more expansive that the other
    arithmetic operators and using the @rec function can be sometimes handy
    to gain some cycles.

    @see div, rem, rec

  **/
  Value divides(Value const & x, Value const &y);
} }
#endif

#include <boost/simd/function/scalar/divides.hpp>
#include <boost/simd/function/scalar/divides.hpp>
#include <boost/simd/function/simd/divides.hpp>

#endif


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

#include <boost/contract.hpp>
#include <cassert>

//[friend_invariant
template<typename T>
class positive {
public:
    void invariant() const {
        BOOST_CONTRACT_ASSERT(value() > 0);
    }

    // Can be considered an extension of enclosing class' public interface...
    friend void swap(positive& object, T& value) {
        boost::contract::old_ptr<T> old_object_value =
                BOOST_CONTRACT_OLDOF(object.value());
        boost::contract::old_ptr<T> old_value = BOOST_CONTRACT_OLDOF(value);
        // ...so it can be made to check invariants via `public_function`.
        boost::contract::check c = boost::contract::public_function(&object)
            .precondition([&] {
                BOOST_CONTRACT_ASSERT(value > 0);
            })
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(object.value() == *old_value);
                BOOST_CONTRACT_ASSERT(value == *old_object_value);
            })
        ;

        T saved = object.value_;
        object.value_ = value;
        value = saved;
    }

private:
    T value_;

    /* ... */
//]

public:
    // Could program contracts for following too.
    explicit positive(T const& value) : value_(value) {}
    T value() const { return value_; }
};

int main() {
    positive<int> i(123);
    int x = 456;
    swap(i, x);
    assert(i.value() == 456);
    assert(x == 123);
    return 0;
}


#ifndef BODY_HPP
#define BODY_HPP

#include <armadillo>
#include "aux.hh"
#include "matrix_tool.hh"

class Body
{
public:
    Body();
    ~Body(){};

    arma::mat get_TBI();
    arma::vec get_POSITION();
    arma::vec get_VELOCITY();
    arma::vec get_ACCELERATION();
    arma::vec get_ANGLE_VEL();
    arma::vec get_ANGLE();
    arma::vec get_ANGLE_ACC();
    arma::vec get_FORCE();
    arma::vec get_TORQUE();
    arma::mat get_M();
    arma::vec get_TBI_Q();
    arma::vec get_TBID_Q();
    unsigned int get_num();

    void set_POSITION(const arma::vec PosIn);
    void set_VELOCITY(const arma::vec VelIn);
    void set_ACCELERATION(const arma::vec AccIn);
    void set_ANGLE(const arma::vec AngIn);
    void set_ANGLE_VEL(const arma::vec AngvelIn);
    void set_ANGLE_ACC(const arma::vec AngaccIn);

    virtual void update(double MassIn, arma::mat IIn, arma::vec PosIn, arma::vec VelIn, arma::vec TBI_QIn, arma::vec ANG_VEL_In, arma::vec FIn, arma::vec TIn) = 0;

protected:
    unsigned int type;  // type define   0: Ground body, 1: Mobilized body
    unsigned int num;   // No. body

    VECTOR(POSITION, 3);
    VECTOR(VELOCITY, 3);
    VECTOR(ACCELERATION, 3);
    VECTOR(ANGLE, 3);
    VECTOR(ANGLE_VEL, 3);
    VECTOR(ANGLE_ACC, 3);
    MATRIX(M, 6, 6);
    VECTOR(FORCE, 3);
    VECTOR(TORQUE, 3);
    VECTOR(APPILED_TORQUE, 3);
    MATRIX(TBI, 3, 3);
    VECTOR(TBI_Q, 4);
    VECTOR(TBID_Q, 4);
};

class Ground : public Body
{
public:
    Ground(unsigned int NumIn);
    ~Ground(){};
    virtual void update(double MassIn, arma::mat IIn, arma::vec PosIn, arma::vec VelIn, arma::vec TBI_QIn, arma::vec ANG_VEL_In, arma::vec FIn, arma::vec TIn){};
};

class Mobilized_body : public Body
{
public:
    Mobilized_body(unsigned int NumIn, arma::vec PosIn, arma::vec VelIn, arma::vec AccIn, arma::vec TBI_QIn, arma::vec ANG_VEL_In, arma::vec ANG_ACC_In, double MIn, arma::mat IIn);
    ~Mobilized_body(){};

    virtual void update(double MassIn, arma::mat IIn, arma::vec PosIn, arma::vec VelIn, arma::vec TBI_QIn, arma::vec ANG_VEL_In, arma::vec FIn, arma::vec TIn) override;
};

#endif  //BODY_HPP


#pragma once

#include <polyfem/Mesh2D.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/LocalBoundary.hpp>

#include <polyfem/InterfaceData.hpp>

#include <Eigen/Dense>
#include <vector>
#include <map>

namespace polyfem
{
	class SpectralBasis2d
	{
	public:

		static int build_bases(
			const Mesh2D &mesh,
			const int quadrature_order,
			const int order,
			std::vector< ElementBases > &bases,
			std::vector< ElementBases > &gbases,
			std::vector< LocalBoundary > &local_boundary);
	};
}


#include <boost/geometry/arithmetic/dot_product.hpp>