// GenTextile - A composite textile generator
// Copyright (C) 2011-2026 Eric Bechet, Frederic Duboeuf
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org> or <duboeuf@outlook.com>.


#ifndef _COMPOSITE_TEXTILE_H_
#define _COMPOSITE_TEXTILE_H_

#include <vector>
#include <set>
#include <genMatrix.h>
#include <fullMatrix.h>
#include "linearSystemPETSc.h"
#include <cmath>
#include <assert.h>
#include <iostream>
#include <fstream>
#include <sstream>

class TVertex;
class TTransformation;
class Bucket;

class Empilement;
class StructureEmpilements;

class Trajectoire;
class Section;
class Fil;
class Tisse;



//////////////////////////////////////////////
// Composite Utils
//////////////////////////////////////////////
class TVertex
{
private :
  double _p[3]; // Pour le produit avec une matrice 4*4, rajouter une 4eme composante egale a 1
  int id;
  int index; // index dans une section
  Section* _section;

public :
  // Constructeurs et operateur de copie :
  TVertex()
  {
    _p[0] = _p[1] = _p[2] = 0.0;
    id = 0;
    index = 0;
    _section = NULL;
  }
  TVertex(const int i, const double x, const double y, const double z)
  {
    _p[0] = x;
    _p[1] = y;
    _p[2] = z;
    id = i;
    index = -1;
    _section = NULL;
  }
  TVertex(const double x, const double y, const double z)
  {
    _p[0] = x;
    _p[1] = y;
    _p[2] = z;
    id = -1;
    index = 0;
    _section = NULL;
  }
  TVertex(const TVertex &other)
  {
    _p[0] = other._p[0];
    _p[1] = other._p[1];
    _p[2] = other._p[2];
    id = other.id;
    index = other.index;
    _section = other._section;
  }
  TVertex & operator=(const TVertex &other)
  {
    if(this != &other)
    {
        _p[0] = other._p[0];
        _p[1] = other._p[1];
        _p[2] = other._p[2];
        _section = other._section;
    }
    return *this;
  }
  inline void set(const int i, const double r)
  {
    _p[i]=r;
  }
  inline void setPosition(const double x, const double y, const double z)
  {
    _p[0]=x;
    _p[1]=y;
    _p[2]=z;
  }
  inline void setVector(const TVertex* v0, const TVertex* v1)
  {
    _p[0]=v1->x()-v0->x();
    _p[1]=v1->y()-v0->y();
    _p[2]=v1->z()-v0->z();
  }
  inline void setSection(Section* S)
  {
    _section=S;
  }
  inline Section* getSection() const
  {
    return _section;
  }
  inline double x() const
  {
    return _p[0];
  }
  inline double y() const
  {
    return _p[1];
  }
  inline double z() const
  {
    return _p[2];
  }
  inline void setIndex(int i)
  {
    index = i;
  }
  inline int getIndex()
  {
    return index;
  }
  inline int getId()
  {
    return id;
  }
  inline void crossproduct(TVertex* v1, TVertex* r)
  {
    r->setPosition(_p[1]*v1->z()-_p[2]*v1->y(), _p[2]*v1->x()-_p[0]*v1->z(), _p[0]*v1->y()-_p[1]*v1->x());
  }
  inline double dotproduct(TVertex* v1)
  {
    return _p[0]*v1->x() + _p[1]*v1->y() + _p[2]*v1->z();
  }
  inline double norm() const
  {
    return sqrt(_p[0]*_p[0] + _p[1]*_p[1] + _p[2]*_p[2]);
  }
  inline double sq_norm() const
  {
    return _p[0]*_p[0] + _p[1]*_p[1] + _p[2]*_p[2];
  }
  inline void normalize()
  {
    double l = norm();
    assert(l!=0);
    _p[0] /= l;
    _p[1] /= l;
    _p[2] /= l;
  }
  inline void operator += (const TVertex &other)
  {
    _p[0] += other._p[0];
    _p[1] += other._p[1];
    _p[2] += other._p[2];
  }

private :
  friend TVertex operator+(const TVertex&, const TVertex&);
  friend TVertex operator-(const TVertex&, const TVertex&);
  friend TVertex operator*(const TVertex&, const TVertex&);
  friend TVertex operator*(const double&, const TVertex&);

public :
  void printTVertex() { std::cout <<"Point : ["<<x()<<","<<y()<<","<<z()<<"]"<< std::endl;}
};

