// 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>.
//

#include "nsurface.h"
#include "nbeziercurve.h"
#include <cmath>

void nparametricsurface::dPdu(double u_,double v_,npoint& ret) const  // first derivative - generic numerical implementation
{
    double eps=(max_u()-min_u())*1e-5;
    npoint P1,P2;
    if (u_<=min_u()+eps/2) u_+=eps/2;
    if (u_>=max_u()-eps/2) u_-=eps/2;
    P(u_-eps/2,v_,P1);
    P(u_+eps/2,v_,P2);
    npoint3 p1(P1);
    npoint3 p2(P2);
    npoint3 r((p2-p1)/eps);
    ret=r;
    ret.w()=0.;
}


void nparametricsurface::d2Pdu2(double u_,double v_,npoint& ret) const  // second derivative - generic numerical implementation
{
    double eps=(max_u()-min_u())*1e-5;
    npoint P1,P2,P3;
    if (u_<=min_u()+eps) u_+=eps;
    if (u_>=max_u()-eps) u_-=eps;
    P(u_-eps,v_,P1);
    P(u_,v_,P2);
    P(u_+eps,v_,P3);
    npoint3 p1(P1);
    npoint3 p2(P2);
    npoint3 p3(P3);
    npoint3 r((p1+p3-p2*2.0)/(eps*eps));
    ret=r;
    ret.w()=0.;
}

void nparametricsurface::dPdv(double u_,double v_,npoint& ret) const  // first derivative - generic numerical implementation
{
    double eps=(max_v()-min_v())*1e-5;
    npoint P1,P2;
    if (v_<=min_v()+eps/2) v_+=eps/2;
    if (v_>=max_v()-eps/2) v_-=eps/2;
    P(u_,v_-eps/2,P1);
    P(u_,v_+eps/2,P2);
    npoint3 p1(P1);
    npoint3 p2(P2);
    npoint3 r((p2-p1)/eps);
    ret=r;
    ret.w()=0.;
}

void nparametricsurface::d2Pdv2(double u_,double v_,npoint& ret) const  // second derivative - generic numerical implementation
{
    double eps=(max_v()-min_v())*1e-5;
    npoint P1,P2,P3;
    if (v_<=min_v()+eps) v_+=eps;
    if (v_>=max_v()-eps) v_-=eps;
    P(u_,v_-eps,P1);
    P(u_,v_,P2);
    P(u_,v_+eps,P3);
    npoint3 p1(P1);
    npoint3 p2(P2);
    npoint3 p3(P3);
    npoint3 r((p1+p3-p2*2.0)/(eps*eps));
    ret=r;
    ret.w()=0.;
}


void nparametricsurface::d2Pdudv(double u_,double v_,npoint& ret) const  // second derivative - generic numerical implementation
{
    double epsu=(max_u()-min_u())*1e-5;
    double epsv=(max_v()-min_v())*1e-5;
    npoint P1,P2,P3,P4;
    if (u_<=min_u()+epsu/2) u_+=epsu/2;
    if (u_>=max_u()-epsu/2) u_-=epsu/2;
    if (v_<=min_v()+epsv/2) v_+=epsv/2;
    if (v_>=max_v()-epsv/2) v_-=epsv/2;
    P(u_-epsu/2,v_-epsv/2,P1);
    P(u_-epsu/2,v_+epsv/2,P2);
    P(u_+epsu/2,v_-epsv/2,P3);
    P(u_+epsu/2,v_+epsv/2,P4);
    npoint3 p1(P1);
    npoint3 p2(P2);
    npoint3 p3(P3);
    npoint3 p4(P4);
    npoint3 r((p4-p2-p3+p1)/(epsu*epsv));
    ret=r;
    ret.w()=0.;
}


