//============================================================================
//
// This file is part of the Shapes Generative Model Library (SGML) project.
//
// Copyright (c) 2010-2014 - Evangelos Kalogerakis, Siddhartha Chaudhuri (authors of the code) / Stanford University, UMass-Amherst
//
// SGML is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.

// SGML is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with SGML.  If not, see <http://www.gnu.org/licenses/>.
//
//============================================================================

#ifndef __S12_COMMON_HPP
#define __S12_COMMON_HPP

#include <cassert>
#include <cstdio>
#include <cstring>
#include <cfloat>
#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <limits>
#include <string>
#include <map>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>
#include <boost/limits.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/timer.hpp>

#include "CLAPACK/f2c.h"
#include "CLAPACK/clapack.h"
#include "trimesh2/TriMesh.h"

#undef max
#undef abs

#include "S12_SymbolVisibility.hpp"
#include "S12_Vector.hpp"
#include "S12_Matrix.hpp"


#define KEY make_pair<char, int>

#define SUPERMAXEPSILON  1e+30
#define SUPEREPSILON     1e-30
#define EPSILON          1e-6

#ifdef _MSC_VER

#define isinf(x) (!_finite(x))
#define isnan(x) _isnan(x)
#define getch _getch

#else

#  include <fenv.h>
#  include <termios.h>
#  include <unistd.h>

inline int getch(void)
{
  struct termios oldt, newt;
  int ch;
  tcgetattr( STDIN_FILENO, &oldt );
  newt = oldt;
  newt.c_lflag &= ~( ICANON | ECHO );
  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
  ch = getchar();
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
  return ch;
}

#endif

namespace SNL {
namespace S12 {

using namespace std;

namespace ShapeBNet_ns
{
  class MeshPart;
  class Label;

  class myXForm
  {
  public:
  myXForm()
  {
    rotation_matrix.push_back( vec(1.0, 0.0, 0.0) );
    rotation_matrix.push_back( vec(0.0, 1.0, 0.0) );
    rotation_matrix.push_back( vec(0.0, 0.0, 1.0) );
    translation = vec(0.0, 0.0, 0.0);
  }
    SNL_VECTOR( vec ) rotation_matrix;
    vec translation;
  };


  static inline std::istream &operator >> (std::istream &is, myXForm& m)
  {
    for (int i = 0; i < 3; i++)
    {
      for (int j = 0; j < 3; j++)
        is >> m.rotation_matrix[i][j];
      is >> m.translation[i];
    }

    m.rotation_matrix[0] = normalize( m.rotation_matrix[0] );
    m.rotation_matrix[1] = normalize( m.rotation_matrix[1] );
    m.rotation_matrix[2] = normalize( m.rotation_matrix[2] );

    return is;
  }


  class Label
  {
  public:
    Label(string& _label, size_t _id): id(_id), name(_label) {};
    size_t id;
    string name;
    SNL_VECTOR(Label*)    adj_labels;
    SNL_VECTOR(MeshPart*) parts;

    inline bool load_data(char* data_path, SNL_VECTOR(MeshPart*)& all_parts, SNL_VECTOR(Label*)& all_labels);
    inline bool equals(Label &other);
  };



  class MeshPart
  {
  public:
    MeshPart(Label* _label, TriMesh* _mesh, TrimeshPart *_trimesh_part, size_t _id):
    id(_id), mesh(_mesh), label(_label), trimesh_part(_trimesh_part)
    {
      trimesh_part->mesh_part_id = id;
      trimesh_part->id = id;
      trimesh_part->label_id = label->id;
      sublabel_id = 0;
    }

    inline bool load_data(char* data_path, SNL_VECTOR(MeshPart*)& all_parts, bool all_features = false);
    inline bool equals(MeshPart &other);

    // mesh part basic features
    size_t   id, old_id;
    size_t   sublabel_id;
    TriMesh  *mesh;
    Label    *label;
    TrimeshPart *trimesh_part;
    myXForm  spca_coord_system;
    pair<vec, point>  transformation_with_respect_to_reference_part;

