// Gnurbs - A curve and surface library
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

// Contribution Detroux Thibaut
//              Nyssen Florence

#include <iostream>
#include <vector>
#include <fstream>

#include "ndisplay.h"
#include "nbspline.h"
#include "nbsplinesurface.h"

#include "linear_algebra.h"

// fonction d'initialisation des courbes trajectoire et profil
void init_trajectoire(nbspline &traj);
void init_profil(nbspline &prof);

// fonction qui calcule la surface extrudee
void extrude(nbspline& traj,nbspline& prof,int Nb_instances,npoint3 &ori,
             nbsplinesurface &surf, data_container &data_)
{

  //Calcul de la séquence nodale selon v
  int degree_v=traj.degree();

  std::vector<double> vec (traj.nb_knots()); //Initialisation
  for (int i=0;i<vec.size();i++)
  {
    vec[i]=traj.u(i);
    std::cout<<vec[i]<< " " ; //Affichage
  }
  std::cout << std::endl;

  //si s=K+q-1 --> ok

  //si s<=K+1
  double max;
  std::vector<double>::iterator pos,pos2;
  while (vec.size()<=Nb_instances+degree_v)
  {
      max=-1.0;
      pos=vec.begin();
      pos2=pos;
      for(int i=0;i<vec.size()-1;++i)
      {
          //ajouter des noeuds dans la séquence nodale
          if(max<(vec[i+1]-vec[i]))//détermination de la valeur à ajouter
          {
              max=(vec[i+1]-vec[i]);
              pos2=pos;
          }
          pos++;
      }
      double val=*pos2;
      vec.insert(pos2+1,val+max/2.);
  }
  std::cout << std::endl ;
  //si s>K+q+1
  if (Nb_instances< vec.size()-degree_v-1)
    Nb_instances=vec.size()-degree_v-1;


  //Séquence nodale selon u
  int degree_u=prof.degree(); //Initialisation du degré
  int nCP_u=prof.nb_CP(); //Initialisation du nombre de points de contrôle




  //Initialisation de la surface
  surf.init(nCP_u,degree_u,Nb_instances,degree_v);
  std::cout<< "Sequences nodales" << std::endl;
  for (int i=0;i<prof.nb_knots();++i) //Initialisation de la séquence nodale selon u
  {
    surf.u(i) = prof.u(i);
    std::cout<<surf.u(i)<< " " ; //Affichage
  }
  std::cout << std::endl;
  for (int i=0;i<vec.size();i++) //Initialisation de la séquence nodale selon v
  {
    surf.v(i) = vec[i];
    std::cout<<surf.v(i)<< " " ; //Affichage
  }
  std::cout << std::endl;  //Calcul des valeurs de v auxquelles on interpole
  //moyenne glissante
  int q=degree_v;
  std::vector<double> vbar (Nb_instances); //Initialisation
//  vbar[0]=surf.v(0);
  for(int k=0;k<Nb_instances;++k)
  {
      vbar[k]=0;
      for (int i=1;i<=q;++i)
      {
          vbar[k]=vbar[k]+1./q*surf.v(k+i);
      }
  }
//  vbar[Nb_instances-1]=surf.v(surf.nb_knots_v()-1);

//  Affichage
  std::cout<< "v bar" << std::endl;
  for (int i=0;i<Nb_instances;++i)
  {
    std::cout<<vbar[i]<< " " ; //Affichage
  }
  std::cout << std::endl;




  //Initialisations pour la création de la matrice A
  nbspline bsplinefonc(surf.nb_knots_v()-1-degree_v,degree_v);
  //std::cout<< "bsplinefonc.u" << std::endl;
  for (int i=0;i<surf.nb_knots_v();++i)
  {
      bsplinefonc.u(i) = surf.v(i);
      //std::cout<< bsplinefonc.u(i) << " ";
  }
  //std::cout<< std::endl;

  Square_Matrix A(Nb_instances,0.0); //Initialisation de la matrice A
  for(int k=0;k<Nb_instances;k++)
  {
      std::vector<double> N(degree_v+1); //Initilisation du vecteur N
      int span=bsplinefonc.findspan(vbar[k]); //Détermination de l'intervalle dans lequel l'élément k de vbar se situe
      bsplinefonc.basisfuns(span,vbar[k],N); //Détermination de la fonctions de forme en vbar[k] situé dans l'intervalle span, résultat dans N

      for (int i=0;i<degree_v+1;++i) //remplissage de la matrice A (degree_v+1 éléments non nuls)
      {
          A(k,span-degree_v+i)=N[i];
      }

      //A.Display(cout);

      //std::cout<< "bsplinefonc.nb_CP" << std::endl;
      //std::cout<< bsplinefonc.nb_CP() << std::endl;
      //std::cout<< std::endl;

      //std::cout<< "surf.nb_knots_u()" << std::endl;
      //std::cout<< surf.nb_knots_u() << std::endl;
      //std::cout<< std::endl;
  }


  //Décomposition LU de la matrice A
  LU_Matrix LU(A);



  //Détermination des points de contrôle de chaque instance
  //Initialisations
  npoint Tprime;
  double norm_Tprime;
  npoint Tsec;
  double norm_Tsec;
  
  npoint3 T;
  npoint3 TT;
  double produit_scalaire;
  npoint3 petitb;
  double norm_petitb;
  npoint3 B;
  double norm_B;

  npoint vect_o;
  npoint3 vect_x;
  npoint3 vect_y;
  npoint3 vect_z;
  double M[4][4];

  Vector Blin_x(Nb_instances);
  Vector Blin_y(Nb_instances);
  Vector Blin_z(Nb_instances);
  Vector X(Nb_instances);

  npoint3 PCi_prof;
  npoint3 Q[Nb_instances];

  for (int boucle_i=0;boucle_i<prof.nb_CP();++boucle_i)
  {
      B=ori; //Valeur initiale de B donnée par l'utilisateur
      for (int k=0;k<Nb_instances;k++)
      {
          //Calcul de B
          traj.dPdu(vbar[k],Tprime);
          traj.d2Pdu2(vbar[k],Tsec);
          Tprime[3]=1.0;
          Tsec[3]=1.0;
          T=npoint3(Tprime);
          T.normalize();
          TT=npoint3(Tsec);
          TT=TT-TT.dotprod(T)*T;          
          double ttnorm= TT.normalize();
          produit_scalaire=B.dotprod(T);

          petitb=B-T*produit_scalaire;
          petitb.normalize();
          B=petitb;
//          if (ttnorm>1e-6)
//            B=B.dotprod(TT)*TT;
//          B=TT*-1.;
          //Détermination de la matrice M
          B.normalize();
          vect_x=T;
          vect_z=B;
          vect_y.crossprod(vect_z,vect_x);
          vect_y.normalize();

          traj.P(vbar[k],vect_o);

          M[0][0]=vect_y[0];M[1][0]=vect_y[1];M[2][0]=vect_y[2];M[3][0]=0;
          M[0][1]=vect_z[0];M[1][1]=vect_z[1];M[2][1]=vect_z[2];M[3][1]=0;
          M[0][2]=vect_x[0];M[1][2]=vect_x[1];M[2][2]=vect_x[2];M[3][2]=0;
          M[0][3]=vect_o[0];M[1][3]=vect_o[1];M[2][3]=vect_o[2];M[3][3]=1;

          //Détermination des Qi,k
          PCi_prof=prof.CP(boucle_i);

          Q[k].x()=M[0][0]*PCi_prof[0]+M[0][1]*PCi_prof[1]+M[0][2]*PCi_prof[2]+M[0][3]*1.0;
          Q[k].y()=M[1][0]*PCi_prof[0]+M[1][1]*PCi_prof[1]+M[1][2]*PCi_prof[2]+M[1][3]*1.0;
          Q[k].z()=M[2][0]*PCi_prof[0]+M[2][1]*PCi_prof[1]+M[2][2]*PCi_prof[2]+M[2][3]*1.0;
         //if (k==1) 
          data_.add_point(Q[k]);

          //Composante X
          Blin_x[k]=Q[k].x();

          //Composante Y
          Blin_y[k]=Q[k].y();

          //Composante Z
          Blin_z[k]=Q[k].z();

      }


      //Composante X
      LU.Solve_Linear_System(Blin_x,X);
      for (int k=0;k<Nb_instances;++k)
      {
          surf.CP(boucle_i,k)[0]=X(k);
      }

      //Composante Y
      LU.Solve_Linear_System(Blin_y,X);
      for (int k=0;k<Nb_instances;++k)
      {
          surf.CP(boucle_i,k)[1]=X(k);
      }

      //Composante Z
      LU.Solve_Linear_System(Blin_z,X);
      for (int k=0;k<Nb_instances;++k)
      {
          surf.CP(boucle_i,k)[2]=X(k);
          surf.CP(boucle_i,k)[3]=1.0;
      //if (boucle_i==1)
  //        data_.add_point(surf.CP(boucle_i,k));
      }
  }

/*
  //enregistrement des coordonnées d'un point de la surface
  npoint pt;
  std::ofstream fichierOUT("vecteur.data");
  surf.P(0,0,pt); for(int ii=0;ii<=2;ii++) { fichierOUT <<pt[ii]<<std::endl; } fichierOUT <<""<<std::endl;
*/

}