void nparametricsurface::fundamental_forms(double u, double v, double M1[2][2], npoint3 J[3], double M2[2][2], double G[2][2][2], double Minv[2][2]) const  //Computes the fundamental forms 
{
  npoint P,Pu,Pv,Puu,Pvv,Puv;
  npoint3 P_u,P_v,P_uu,P_uv,P_vv;
  npoint3 N;
  d2P(u,v,P,Pu,Pv,Puu,Pvv,Puv);
  Pu[3]=1.0;Pv[3]=1.0;Puu[3]=1.0;Pvv[3]=1.0;Puv[3]=1.0;
  P_u=Pu;P_v=Pv;P_uu=Puu;P_vv=Pvv;P_uv=Puv;
//  dPdu(u,v,P);P[3]=1.0; P_u=P; //P_u : the derivative w/r to u at pt
//  dPdv(u,v,P);P[3]=1.0; P_v=P; //P_v : the derivative w/r to u at pt
  if (J)
  {
    N.crossprod(P_u,P_v);    // N : unit normal by cross product P_u X P_v
    N.normalize();
    J[0]=P_u;
    J[1]=P_v;
    J[2]=N;
  }
//  d2Pdu2(u,v,P);P[3]=1.0; P_uu=P;  //P_uu : second derivative w/r to u at pt
//  d2Pdv2(u,v,P);P[3]=1.0; P_vv=P;   //P_vv : second derivative w/r to v at pt
//  d2Pdudv(u,v,P);P[3]=1.0; P_uv=P;  //P_uv : second derivative w/r to u and v at pt

  M1[0][0] = P_u.dotprod(P_u);
  M1[1][0] = M1[0][1] = P_u.dotprod(P_v);
  M1[1][1] = P_v.dotprod(P_v);
  if (M2)
  {
    if (!J)
    {
      N.crossprod(P_u,P_v);    // N : unit normal by cross product P_u X P_v
      N.normalize();
    }
    M2[0][0] = N.dotprod(P_uu);
    M2[0][1] = M2[1][0] = N.dotprod(P_uv);
    M2[1][1] = N.dotprod(P_vv);
  }
  if (G)
  {
    double (*Mi)[2];
    double Mii[2][2];
    if (Minv) Mi=Minv; else Mi=Mii;
    inverse(M1,Mi);
    
    G[0][0][0]=P_uu.dotprod(P_u)*Mi[0][0]+P_uu.dotprod(P_v)*Mi[1][0];
    G[0][0][1]=P_uu.dotprod(P_u)*Mi[0][1]+P_uu.dotprod(P_v)*Mi[1][1];
    G[1][0][0]=G[0][1][0]=P_uv.dotprod(P_u)*Mi[0][0]+P_uv.dotprod(P_v)*Mi[1][0];
    G[1][0][1]=G[0][1][1]=P_uv.dotprod(P_u)*Mi[0][1]+P_uv.dotprod(P_v)*Mi[1][1];
    G[1][1][0]=P_vv.dotprod(P_u)*Mi[0][0]+P_vv.dotprod(P_v)*Mi[1][0];
    G[1][1][1]=P_vv.dotprod(P_u)*Mi[0][1]+P_vv.dotprod(P_v)*Mi[1][1];
  }
}


void nparametricsurface::rotate(double u, double v, npoint3 V,double angle,npoint3 &Vrot,double M1[2][2]) // rotates a vector in u-v so that the corresponding vector rotates by the prescribed value in cartsian coodinates
{
  double (*MM)[2];
  npoint3 J[3];
  double MM1[2][2];
  if (M1)
    MM=M1;
  else MM=MM1;
  fundamental_forms(u,v,MM,J);
  double a=J[0][0];
  double b=J[0][1];
  double c=J[0][2];
  double d=J[1][0];
  double e=J[1][1];
  double f=J[1][2];

  double m11=c*c+b*b+a*a;
  double m12=c*f+b*e+a*d;
  double k=sqrt(m11)*sqrt( (b*b+a*a)*f*f+
                           (c*c+a*a)*e*e+
                           (c*c+b*b)*d*d
                           -2*c*f*(b*e+a*d)-2*a*b*d*e);
  double ai=sqrt(m11);
  double bi=m12/sqrt(m11);
  double ci=k/m11;

  double ar=1/sqrt(m11);
  double br=-m12/k;
  double cr=m11/k;
  
  Vrot[0]=V[0]*ai+V[1]*bi;
  Vrot[1]=V[1]*ci;
  V[0]=Vrot[0]*cos(angle)-Vrot[1]*sin(angle);
  V[1]=Vrot[0]*sin(angle)+Vrot[1]*cos(angle);
  Vrot[0]=V[0]*ar+V[1]*br;
  Vrot[1]=V[1]*cr;
  Vrot[2]=0.;
}