    // for boundary extraction
    SNL_VECTOR( MeshPart* )            adjacent_parts;                // A #adjacent parts
    SNL_VECTOR( int )                  should_be_attached_to_boundary_id_of_ref_part;  // A #adjacent parts,                                                                                                                                                                                                                                                                                                         // which element of adjacent_parts of ref part, this boundary corresponds to.
    SNL_VECTOR( point )                whole_boundary_variation;      // A #adjacent parts
    SNL_VECTOR( point )                whole_boundary_center_of_mass; // A #adjacent parts
    SNL_VECTOR( SNL_VECTOR(int) )      boundary_vertices;             // A x B, B #boundary vertices (+ metadata info, such as -1,-2)
    SNL_VECTOR( SNL_VECTOR(point) )    boundary_center_of_mass;       // A x S, S is #slots
    SNL_VECTOR( SNL_VECTOR(int) )      mapped_boundary_vertices;      // A x B, B #boundary vertices (+ metadata info, such as -1,-2)
    SNL_VECTOR( SNL_VECTOR(point) )    boundary_variation;            // A x S, S is #slots
    SNL_VECTOR( SNL_VECTOR(int) )      boundary_slot_ids;             // A x S, S is #slots,
    // -1 means that the adj label of this slot is very rare, 0 failure in enumerating this slot
    int                                ground_vertex;                 // -1 if it does not exist
    int                                mapped_ground_vertex;          // -1 if it does not exist
    // parameters
    SNL_VECTOR( double )               feature_parameters;            // FIRST 3 numbers store scale
    SNL_VECTOR( double )               features;

  };