class TTransformation
{
private :
  double _t[4][4];

public :
  TTransformation()
  {
    _t[0][0]=_t[0][1]=_t[0][2]=_t[0][3]=
    _t[1][0]=_t[1][1]=_t[1][2]=_t[1][3]=
    _t[2][0]=_t[2][1]=_t[2][2]=_t[2][3]=
    _t[3][0]=_t[3][1]=_t[3][2]=_t[3][3]=0;
  }
  TTransformation(double x, double y, double z)
  {
    _t[0][0]=x;
    _t[1][1]=y;
    _t[2][2]=z;
    _t[3][3]=1;
    _t[0][1]=_t[0][2]=_t[0][3]=
    _t[1][0]=_t[1][2]=_t[1][3]=
    _t[2][0]=_t[2][1]=_t[2][3]=
    _t[3][0]=_t[3][1]=_t[3][2]=0;
  }
  TTransformation(TVertex &e1, TVertex &e2, TVertex &e3)
  {
    _t[0][0]=e1.x();
    _t[1][0]=e1.y();
    _t[2][0]=e1.z();

    _t[0][1]=e2.x();
    _t[1][1]=e2.y();
    _t[2][1]=e2.z();

    _t[0][2]=e3.x();
    _t[1][2]=e3.y();
    _t[2][2]=e3.z();

    _t[3][0]=_t[3][1]=_t[3][2]=_t[0][3]=_t[1][3]=_t[2][3]=0;
    _t[3][3]=1;
  }
  TTransformation(const TTransformation &other)
  {
    for (int i=0; i<4; i++)
      for (int j=0; j<4; j++)
        _t[i][j] = other._t[i][j];
  }
  TTransformation & operator=(const TTransformation &other)
  {
    if(this != &other)
      for (int i=0; i<4; i++)
        for (int j=0; j<4; i++)
          _t[i][j] = other._t[i][j];
    
    return *this;
  }
  void set(int i, int j, double r) { _t[i][j]=r; }
  double getT(int i, int j) const {return _t[i][j];}

private :
  friend TVertex operator* (const TTransformation &M, const TVertex &v);

public :
  void printTTransformation()
  {
    std::cout<<"Transformation : ["<<_t[0][0]<<","<<_t[0][1]<<","<<_t[0][2]<<","<<_t[0][3]<<"]"<<std::endl;
    std::cout<<"               : ["<<_t[1][0]<<","<<_t[1][1]<<","<<_t[1][2]<<","<<_t[1][3]<<"]"<<std::endl;
    std::cout<<"               : ["<<_t[2][0]<<","<<_t[2][1]<<","<<_t[2][2]<<","<<_t[2][3]<<"]"<<std::endl;
    std::cout<<"               : ["<<_t[3][0]<<","<<_t[3][1]<<","<<_t[3][2]<<","<<_t[3][3]<<"]"<<std::endl;
  }
};

class Bucket
{
private:
  TVertex _min;
  TVertex _max;
  int _nx;
  int _ny;
  int _nz;
  int _maxn;
  double _ref_size;
  std::vector<std::vector<TVertex*> > _bucket;
  int _bsize;
  mutable std::vector<int> _flag;
  mutable int _request_id;

public:
  Bucket(TVertex min, TVertex max, double enlarge, int max_size);
//     inline size_t memory()
//     {
//         size_t total = 0;
//         for (int i = 0; i < _bucket.size(); i++)
//             total += _bucket[i].size()*sizeof(TVertex*);
//         return sizeof(*this) + _flag.size()*sizeof(int) + total;
//     }
  int get_index(int x, int y, int z) const;
  void get_coord(TVertex* const pt, int &x, int &y, int &z) const;
  TVertex* get_closest_in_bucket(int index, TVertex* const pt, double* sq_dist) const;
  TVertex* get_closest(TVertex* const pt) const;
  int contain(TVertex* const pt) const;
  void add_point(TVertex *pt);
  bool isUsed() const;
  void PrintBucket(char *name) ;
};