int main(void)
{
  data_container data;
  ndisplay display;
  nbspline trajectoire(7,3); // la trajectoire
  nbspline profil(6,2); // le profil
  npoint3 orientation(0,1,0); // l'orientation de depart du profil sur la courbe trajectoire
  nbsplinesurface surface_extrudee; // la surface extrudee

  init_trajectoire(trajectoire);
  init_profil(profil);


  // affichage des courbes profil et trajectoire, et de la surface resultante
  properties p= data.getproplines();
  p.thickness=2.0;
  p.pointsize=2.0;
  p.c=color(255,0,0,255);
  data.setproppoints(p);
  data.setproplines(p);
  
  profil.Display(data);
  p.c=color(0,255,0,255);
  data.setproppoints(p);;
  data.setproplines(p);
  trajectoire.Display(data);
  p.pointsize=2.0;
  p.c=color(255,0,255,255);
  data.setproppoints(p);
  extrude(trajectoire,profil,10,orientation,surface_extrudee, data); // appel a la fonction calculant l'extrusion
  p.pointsize=2.0;
  p.c=color(255,255,255,255);
  data.setproppoints(p);
  surface_extrudee.Display(data);
  display.init_data(data);
  display.display();
  return 0;
}

void init_trajectoire(nbspline &traj)
{
  //trajectoire quelconque
  int degree=traj.degree();
  int nCP=traj.nb_CP();

  //création de la séquence nodale
  for (int i=0;i<degree+1;++i)
  {
    traj.u(i)=0;
    traj.u(i+nCP)=nCP-degree;
  }
  for (int i=1;i<nCP-degree;++i)
  {
    traj.u(i+degree)=i;
  }

  // création des points de contrôle
  for (int i=1;i<nCP-1;++i)
    traj.CP(i)=npoint(0.5*sin((i-1)*0.5/(nCP-3)*n_pi)-0.5,0.5*cos((i-1)*0.5/(nCP-3)*n_pi)-0.5,i*1.0/(nCP-1)+0.5);
    traj.CP(0)=npoint(-1,0.,0);
    traj.CP(nCP-1)=npoint(0,-1,2);

}

void init_profil(nbspline &prof)
{
  // profil dans le plan x-y, point de base = origine du repere
  // le profil sera oriente convenablement
  // lors de la creation des instances.
  int degree=prof.degree();
  int nCP=prof.nb_CP();


  //création de la séquence nodale
  for (int i=0;i<degree+1;++i)
  {
    prof.u(i)=0;
    prof.u(i+nCP)=nCP-degree;
  }
  for (int i=1;i<nCP-degree;++i)
  {
    prof.u(i+degree)=i;
  }

  // création des points de contrôle
  for (int i=0;i<nCP;++i)
    prof.CP(i)=npoint(0.5*sin((i-(nCP-1)/2.)/(nCP-1)*n_pi),-0.25*cos(4*(i-(nCP-1)/2.)/(nCP-1)*n_pi)+0.02,0);
}