  inline bool Label::load_data(char* data_path, SNL_VECTOR(MeshPart*)& all_parts, SNL_VECTOR(Label*)& all_labels)
  {
    char label_filename[ 4096 ];
    sprintf(label_filename, "%sL_%d.txt", data_path, (int)this->id);
    ifstream label_file( label_filename );
    std::cout << "Reading data for label: " << this->name << " from file " << label_filename << "...";

    size_t checked_id;
    label_file >> checked_id;
    if (this->id != checked_id)
    {
      std::cout << "label id does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }

    string checked_name;
    label_file >> checked_name;
    if (this->name != checked_name)
    {
      std::cout << "label name does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }

    size_t dat;
    label_file >> dat;
    this->parts.resize( dat );
    for (size_t p = 0; p < this->parts.size(); p++)
    {
      label_file >> dat;
      if (dat >= all_parts.size())
      {
        std::cout << "part ids from file are larger than number of parts read from meshlist.txt" << std::endl;
        return false;
      }
      this->parts[p] = all_parts[ dat ];
      if ( all_parts[ dat ]->label->id != this->id)
      {
        std::cout << "label ids of parts from file do not agree with the ones processed from meshlist.txt" << std::endl;
        return false;
      }
    }

    label_file >> dat;
    this->adj_labels.resize( dat );
    for (size_t i = 0; i < this->adj_labels.size(); i++)
    {
      label_file >> dat;
      if (dat >= all_labels.size())
      {
        std::cout << "label ids from file are larger than number of parts read from meshlist.txt" << std::endl;
        return false;
      }
      this->adj_labels[i] = all_labels[ dat ];
    }

    std::cout << "Done." << std::endl;
    return true;
  }


  inline bool MeshPart::load_data(char* data_path, SNL_VECTOR(MeshPart*)& all_parts, bool all_features)
  {
    char part_filename[ 4096 ];
    sprintf(part_filename, "%sP_%ld.txt", data_path, (long)this->id);
    ifstream part_file( part_filename );
    std::cout << "Reading data for part: " << this->id << " from file " << part_filename << "...";

    size_t checked_id;
    part_file >> checked_id;
    if (this->id != checked_id)
    {
      std::cout << "part id does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }
    part_file >> this->sublabel_id;
    string mesh_filename;
    part_file >> mesh_filename;

    part_file >> checked_id;
    if (this->label->id != checked_id)
    {
      std::cout << "label id of part does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }
    part_file >> checked_id;
    if (this->mesh->id != checked_id)
    {
      std::cout << "mesh id of part does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }
    string checked_name;
    part_file >> checked_name;
    if (this->label->name != checked_name)
    {
      std::cout << "label of part does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }
    part_file >> checked_name;
    if (this->trimesh_part->label != checked_name)
    {
      std::cout << "label of trimesh part does not match with the one produced through reading meshlist.txt" << std::endl;
      return false;
    }

    part_file >> this->spca_coord_system;
    part_file >> transformation_with_respect_to_reference_part.first[0] >> transformation_with_respect_to_reference_part.first[1] >> transformation_with_respect_to_reference_part.first[2];
    part_file >> transformation_with_respect_to_reference_part.second[0] >> transformation_with_respect_to_reference_part.second[1] >> transformation_with_respect_to_reference_part.second[2];

    size_t dat;
    part_file >> dat;
    this->adjacent_parts.resize( dat );
    for (size_t j = 0; j < this->adjacent_parts.size(); j++)
    {
      part_file >> dat;
      this->adjacent_parts[j] = all_parts[ dat ];
    }

    part_file >> ground_vertex >> mapped_ground_vertex;

    part_file >> dat;
    this->boundary_vertices.resize( dat );
    this->mapped_boundary_vertices.resize( dat );
    for (size_t a = 0; a < boundary_vertices.size(); a++)
    {
      part_file >> dat;
      this->boundary_vertices[a].resize( dat );
      this->mapped_boundary_vertices[a].resize( dat );
      for (size_t j = 0; j < boundary_vertices[a].size(); j++)
      {
        part_file >> this->boundary_vertices[a][j];
        part_file >> this->mapped_boundary_vertices[a][j];
      }
    }

    this->whole_boundary_variation.resize( this->boundary_vertices.size() );
    this->whole_boundary_center_of_mass.resize( this->boundary_vertices.size() );
    this->should_be_attached_to_boundary_id_of_ref_part.resize( this->boundary_vertices.size() );
    for (size_t a = 0; a < boundary_vertices.size(); a++)
      part_file >> should_be_attached_to_boundary_id_of_ref_part[a] >> whole_boundary_variation[a] >> whole_boundary_center_of_mass[a];

    part_file >> dat;
    this->boundary_center_of_mass.resize( dat );
    if (this->boundary_center_of_mass.size() != this->adjacent_parts.size() ||
      this->boundary_center_of_mass.size() != this->boundary_vertices.size()  ||
      this->boundary_center_of_mass.size() != this->whole_boundary_variation.size() )
    {
      std::cout << "size of boundary_center_of_mass != size of adjacent parts / boundary_vertices /  whole_boundary_variation" << std::endl;
      return false;
    }
    for (size_t a = 0; a < boundary_center_of_mass.size(); a++)
    {
      part_file >> dat;
      this->boundary_center_of_mass[a].resize( dat );
      // for each slot
      for (size_t j = 0; j < boundary_center_of_mass[a].size(); j++)
      {
        part_file >> this->boundary_center_of_mass[a][j];
      }
    }

    part_file >> dat;
    this->boundary_variation.resize( dat );
    if (this->boundary_variation.size() != this->adjacent_parts.size() ||
      this->boundary_variation.size() != this->boundary_vertices.size()  ||
      this->boundary_variation.size() != this->whole_boundary_variation.size() )
    {
      std::cout << "size of boundary_variation != size of adjacent parts / boundary_vertices /  whole_boundary_variation" << std::endl;
      return false;
    }
    for (size_t a = 0; a < boundary_variation.size(); a++)
    {
      part_file >> dat;
      this->boundary_variation[a].resize( dat );
      // for each slot
      for (size_t j = 0; j < boundary_variation[a].size(); j++)
      {
        part_file >> this->boundary_variation[a][j];
      }
    }

    part_file >> dat;
    this->boundary_slot_ids.resize( dat );
    if (this->boundary_slot_ids.size() != this->adjacent_parts.size() ||
      this->boundary_slot_ids.size() != this->boundary_vertices.size()  ||
      this->boundary_slot_ids.size() != this->whole_boundary_variation.size() )
    {
      std::cout << "size of boundary_slot_ids != size of adjacent parts / boundary_vertices /  whole_boundary_variation" << std::endl;
      return false;
    }
    for (size_t a = 0; a < boundary_slot_ids.size(); a++)
    {
      part_file >> dat;
      this->boundary_slot_ids[a].resize( dat );
      // for each slot
      for (size_t j = 0; j < boundary_slot_ids[a].size(); j++)
      {
        part_file >> this->boundary_slot_ids[a][j];
      }
    }

    if (!all_features)
    {
			part_file >> dat;
			this->features.resize( 6 );
			for (size_t j = 0; j < 3; j++)
			{
				part_file >> this->features[j];
			}
			this->features[3] = this->spca_coord_system.translation[0];
			this->features[4] = this->spca_coord_system.translation[1];
			this->features[5] = this->spca_coord_system.translation[2];

			//part_file >> dat;
			//this->features.resize( dat );
			//for (size_t j = 0; j < this->features.size(); j++)
			//{
			//	part_file >> this->features[j];
			//}
    }
    else
    {
      part_file >> dat;
      this->features.resize( dat+3 );
      for (size_t j = 0; j < 3; j++)
      {
        part_file >> this->features[j];
      }
      this->features[3] = this->spca_coord_system.translation[0];
      this->features[4] = this->spca_coord_system.translation[1];
      this->features[5] = this->spca_coord_system.translation[2];
      for (size_t j = 3; j < dat; j++)
      {
        part_file >> this->features[j+3];
      }
    }

    std::cout << "Done." << std::endl;
    return true;
  }

  typedef TriMesh Mesh;

  inline void set_number_of_parts_per_label(Mesh& mesh, SNL_VECTOR(Label*)& labels)
  {
      mesh.number_of_parts_per_label.clear();
      mesh.number_of_parts_per_label.resize( labels.size(), 0 );
      for (size_t p = 0; p < mesh.parts.size(); p++)
      {
        mesh.number_of_parts_per_label[ mesh.parts[p]->label_id ]++;
      }
  }


  inline bool MeshPart::equals(MeshPart &other)
  {
    bool isequal = id == other.id;
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different id " << std::endl;
    }
    isequal &= sublabel_id == other.sublabel_id;
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different sublabel id " << std::endl;
    }
    isequal &= mesh->id == other.mesh->id;
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different mesh id " << std::endl;
    }
    isequal &= label->id == other.label->id;
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different label id " << std::endl;
    }
    isequal &= trimesh_part->id == other.trimesh_part->id;
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different trimesh part id " << std::endl;
    }
    isequal &= (spca_coord_system.translation[0] == other.spca_coord_system.translation[0]) & (spca_coord_system.translation[1] == other.spca_coord_system.translation[1]) & (spca_coord_system.translation[2] == other.spca_coord_system.translation[2]);
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different transformation " << std::endl;
    }
    isequal &= features.size() == other.features.size();
    if (!isequal)
    {
      std::cout << "parts " << id << "<->" << other.id << " have different features " << std::endl;
    }

    return isequal;
  }