//////////////////////////////////////////////
// Composite Structure
//////////////////////////////////////////////

class Empilement
{
private :
  std::vector<Fil*> _empilement;
  int _i, _j; // emplacement dans la structure

  double _epaisseurTextile;
  double _epaisseurComposite; // Permet de dilater l'empilement dans le cas ou de la matrice se positionne entre les fils

  std::vector<double> _positionsTextile;
  std::vector<double> _positionsComposite;

  int _calculated; // 0 initialement, 1 si les positions du textil on deja ete calculees, 2 si celles du composite ont ete calculees

public :
  Empilement() : _empilement(), _i(-1), _j(-1),
                 _epaisseurTextile(0.), _epaisseurComposite(0.),
                 _positionsTextile(), _positionsComposite(),
                 _calculated(0) {}
  Empilement(int i, int j, double epaisseur) : _empilement(), _i(i), _j(j),
                                               _epaisseurTextile(0.), _epaisseurComposite(epaisseur),
                                               _positionsTextile(), _positionsComposite(),
                                               _calculated(0) {}
  Empilement(const Empilement &empilement) : _empilement(empilement._empilement), _i(empilement._i), _j(empilement._j),
                                             _epaisseurTextile(empilement._epaisseurTextile), _epaisseurComposite(empilement._epaisseurComposite),
                                             _positionsTextile(empilement._positionsTextile), _positionsComposite(empilement._positionsComposite),
                                             _calculated(empilement._calculated){}
  Empilement & operator=(const Empilement &empilement);
  void addFil(Fil* fil);
  double getPosition(Fil* fil);

  double getEpaisseurComposite() {return _epaisseurComposite;}
  double getCalculated() {return _calculated;}

private :
  void calculPositions();
  void calculPositionsTextile();
  void calculPositionsComposite();
  int contain(Fil* fil);

public :
  void printEmpilement();
};

class StructureEmpilements
{
private :
  std::vector<Empilement*> _structureEmpilements;
  int _nbZonesChaine, _nbZonesTrame;

public :
  StructureEmpilements () : _structureEmpilements(), _nbZonesChaine(0), _nbZonesTrame(0) {}
  StructureEmpilements (int nbZonesChaine, int nbZonesTrame, double epaisseur);
  ~StructureEmpilements() { cleanEmpilements(); }

  Empilement* getEmpilement(int i, int j);
  void addFilInEmpilement (int i, int j, Fil * fil);

  void cleanEmpilements();

  void printStructureEmpilement()
  {
    std::cout<<"StructureEmpilement : "<<std::endl;
    for (int i=0; i<_nbZonesChaine;i++)
      for (int j=0; j<_nbZonesTrame;j++)
        getEmpilement(i,j)->printEmpilement();
  }
};



//////////////////////////////////////////////
// Composite Textile
//////////////////////////////////////////////

class Trajectoire
{
private :
  double _h; // hauteur
  double _p; // longueur
  std::vector<TVertex> _xyz;
  std::vector<TVertex> _dxyz;
  Fil* _fil;

public :
  Trajectoire () : _h(0.), _p(0.), _xyz(), _dxyz(), _fil() {}
  Trajectoire (int nbdx);
  Trajectoire (double h, double p, TTransformation* M, Trajectoire* traj);
  Trajectoire (const Trajectoire &trajectoire) : _h(trajectoire._h), _p(trajectoire._p), _xyz(trajectoire._xyz), _dxyz(trajectoire._dxyz), _fil(trajectoire._fil) {}
  Trajectoire & operator=(const Trajectoire &trajectoire);
  ~Trajectoire() { _fil=NULL; }
  void linktoFil(Fil* fil) {_fil=fil;}
  //std::vector<Fil *>  &getLinktoFil() {return &_fil;}
  int getNbdx() const {return _xyz.size();}
  TVertex* getXYZ(int i) {assert(i<getNbdx()); return &(_xyz[i]);}
  TVertex* getDxyz(int i) {assert(i<getNbdx()); return &(_dxyz[i]);}
  double getH() const {return _h;}
  double getP() const {return _p;}