void nparametricsurface::principal_curvatures(double u, double v,double kappa[2],double V[2][2], double m1[2][2], double m2[2][2],double thr) const //Calcule les directions principales à partir du point pt sur la surface
{
  double (*M1)[2];
  double (*M2)[2];
  if (m1) M1=m1; else M1=new double[2][2];
  if (m2) M2=m2; else M2=new double[2][2];

  fundamental_forms(u,v,M1,0,M2);
  
  double a=det(M1);
  double c=det(M2);
  double b=prodcc(M1,M2)-trace(M1)*trace(M2);
  double delta=b*b-4*a*c;
  if (delta<(thr*thr*a*c))
  {
    for (int i=0;i<2;++i) 
      for (int j=0;j<2;++j)
        V[i][j]=(i==j)?1./sqrt(M1[i][i]):0.;
    kappa[0]=kappa[1]=(-b)/(2*a);
  }
  else 
  {
    delta=sqrt(delta);
    kappa[0]=(-b+delta)/(2*a);
    kappa[1]=(-b-delta)/(2*a);
    double V1[2][2];
    double V2[2][2];
    double norm[2][2];
    for (int i=0;i<2;++i)
    {
      V1[i][0]=-M2[1][0]+kappa[i]*M1[1][0];
      V1[i][1]=-kappa[i]*M1[0][0]+M2[0][0];
      V2[i][0]=M2[1][1]-kappa[i]*M1[1][1];
      V2[i][1]=kappa[i]*M1[1][0]-M2[1][0];
      norm[i][0]=0;
      norm[i][1]=0;
      for (int j=0;j<2;++j) 
      {
        norm[i][0]+=V1[i][j]*V1[i][j];norm[i][1]+=V2[i][j]*V2[i][j];
      }
      norm[i][0]=sqrt(norm[i][0]);
      norm[i][1]=sqrt(norm[i][1]);
    }
    int tt=0;
    double min=norm[0][0];
    for (int i=0;i<2;++i) for (int j=0;j<2;++j) if (min>norm[i][j]) { min=norm[i][j] ; tt=j;}
    for (int i=0;i<2;++i)
    if (tt==0)
      for (int j=0;j<2;++j) V[i][j]=V1[i][j];
    else
      for (int j=0;j<2;++j) V[i][j]=V2[i][j];
    double VV[2][2];
    multi(V,M1,VV);
    double scale[2];
    for (int i=0;i<2;++i)
    {
      scale[i]=0;
      for (int j=0;j<2;++j) scale[i]+=VV[i][j]*V[i][j];
      scale[i]=sqrt(fabs(scale[i]));
      if (scale[i]!=0)
        for (int j=0;j<2;++j) V[i][j]/=scale[i];
      else
        for (int j=0;j<2;++j) V[i][j]=(i==j)?1./sqrt(M1[i][i]):0.;
    }
    if (V[0][0]<0)
    {
      V[0][0]=-V[0][0];
      V[0][1]=-V[0][1];
    }
    else if ((V[0][0]==0) && (V[0][1]<0))
    {
      V[0][0]=-V[0][0];
      V[0][1]=-V[0][1];
    }
    double pvec=det(V);
    if (pvec<0)
    {
      V[1][0]=-V[1][0];
      V[1][1]=-V[1][1];
    }
  }
  if (!m1) delete[] M1;
  if (!m2) delete[] M2;
}

void nparametricsurface::Display(data_container& data) const    // default : uses P(u,res)
{
  properties p,p1;
  p=data.getproppoints();
  p1=p;
  if (p.nb_uid<2) p.nb_uid=2;
  for (int i=0;i< nb_CP();i++)
  {
    p.uids[1]=i;
    data.setproppoints(p);
    data.add_point(CP(i));
  }
  data.setproppoints(p1);
  const int nbptsu=10*nb_CP_u() +100;
  const int nbptsv=10*nb_CP_v() +100;
  std::vector<npoint> dep(nbptsv+1);
  std::vector<npoint> arr(nbptsv+1);
  bool first=true;
  double epsu= (max_u()-min_u()) / (nbptsu);
  double epsv= (max_v()-min_v()) / (nbptsv);
  for (double u=min_u();u<= (max_u() +epsu/2);u+=epsu)
  {
    if (first)
    {
      int i=0;
      for (double v=min_v();v<= (max_v() +epsv/2);v+=epsv)
      {
        P(u,v,arr[i++]);
      }
      first=false;
    }
    else
    {
      dep=arr;
      int i=0;
      for (double v=min_v();v<= (max_v() +epsv/2);v+=epsv)
      {
        P(u,v,arr[i++]);
      }
      quad q;
      for (int j=0;j< (i-1);++j)
      {
        q.pts[0]=dep[j];
        q.pts[1]=arr[j];
        q.pts[2]=arr[j+1];
        q.pts[3]=dep[j+1];
        data.add_quad(q);
      }
//      return;
    }
  }
}

