#include <vector>
#include <set>
#include <fullMatrix.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 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()];}

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

    void addNode(TVertex* node, int k);
};

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
    fullMatrix<int> _structure; // decrit la profondeur de la i-eme chaine dans la j-eme zone de trame
    std::vector<int> _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 fullMatrix<int> & structure, const std::vector<int> & multipliciteChaine);
    void genereTisse(const double nbdx, const double nbdyz);

    void cleanChainesTrames();
    void cleanBuckets();
    void printTisseGeo();
    void printTisseGeoSurf();
    void printLsNodes();
    void printLsNodesByFils();

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