  void printTrajectoire()
  {
    std::cout<<"Trajectoire : z, dz, (h,p)=("<<_h<<", "<<_p<<")"<<std::endl;
    std::ostringstream temp;
    for (int i=0; i<getNbdx(); i++)
    {
      temp<<"            : "<<_xyz[i].x()<<", "<< _dxyz[i].x()<<"\n";
      temp<<"            : "<<_xyz[i].y()<<", "<< _dxyz[i].y()<<"\n";
      temp<<"            : "<<_xyz[i].z()<<", "<< _dxyz[i].z()<<"\n";
    }
    //temp<<"            : link with fil type "<< _fil->getType();
    //temp<<" ( " << _fil->getZone()<<", "<< _fil->getCouche()<<")"<<"\n";
    std::cout<<temp.str()<<std::endl;
  }
};

class Section
{
private :
  Fil* _fil;
  int _index; // index de section dans le fil

  TVertex* _centre;
  std::vector<TVertex*> _noeuds;

public :
  Section () : _noeuds() {_fil = NULL; _index = -1; _centre = NULL;}
  Section (Fil* fil, TVertex* centre, TVertex* tangente, std::vector< double >* axes, const double nbdyz);
  Section (const Section &section) : _fil(section._fil), _index(section._index), _centre(section._centre), _noeuds(section._noeuds) {}  // a changer
  ~Section() {}

  Fil* getFil() {return _fil;}
  Section* getSectionBefor();
  Section* getSectionAfter();
  TVertex* getCentre() {return _centre;}
  int getSizeSection() {return _noeuds.size();}
  TVertex* getNode(int i) {if(i==-1) return _noeuds.back(); return _noeuds[i%getSizeSection()];}

  int getIndex() {return _index;}
  void setIndex(int index) {_index=index;}

  void addNode(TVertex* node);
};

class Fil
{
private :
  Tisse * _tisse;
  std::vector<double> * _axes;
  std::vector<Empilement *> _empilements;
  std::vector<Trajectoire *> _trajectoires;
  std::vector<Section *> _sections;

  int _type; // 0 = chaine, 1 = trame
  int _id; // numero de fil (distinction chaine et trame)
  int _zone;
  int _couche;

  int _numBucket;

  int _sizeFil;

public :
  Fil () :
    _axes(), _empilements(), _trajectoires(), _sections(),
    _type(-1), _id(-1), _zone(-1), _couche(-1), _numBucket(-1), _sizeFil(0)
    {_tisse = NULL;}
  Fil (Tisse * tisse, std::vector<double> * axes, int type, int id, int zone, int couche) :
    _tisse(tisse), _axes(axes), _empilements(), _trajectoires(), _sections(),
    _type(type), _id(id), _zone(zone), _couche(couche), _numBucket(2*type+(couche%2)), _sizeFil(0) {}
  Fil (const Fil &fil) :
    _tisse(fil._tisse), _axes(fil._axes), _empilements(fil._empilements), _trajectoires(fil._trajectoires), _sections(fil._sections),
    _type(fil._type), _id(fil._id), _zone(fil._zone), _couche(fil._couche), _numBucket(fil._numBucket), _sizeFil(0) {}
  Fil & operator=(const Fil &fil);
  ~Fil() { _tisse=NULL; _empilements.clear(); cleanTrajectoires(); cleanSections(); }