void nparametricsurface::relative_curvatures(const nparametriccurve &c, double t,double kappa[2], double M1[2][2], double M2[2][2]) const  //Computes normal and geodesic curvatures of an embedded curve
{
  nembeddedcurve emb_crv(c,*this);
  emb_crv.relative_curvatures(t,kappa,M1,M2);
}

void nparametricsurface::find_geodesic(double u,double v, npoint3 dpdt,npoint3 &d2pdt2) const  // find the local second derivatives of a curve, givent its tangent vector, that is a geodesic
{
  double G[2][2][2];
  npoint3 J[3];
  double M1[2][2],M2[2][2];
  fundamental_forms(u,v,M1,J,M2,G);
//  double VV[2]={dpdt[0],dpdt[1]};
//  double ds2=multi(VV,M1,VV);
//  double ds=sqrt(ds2);
  for (int k=0;k<2;++k)
  {
    d2pdt2[k]=0;
    for (int i=0;i<2;++i)
      for (int j=0;j<2;++j)
        d2pdt2[k]-=G[i][j][k]*dpdt[i]*dpdt[j];
  }
  d2pdt2[2]=0.;
}


bool nparametricsurface::walk(double u, double v, npoint3 dir,double length,npoint3 &newpos,npoint3 &newdir,int steps) // walks along a geodesic
{
  npoint3 pt0(u,v,0);
  npoint3 pt1;//=pt0+dir*delta;
  npoint3 pt2;
  npoint3 dpdt=dir;
  npoint3 d2pdt2;
  double M1[2][2];
  bool ret=true;
  bool notok=true;
  while (notok)
  {
    pt0=npoint3(u,v,0);
    int i;
    double l1=0.;
    double l2=0.;
    for (i=0;i<steps;++i)
    {
      fundamental_forms(pt0[0],pt0[1],M1);
/*      for (int c=0;c<2;++c)
      {
        for (int l=0;l<2;++l)
          std::cout << M1[c][l] << " ";
        std::cout << std::endl;
      }
        std::cout << std::endl;
*/        
      dpdt.normalize();
      double ds2=multi(dpdt.array(),M1,dpdt.array());
//      std::cout << ds2 << std::endl;
      dpdt/=sqrt(ds2);
      dpdt*=length/steps;
      find_geodesic(pt0[0],pt0[1],dpdt,d2pdt2);
//      d2pdt2.print(std::cout);
      pt1=pt0+dpdt;
      pt2=d2pdt2*2+pt1*2-pt0;
      nbeziercurve crv(3);
      crv.CP(0)=pt0;crv.CP(1)=pt1;crv.CP(2)=pt2;
      npoint3 ppt1=(pt1+pt0)/2;
      npoint pt,pts1,pts2,pts3;
      crv.P(0.5,pt);
//      pt.print(std::cout);
      P(pt0[0],pt0[1],pts1);
      P(ppt1[0],ppt1[1],pts2);
      P(pt[0],pt[1],pts3);
      npoint3 Pts1,Pts2,Pts3;
      Pts1=pts1;Pts2=pts2;Pts3=pts3;
      npoint3 dpt1=Pts2-Pts1;
      npoint3 dpt2=Pts3-Pts2;
      npoint3 dpt3=Pts3-Pts1;
      double l1=dpt1.norm()+dpt2.norm();
      double l2=dpt3.norm();
//      std::cout<< l1 << " " << l2 << std::endl;
      if (((l1-l2)/length ) > 1e-5) 
      {
//        std::cout << l2 << " <= L <= " << l1 << " delta%= " << (l1-l2)/length << std::endl;
        int rest=steps-i;
        steps*=2;
        i=steps-rest*2-1;

//        std:: cout << steps << std::endl;
        continue;
//        break;
      }
      
      if (((l1-l2)/length ) < 1e-10) 
      {
        int rest=steps-i;
//        std:: cout << steps << std::endl;
        if (steps%2==0 && (rest%2==0))
        {
          steps/=2;
          i=steps-rest/2;
//          std:: cout << steps << std::endl;
//          continue;
        }
//        break;
      }
      pt0=pt;   
      crv.dPdu(0.5,pt);
      pt[3]=1.0;
      dpdt=pt;
      if (pt0[0]<=max_u()&&pt0[0]>=min_u()&&pt0[1]<=max_v()&&pt0[1]>=min_v())
      {
        newpos=pt0;
        newdir=dpdt;
      }
      else
      {
        ret=false;
        notok=false;
        break;
      }
    }
    if (i==steps) notok=false;
  }
  return ret;
}