  inline bool Label::equals(Label &other)
  {
    bool isequal = id == other.id;
    if (!isequal)
    {
      std::cout << "labels " << name << "<->" << other.name << " have different id " << std::endl;
    }
    isequal &= name == other.name;
    if (!isequal)
    {
      std::cout << "labels " << name << "<->" << other.name << " have different name " << std::endl;
    }
    for (size_t a = 0; a < adj_labels.size(); a++)
    {
      isequal &= adj_labels[a]->name == other.adj_labels[a]->name;
      if (!isequal)
      {
        std::cout << "labels " << name << "<->" << other.name << " have different adj labels " << a << std::endl;
      }
    }
    for (size_t p = 0; p < parts.size(); p++)
    {
      isequal &= parts[p]->id == other.parts[p]->id;
      if (!isequal)
      {
        std::cout << "labels " << name << "<->" << other.name << " have different parts " << p << std::endl;
      }
    }

    return isequal;
  }

}

using namespace ShapeBNet_ns;














/////////////////////////////////////////////////////////////
////////////// TEST FUNCTIONS ///////////////////////////
/////////////////////////////////////////////////////////////


/*
bool CPTTest()
{
        bool correct = true;
        RootNode a, b, c, d;  //d is test, a,b,c parents
        a.domain_size = 5;
        b.domain_size = 4;
        c.domain_size = 2;
        d.domain_size = 3;
        d.parents.push_back( &a );
        d.parents.push_back( &b );
        d.parents.push_back( &c );
        a.initialize();
        b.initialize();
        c.initialize();
        d.initialize();
        d.observation.push_back(1);
        a.getFactor();
        b.getFactor();
        c.getFactor();
        d.getFactor();
        size_t pn = 0;
        for (size_t i = 0; i < a.domain_size; i++)
        {
                for (size_t j = 0; j < b.domain_size; j++)
                {
                        for (size_t k = 0; k < c.domain_size; k++)
                        {
                                size_t l = 1;
//                                for (size_t l = 0; l < d.domain_size; l++, pn++)
//                                {
                                        SNL_VECTOR(size_t) x;
                                        x.push_back( l );
                                        x.push_back( i );
                                        x.push_back( j );
                                        x.push_back( k );
                                        d.factor.b[ d.factor.acn(x) ] = double(pn);
                                        std::cout << d.factor.acn(x) << std::endl;
                                        std::cout << d.factor.getValues(pn);
                                        std::cout << l << " " << i << " " << j << " " << k << std::endl;
                                        pn++;
//                                }
                        }
                }
        }
        correct &= ( size( unique( d.factor.b ) ) == d.factor.b.size() );
        return correct;
}




bool CPTTest()
{
  bool correct = true;
  RootNode a, b, c, d;  //d is test, a,b,c parents
  a.domain_size = 5;
  b.domain_size = 4;
  c.domain_size = 2;
  d.domain_size = 3;
  d.parents.push_back( &a );
  d.parents.push_back( &b );
  d.parents.push_back( &c );
  a.initialize();
  b.initialize();
  c.initialize();
  d.initialize();
  d.observation.push_back(1);
  a.observation.push_back(4);
  a.getFactor();
  b.getFactor();
  c.getFactor();
  d.getFactor();
  size_t pn = 0;
  size_t i = 4;
  for (size_t j = 0; j < b.domain_size; j++)
  {
    for (size_t k = 0; k < c.domain_size; k++)
    {
      size_t l = 1;
      SNL_VECTOR(size_t) x;
      x.push_back( l );
      x.push_back( i );
      x.push_back( j );
      x.push_back( k );
      d.factor.b[ d.factor.acn(x) ] = double(pn);
      std::cout << d.factor.acn(x) << std::endl;
      std::cout << d.factor.getValues(pn);
      std::cout << l << " " << i << " " << j << " " << k << std::endl;
      pn++;
    }
  }
  correct &= ( size( unique( d.factor.b ) ) == d.factor.b.size() );
  return correct;
}
*/








/*
namespace ShapeBNet_ns
{
  class MeshPart;
  class Mesh;
  class Label;


  class Label
  {
  public:
    size_t id;
    string name;
    SNL_VECTOR(Label*)    adj_labels;
    SNL_VECTOR(MeshPart*) parts;

    Label(string& _label, size_t _id, SNL_VECTOR(MeshPart*)& _parts, SNL_VECTOR(Label*)& _adj_labels): id(_id), name(_label), parts(_parts), adj_labels(_adj_labels) { };
  };


  class MeshPart
  {
  public:

    size_t  id;
    size_t  sublabel_id;
    Mesh *mesh;
    Label   *label;
    SNL_VECTOR( float )         features;
    SNL_VECTOR( MeshPart* )     adjacent_parts;                // A #adjacent parts

    MeshPart(Label* _label, Mesh* _mesh, size_t _id, size_t _sublabel_id, SNL_VECTOR(float)& _features, SNL_VECTOR(MeshPart*)& _adj_parts): id(_id), label(_label), mesh(_mesh), sublabel_id(_sublabel_id), features(_features), adjacent_parts(_adj_parts) { };

  };


  class Mesh
  {
  public:
    size_t id;
    SNL_VECTOR(MeshPart*) parts;
    string filename;
    SNL_VECTOR(int) number_of_parts_per_label;

    Mesh( string& _filename, size_t _id, SNL_VECTOR(MeshPart*)& _parts, SNL_VECTOR(Label*)& labels ): id(_id), filename(_filename), parts(_parts)
    {
      set_number_of_parts_per_label(labels);
    }
  };

  void set_number_of_parts_per_label(Mesh& mesh, SNL_VECTOR(Label*)& labels)
  {
      mesh.number_of_parts_per_label.clear();
      mesh.number_of_parts_per_label.resize( labels.size(), 0 );
      for (size_t p = 0; p < mesh.parts.size(); p++)
      {
        mesh.number_of_parts_per_label[ mesh.parts[p]->label->id ]++;
      }
  }


}
*/

} // namespace S12
} // namespace SNL

#endif