  inline Tisse* getTisse() {return _tisse;}
  inline double getAxe(int i) {return _axes->at(i);}
  inline int getNbEmpilements() {return _empilements.size();}
  inline Empilement* getEmpilement(int i) {return _empilements[i];}
  inline double getPositionInEmpilement(int i) {return _empilements[i]->getPosition(this);}
  inline int getNbSection() {return _sections.size();}
  inline Section* getSection(int i) {return _sections[i];}
  inline int getType() {return _type;}
  inline int getId() {return _id;}
  inline int getZone() {return _zone;}
  inline int getCouche() {return _couche;}
  inline int getNumBucket() {return _numBucket;}
  inline int getSizeFil() {return _sizeFil;}

  inline void addEmpilement(Empilement* const empilement) {_empilements.push_back(empilement);}
  inline void addTrajectoire(Trajectoire* const trajectoire) {_trajectoires.push_back(trajectoire);}
  inline void addSection(Section* section) {section->setIndex(_sections.size()); _sections.push_back(section); _sizeFil += section->getSizeSection();}

  void genereFil (const int nbdyz);

  void cleanTrajectoires();
  void cleanSections();

  void printFil()
  {
    std::cout<<"Fil : type, id, (zone, couche)"<<std::endl;
    std::cout<<"    : "<<_type<<", "<<_id<<", ("<<_zone<<", "<<_couche<<")"<<std::endl;
  }
};

class Tisse
{
private :
  double _epaisseur; // epaisseur caracteristique du VER
  double _dimChaine, _dimTrame; // longueurs caracteristiques du VER dans les sens chaine et trame
  std::vector<double> _axesChaine, _axesTrame; // longueurs du grand axe et du petit axe dans les sections sens chaine et trame

  int _nbdx, _nbdyz; // nombre de point le long de la trajectoire et de la section

  int _layer; // nombre de couche de trame
  genMatrix<double> _structure; // decrit la profondeur de la i-eme chaine dans la j-eme zone de trame
  std::vector<double> _multipliciteChaine; // decrit le nombre de couches dans chaque zone de chaine

  StructureEmpilements * _empilementsStructure;
  std::vector< std::vector<Fil *> > _zonesChaine;
  std::vector< std::vector<Fil *> > _zonesTrame;
  Trajectoire* _modelTrajectoire;

  int _nbFilsChaine;
  int _nbFilsTrame;

  std::vector< Bucket* > _buckets; // 0,1 chaines, 2,3 trames, pair ou impair suivant la profondeur elle meme paire ou impaire

public :
  Tisse () :
    _epaisseur(0.), _dimChaine(0.), _dimTrame(0.), _axesChaine(), _axesTrame(),
    _nbdx(0), _nbdyz(0),
    _layer(0), _structure(), _multipliciteChaine(),
    _zonesChaine(), _zonesTrame(),
    _nbFilsChaine(0), _nbFilsTrame(0),
    _buckets(0)
    {_empilementsStructure=NULL; _modelTrajectoire=NULL;}

  Tisse (const double epaisseur, const double dimChaine, const double dimTrame, const std::vector<double> &axesChaine, const std::vector<double> &axesTrame) :
    _epaisseur(epaisseur), _dimChaine(dimChaine), _dimTrame(dimTrame), _axesChaine(axesChaine), _axesTrame(axesTrame),
    _nbdx(11), _nbdyz(8),
    _layer(), _structure(), _multipliciteChaine(),
    _zonesChaine(), _zonesTrame(),
    _nbFilsChaine(0), _nbFilsTrame(0),
    _buckets(0)
    {_empilementsStructure=NULL; _modelTrajectoire=NULL;}

  Tisse (const Tisse &tisse) :
    _epaisseur(tisse._epaisseur), _dimChaine(tisse._dimChaine), _dimTrame(tisse._dimTrame), _axesChaine(tisse._axesChaine), _axesTrame(tisse._axesTrame),
    _nbdx(tisse._nbdx), _nbdyz(tisse._nbdyz),
    _layer(tisse._layer), _structure(tisse._structure), _multipliciteChaine(tisse._multipliciteChaine),
    _empilementsStructure(tisse._empilementsStructure), _zonesChaine(tisse._zonesChaine), _zonesTrame(tisse._zonesTrame), _modelTrajectoire(tisse._modelTrajectoire),
    _nbFilsChaine(tisse._nbFilsChaine), _nbFilsTrame(tisse._nbFilsTrame),
    _buckets(tisse._buckets) {}

  ~Tisse() { cleanChainesTrames(); delete _empilementsStructure; }

  int getNbFils() const {return _nbFilsChaine + _nbFilsTrame;}
  int getNbBuckets() const;

  void allocateZonesChaine(int nbZonesChaines);
  void allocateZonesTrame(int nbZonesTrames);
  void addFilsChaine(int zone, int nbcouche);
  void addFilsTrame(int zone, int nbcouche);

  void analyseStructure(const int layer, const genMatrix<double> &structure, const std::vector<double> &multipliciteChaine);
  void genereTisse(const double nbdx, const double nbdyz);

  void cleanChainesTrames();
  void cleanBuckets();
  void printTisseGeo(const std::string &fileName);
  void printTisseGeoSurf(const std::string &fileName);
  void printLsNodes(const std::string &fileName);
  void printLsNodesByFils(const std::string &fileName);

  Bucket* getBucket(int i) const {return _buckets[i];}
};




//////////////////////////////////////////////
// Calcul de la courbure d'une surface
//////////////////////////////////////////////

TTransformation inline buildLocalTransformation(const TVertex *pt, const TVertex &n)
{
  TVertex X,Y,Z(n); // local axis
  TVertex x(1.,0.,0.),y(0.,1.,0.), r;
  Z.crossproduct(&x, &r);
  if (r.norm()!=0)
    X=r;
  else
  {
    Z.crossproduct(&y, &r);
    X=r;
  }
  Z.crossproduct(&X, &Y);

  X.normalize();
  Y.normalize();
  Z.normalize();
  TTransformation T(X,Y,Z);
  TVertex Lpt = T*(*pt);
  T.set(0,3,-Lpt.x());
  T.set(1,3,-Lpt.y());
  T.set(2,3,-Lpt.z());

  return T;
}

std::vector<TVertex> inline buildLocalPoints(const std::vector<TVertex*> &pts, const TTransformation &T)
{
  std::vector<TVertex> Lpts;
  for (int i=0; i<pts.size(); ++i)
    Lpts.push_back(T*(*pts[i]));

//   for (int i=0; i<Lpts.size(); ++i) Lpts[i].printTVertex();
  return Lpts;
}

void inline buildProblem(const std::vector<TVertex*> &pts, const TVertex &n, genMatrix<double> &M, genVector<double> &v)
{
  TTransformation T = buildLocalTransformation(pts[0], n);
//   T.printTTransformation();

  std::vector<TVertex> Lpts = buildLocalPoints(pts,T);
  M.resize(Lpts.size(),3);
  v.resize(Lpts.size());

  for (int i=0; i<Lpts.size(); ++i)
  {
    M(i,0) = Lpts[i].x() * Lpts[i].x();
    M(i,1) = Lpts[i].x() * Lpts[i].y();
    M(i,2) = Lpts[i].y() * Lpts[i].y();
    v(i) = Lpts[i].z();
  }
}

genVector<double> inline buildSolution(genMatrix<double> &M, genVector<double> &v) // Résoud les equations matricielles du type : Ax = b et retourne le vecteur x solution.
{
  fullMatrix<double> fullM(M.getDataPtr(),M.size1(),M.size2());
  fullVector<double> fullv(v.getDataPtr(),v.size());

  fullMatrix<double> fullA(M.size2(),M.size2());
  fullVector<double> fullb(M.size2());

  fullA.gemm(fullM,fullM,1.,0., true, false);// A=Mt.M
  fullM.multWithATranspose(fullv,1,1,fullb); // b=Mt.v

  linearSystemPETSc<double> system;

  system.allocate(fullA.size1());

  for (int i=0; i<fullA.size1(); ++i)
    for (int j=0; j<fullA.size2(); ++j)
      system.addToMatrix(i,j,fullA(i,j));

  for (int j=0; j<fullb.size(); ++j)
    system.addToRightHandSide(j,fullb(j));

  system.systemSolve();
  genVector<double> r(fullA.size1());
  fullVector<double> fullr(r.getDataPtr(),r.size());
  for (int i=0; i<r.size(); ++i)
    system.getFromSolution(i, fullr(i));

  return r;
}

void inline buildSupport(TVertex* pt, Section* section, std::vector<TVertex*> &pts)
{
  int index = pt->getIndex();
  Section* sectionBefor = section->getSectionBefor();
  Section* sectionAfter = section->getSectionAfter();

  pts.push_back(pt);
  pts.push_back(section->getNode(index-1));
  pts.push_back(section->getNode(index+1));

  if (sectionBefor==NULL && sectionAfter!=NULL)
  { // courbure symétrique
    pts.push_back(new TVertex(2.**section->getNode(index-1)-*sectionAfter->getNode(index-1)));
    pts.push_back(new TVertex(2.**pt-*sectionAfter->getNode(index)));
    pts.push_back(new TVertex(2.**section->getNode(index+1)-*sectionAfter->getNode(index+1)));
    pts.push_back(sectionAfter->getNode(index-1));
    pts.push_back(sectionAfter->getNode(index));
    pts.push_back(sectionAfter->getNode(index+1));
  }
  else if (sectionBefor!=NULL && sectionAfter==NULL)
  { // courbure symétrique
    pts.push_back(sectionBefor->getNode(index-1));
    pts.push_back(sectionBefor->getNode(index));
    pts.push_back(sectionBefor->getNode(index+1));
    pts.push_back(new TVertex(2.**section->getNode(index-1)-*sectionBefor->getNode(index-1)));
    pts.push_back(new TVertex(2.**pt-*sectionBefor->getNode(index)));
    pts.push_back(new TVertex(2.**section->getNode(index+1)-*sectionBefor->getNode(index+1)));
  }
  else if (sectionBefor!=NULL && sectionAfter!=NULL)
  {
    pts.push_back(sectionBefor->getNode(index-1));
    pts.push_back(sectionBefor->getNode(index));
    pts.push_back(sectionBefor->getNode(index+1));
    pts.push_back(sectionAfter->getNode(index-1));
    pts.push_back(sectionAfter->getNode(index));
    pts.push_back(sectionAfter->getNode(index+1));
  }
  else
    printf("Section error\n");
}

double inline CurvatureGauss (const std::vector<TVertex*> &pts, const TVertex &n)
{
  genMatrix<double> M,A;
  genVector<double> v,b;

  buildProblem(pts,n,M,v);
//   M.print("M"); v.print("v");

  genVector<double> r = buildSolution(M,v);
//   printf("a : %lf, b : %lf, c : %lf, \n", r(0), r(1), r(2));
  
  return 4*r(0)*r(2)-r(1)*r(1); //4*ac-b² Gauss Curvature
}

double inline CurvatureMean (const std::vector<TVertex*> &pts, const TVertex &n)
{
  genMatrix<double> M,A;
  genVector<double> v,b;

  buildProblem(pts,n,M,v);
//   M.print("M"); v.print("v");

  genVector<double> r = buildSolution(M,v);
//   printf("a : %lf, b : %lf, c : %lf, \n", r(0), r(1), r(2));
  
  return r(0)+r(2); //a+c     Mean Curvature
}

double inline CurvatureMethode1 (TVertex* pt, Section* section, TVertex &n)
{
  // Ajout de sa courbure
  double curvature;
  // Direction fils
  double curvatureFils;
  int index = pt->getIndex();
  Section* sectionBefor = section->getSectionBefor();
  Section* sectionAfter = section->getSectionAfter();
  if (sectionBefor==NULL)
  {
    TVertex * vAfter=  sectionAfter->getNode(index);
    TVertex * centreAfter = sectionAfter->getCentre();
    TVertex nAfter;
    nAfter.setVector(centreAfter, vAfter);
    nAfter.normalize();

    TVertex s, TBefor, TAfter, v, z(0.,0.,1.);
    s.setVector(pt, vAfter);
    double norm = s.norm();
    s.normalize();
    z.crossproduct(&s,&v);
    v.crossproduct(&n,&TBefor);
    v.crossproduct(&nAfter,&TAfter);
    curvatureFils = fabs(n.dotproduct(&TAfter)-n.dotproduct(&TBefor))/norm;
  }
  else if (sectionAfter==NULL)
  {
    TVertex * vBefor=  sectionBefor->getNode(index);
    TVertex * centreBefor = sectionBefor->getCentre();
    TVertex nBefor;
    nBefor.setVector(centreBefor, vBefor);
    nBefor.normalize();

    TVertex s, TBefor, TAfter, v, z(0.,0.,1.);
    s.setVector(vBefor, pt);
    double norm = s.norm();
    s.normalize();
    z.crossproduct(&s,&v);
    v.crossproduct(&nBefor,&TBefor);
    v.crossproduct(&n,&TAfter);
    curvatureFils = fabs(n.dotproduct(&TAfter)-n.dotproduct(&TBefor))/norm;
  }
  else
  {
    TVertex * vBefor=  sectionBefor->getNode(index);
    TVertex * centreBefor = sectionBefor->getCentre();
    TVertex nBefor;
    nBefor.setVector(centreBefor, vBefor);
    nBefor.normalize();

    TVertex * vAfter=  sectionAfter->getNode(index);
    TVertex * centreAfter = sectionAfter->getCentre();
    TVertex nAfter;
    nAfter.setVector(centreAfter, vAfter);
    nAfter.normalize();

    TVertex s, TBefor, TAfter, v, z(0.,0.,1.);
    s.setVector(vBefor, vAfter);
    double norm = s.norm();
    s.normalize();
    z.crossproduct(&s,&v);
    v.crossproduct(&nBefor,&TBefor);
    v.crossproduct(&nAfter,&TAfter);
    curvatureFils = fabs(n.dotproduct(&TAfter)-n.dotproduct(&TBefor))/norm;
  }
  // Direction section
  double curvatureSection;
  TVertex * centre = section->getCentre();
  TVertex * vBefor=  section->getNode(index-1);
  TVertex nBefor;
  nBefor.setVector(centre, vBefor);
  nBefor.normalize();

  TVertex * vAfter=  section->getNode(index+1);
  TVertex nAfter;
  nAfter.setVector(centre, vAfter);
  nAfter.normalize();

  TVertex s, TBefor, TAfter, v, z(0.,0.,1.);
  s.setVector(vBefor, vAfter);
  double norm = s.norm();
  s.normalize();
  z.crossproduct(&s,&v);
  v.crossproduct(&nBefor,&TBefor);
  v.crossproduct(&nAfter,&TAfter);
  curvatureSection = fabs(n.dotproduct(&TAfter)-n.dotproduct(&TBefor))/norm;

  curvature = (curvatureFils > curvatureSection) ? curvatureFils : curvatureSection;
  return curvature;
}

double inline CurvatureMethode2 (TVertex* pt, Section* section, TVertex &n)
{ // Gauss curvature estimates using Simple Quadric Fitting
  std::vector<TVertex*> pts;

  buildSupport(pt, section, pts);

  return CurvatureGauss(pts, n);
}

double inline CurvatureMethode3 (TVertex* pt, Section* section, TVertex &n)
{ // Mean curvature estimates using Simple Quadric Fitting
  std::vector<TVertex*> pts;

  buildSupport(pt, section, pts);

  double curvature = CurvatureMean(pts, n);
//   if (curvature>1e4)
//     printf("FileType : %d, FileId : %d, SectionIndex : %d, PointIndex : %d, curvature : %lf \n", section->getFil()->getType(), section->getFil()->getId(), section->getIndex(), pt->getIndex(), curvature);

  return curvature;
}


#endif //_COMPOSITE_TEXTILE_H_