/**
*******************************************************************************

  @file /gui/engine/AdvGeom.cpp
  @brief Krivky, cesty
  

  @author Vajicek
  @version 0.1
 
******************************************************************************/
#include "gui/engine/AdvGeom.h"

//
#include "common/exc.h"
#include "common/Log.h"

//
#include <GL/gl.h>
#include <assert.h>

/*****************************************************************************/

namespace gui{


TGenericCurve::TGenericCurve(){
  controlPoints = NULL;
  controlPoints_c = 0;
}

TGenericCurve::~TGenericCurve(){
  if(controlPoints)KMemFree(controlPoints);
}

P3F* TGenericCurve::controlPoint(int i)
{
  return controlPoints+i;
}

int TGenericCurve::controlPointCount(){
  return controlPoints_c;
}

void TGenericCurve::addPoint(float x, float y, float z){
  
  controlPoints = (P3F*)KExtArrayRealloc( controlPoints, sizeof(P3F), 
                                          controlPoints_c+1, controlPoints_c);

  controlPoints[ controlPoints_c ].x = x;
  controlPoints[ controlPoints_c ].y = y;
  controlPoints[ controlPoints_c ].z = z;
  controlPoints_c++;
}

void TGenericCurve::addPoint(P3F* p){
  addPoint(p->x, p->y, p->z);
}

void TGenericCurve::clear(){
  if(controlPoints)
    KMemFree(controlPoints);
  controlPoints_c = 0;
}









/*****************************************************************************/

void TJoinQuadricBezierCurve::matrix(int i, MX4 m){
  P3F *p1,*p2;
  P3F a,b,c,d;
  MX4 G;

  //VYROB MATICI
  p1 = controlPoints+i; p2 = controlPoints+i+1;

  SETV3(a.p, (p1->x + p2->x)/2, (p1->y + p2->y)/2, (p1->z + p2->z)/2);

  p1 = controlPoints+i+1; p2 = controlPoints+i+2;

  SETV3(d.p, (p1->x + p2->x)/2, (p1->y + p2->y)/2, (p1->z + p2->z)/2);

  SETV3(b.p, p1->x, p1->y, p1->z);
  SETV3(c.p, p1->x, p1->y, p1->z);
  //
  SetMX4(G, a.x, a.y, a.z, 1,
            b.x, b.y, b.z, 1,
            c.x, c.y, c.z, 0,
            d.x, d.y, d.z, 0);

  MulMatrixMX4(m,M,G);
}

void TJoinQuadricBezierCurve::evalSecDerivation(float t, P3F* p){
  int i = calcInv(&t);
  P4F T,W;
  MX4 MG;

  //
  matrix(i, MG);
  //VYPOCET
  SETV4(T.p, 6*t, 2, 0, 0);
  ApplyVectorMX4(T.p, MG, W.p);

  CopyV3(p->p,W.p);
}

void TJoinQuadricBezierCurve::evalDerivation(float t, P3F* p){
  int i = calcInv(&t);
  P4F T,W;
  MX4 MG;

  //
  matrix(i, MG);
  //VYPOCET
  if(t==0)t=0.001f;
  SETV4(T.p, 3*t*t, 2*t, 1, 0);
  ApplyVectorMX4(T.p, MG, W.p);

  CopyV3(p->p,W.p);
}

int TJoinQuadricBezierCurve::calcInv(float* t){
  int n = controlPoints_c-2;    //pocet useku
  float f = *t * nsize;         //prevod na <0,1> -> <0..pocet useku>
  int j = (int)floor(f);        //cislo normovaneho subuseku
  int i = (int)floor(inv[j]*n); //cislo useku

  float tt = f - j;             //zbytek

  //interpoluj v subuseku
  f = (inv[j]*n*(1-tt) + inv[j+1]*n*tt) - i;
  if(f > 1){
    f = f - 1;
    i++;
  }

  *t = f;
  return i;
}

/**vyrobi transformacni matici*/
void TJoinQuadricBezierCurve::eval(float t, MX4 m){
  P3F T,TT, N, B, P;
  
  //z polohy translaci
  eval(t, &P);
  //z derivaci rotaci
  evalDerivation(t, &T);
  evalSecDerivation(t, &TT);

  //b(u) = (f'(u) × f''(u)) / | (f'(u) × f''(u)) |
  VectMulV3(N.p,TT.p,T.p);  
  float sN = SizeV3(N.p);
  MulV3(N.p,(1/sN));

  //n(u) = ( b(u) × f'(u) ) / | b(u) × f'(u) |
  VectMulV3(B.p,T.p,N.p);  
  float sB = SizeV3(B.p);
  MulV3(B.p,(1/sB));

  //t(u) = f'/|f|
  float sT = SizeV3(T.p);
  MulV3(T.p,(1/sT));

  SetMX4(m,   T.x,  T.y,  T.z,  0, 
              N.x,  N.y,  N.z,  0,
              B.x,  B.y,  B.z,  0,
              P.x,  P.y,  P.z,  1);
}

void TJoinQuadricBezierCurve::eval(float t, P3F* p){
  //
  int i = calcInv(&t);
  
  //VYPOCET pro usek i a parametr t
  P4F T,W;
  MX4 MG;
  //
  matrix(i, MG);
  //
  SETV4(T.p, t*t*t, t*t, t, 1);
  ApplyVectorMX4(T.p, MG, W.p);
  //
  CopyV3(p->p,W.p);
}

void TJoinQuadricBezierCurve::build(){
  int n = controlPoints_c - 2; //pocet useku
  MX4 MG;
  int i,j;
  P4F T,W;
  int ci = 0;
  float t;
  float px,py,pz;
  //
  nsize = n*size;
  float* cum = (float*)KMemAlloc( (nsize + 1)*sizeof(float) );
  //

  for(i=0; i < n;i++){
    //VYROB MATICI
    matrix(i, MG);
    //
    j = (i+1-n)?0:-1;
    t = 0;
    while( j<size ){

      SETV4(T.p, t*t*t, t*t, t, 1);

      ApplyVectorMX4(T.p, MG, W.p);

      if(ci>0)	cum[ci] = cum[ci-1] + (float)DIST(px, py, pz, W.x, W.y, W.z);
      else 			cum[ci]=0;

      ci++;
      t = t + delta;
      j++;
      px = W.x;py = W.y;pz = W.z;
    }
  }//for

  //VYROB INVERZI
  float D2 = delta/n;
  float TT = 0;
  len = cum[ nsize ];

  if(inv)KMemFree(inv);
  inv = (float*)KMemAlloc( (nsize + 1)*sizeof(float) );
  inv_c = (nsize + 1);

  i = 0;
  j = 1;
  inv[0] = 0;
  while( j < nsize + 1 ){
    //prochazi useky
    while( (cum[i] < (j*D2)*len) && (i < nsize + 1) )
	    i++;
    //relativni pozice uvnitr useku
    TT = ( j*D2*len - cum[i-1] ) / ( cum[i] - cum[i-1] );
    inv[j] = ((i - 1) + TT)*D2;

    j++;
  }

  KMemFree(cum);
}

void TJoinQuadricBezierCurve::addPoint(float x, float y, float z){

  TGenericCurve::addPoint(x,y,z);

  if(controlPoints_c>2)
    nsize = ( controlPoints_c - 2 ) * size;
  else 
    nsize = 0;
}

float TJoinQuadricBezierCurve::length(){
  return len;
}

TJoinQuadricBezierCurve::~TJoinQuadricBezierCurve(){
  if(inv)
    KMemFree( inv );
}

TJoinQuadricBezierCurve::TJoinQuadricBezierCurve(int _size){
  SetMX4(M, -1,  3,   -3,  1,
            3,  -6,   3,   0,
            -3,  3,   0,   0,
            1,   0,   0,   0);
  size = _size;
  nsize = 0;
  delta = 1.0f/size;
  //
  inv_c = 0;
  inv = NULL;
  len = -1;
}

void TJoinQuadricBezierCurve::clear(){
  TGenericCurve::clear();

  if(inv)
    KMemFree(inv);
  inv_c = 0;
}









/*****************************************************************************/

void TJoinLinearCurve::evalSecDerivation(float t, P3F* p){
  SETV3(p->p,0,0,0);
}
void TJoinLinearCurve::evalDerivation(float t, P3F* p){
  float len = length();
  float dist = 0; 
  t = MAX(MIN(1, t), 0);
  int i = 0;
  //TODO: binarni vyhledavani
  while(dist/len > t){
    dist += DISTV3(controlPoints[i].p, controlPoints[i+1].p);
    i++;
  }
  assert( i < controlPoints_c);
  CopyV3(p->p, controlPoints[i].p);
  SubV3(p->p, controlPoints[i-1].p);
  NormalizeV3(p->p);
  dist = DISTV3(controlPoints[i-1].p, controlPoints[i].p);
  MulV3(p->p, len/dist);    
}
void TJoinLinearCurve::eval(float t, P3F* p){
  float len = length();
  float dist = 0; 
  t = MAX(MIN(1, t), 0);
  int i = 0;
  while(dist/len > t){
    dist += DISTV3(controlPoints[i].p, controlPoints[i+1].p);
    i++;
  }
  assert( i < controlPoints_c);
  float t2 = dist/len;
  float t1 = (dist - DISTV3(controlPoints[i-1].p, controlPoints[i].p))/len;
  float tt = t2-t/(t2-t1);
  //
  CopyV3(p->p, controlPoints[i].p);
  SubV3(p->p, controlPoints[i-1].p);    
  MulV3(p->p, tt);
}
void TJoinLinearCurve::eval(float t, MX4 m){
  P3F p;
  eval(t, &p);
  SetTransMX4(m, p.x, p.y, p.z);
}
void TJoinLinearCurve::build(){
}
float TJoinLinearCurve::length(){
  float len = 0;
  for(int i = 0; i < controlPoints_c-1; i++){
    len += DISTV3(controlPoints[i].p, controlPoints[i+1].p);
  }
  return len;
}
//
TJoinLinearCurve::TJoinLinearCurve(){
  controlPoints_c = 0;
  controlPoints = NULL;
}









/*****************************************************************************/

float THermiteanCubic::arcParam(float t)
{
  //nalezni usek
  int i = 1;
  while( t > (cum[i]/cum[cum_c-1]) ){
      i++;      
  }    
  //dolni + horni hranice
  float t1 = cum[i-1]/cum[cum_c-1];
  float t2 = cum[i]/cum[cum_c-1];
  //linearni interpolace - urceni polohy tt (0,1) mezi t1 a t2
  float tt = (t-t1)/(t2-t1);
  return (i - 1 + tt) / (cum_c-1); 
}

void THermiteanCubic::evalSecDerivation(float t, P3F* p){
  t = arcParam(t); 
  
  P4F tt, pp;
  SETV4(tt.p, 6*t, 2, 0, 0);
  
  ApplyVectorMX4(tt.p, mg, pp.p);
  CopyV3(p->p, pp.p);
}


void THermiteanCubic::evalDerivation(float t, P3F* p){
  t = arcParam(t); 
  
  P4F tt, pp;
  SETV4(tt.p, 3*t*t, 2*t, 1, 0);
  
  ApplyVectorMX4(tt.p, mg, pp.p);
  CopyV3(p->p, pp.p);
}


void THermiteanCubic::eval(float t, P3F* p){
  t = arcParam(t); 
  
  P4F tt, pp;
  SETV4(tt.p, t*t*t, t*t, t, 1);
  
  ApplyVectorMX4(tt.p, mg, pp.p);
  CopyV4toV3(p->p, pp.p);
}


void THermiteanCubic::eval(float t, MX4 m){
  P3F pos, dir, nor, bnor;
  //
  eval(t, &pos);
  evalDerivation(t, &dir);
  evalSecDerivation(t, &nor);
  //
  VectMulV3(bnor.p, dir.p, nor.p);
  SetMX4( m,  
          bnor.x, bnor.y, bnor.z, 0,
          dir.x, dir.y, dir.z, 0,
          nor.x, nor.y, nor.z, 0,
          pos.x, pos.y, pos.z, 1);
}


void THermiteanCubic::build(){
  MX4 g;
  float t;
  P4F tt;
  P4F p1,p2;
  SetMX4(g, 
        controlPoints[0].x,controlPoints[0].y, controlPoints[0].z, 1,
        controlPoints[2].x,controlPoints[2].y, controlPoints[2].z, 1,
        controlPoints[1].x,controlPoints[1].y, controlPoints[1].z, 0,
        controlPoints[3].x,controlPoints[3].y, controlPoints[3].z, 0);
  MulMatrixMX4(mg, m, g);

  SETV4(p1.p, controlPoints[0].x, controlPoints[0].y, controlPoints[0].z, 1);
  cum[0] = 0;
  for(int i=1; i < cum_c;i++){
    t = ((float)i)/(cum_c-1);
    SETV4(tt.p, t*t*t, t*t, t, 1);
    //
    ApplyVectorMX4(tt.p, mg, p2.p);
    MulV4(p2.p, (1.0f/p2.p[3]));
    cum[i] = cum[i-1] + DISTV3(p1.p, p2.p);
    //
    CopyV4(p1.p, p2.p);      
  }
}

float THermiteanCubic::length(){
  return cum[cum_c-1];
}

THermiteanCubic::THermiteanCubic(int seg){
  SetMX4(m, 
        2,-2, 1, 1,
      -3, 3,-2,-1,
        0, 0, 1, 0,
        1, 0, 0, 0);

  cum = (float*)KMemAlloc(sizeof(float)* seg);
  cum_c = seg;
}

THermiteanCubic::~THermiteanCubic(){
  if(cum)KMemFree(cum);
}









/*****************************************************************************/

int TJoinHermiteanCubic::curveCount(){
  if(controlPoints_c < 4)
    return 0;

  return (controlPoints_c - 2) / 2;
}


float TJoinHermiteanCubic::arcParam(float t, int* seg)
{
  //nalezni usek
  int i = 1; // BUG ???????
  try
  {
    while( t > (cum[i]/cum[cum_c-1]) )
    { 
      i++;      
      if (i>=cum_c)
      {
        printf("DIVIDION BY ZERO BUG t: %f, div: %f, seg_c: %i, i: %i",t,cum[i]/cum[cum_c-1],seg_c,i);
//        break;
      }
      if ((cum[cum_c-1]==0) || ((cum[i]/cum[cum_c-1])==0))
      {
        printf("DIVIDION BY ZERO BUG t: %f, div: %f, seg_c: %i, i: %i",t,cum[i]/cum[cum_c-1],seg_c,i);
        //break;
      }

    }    
  }
  catch(...)
  {
    printf("DIVIDION BY ZERO BUG t: %f, div: %f, seg_c: %i, i: %i",t,cum,seg_c,i);
  }
  //
  int s = (i-1) / (seg_c-1);
  int sb = s * (seg_c-1);
  int se = (s+1) * (seg_c-1);
  //relativni pozice dolni + horni hranice
  float t1 = (cum[i-1] - cum[sb])/(cum[se] - cum[sb]);
  float t2 = (cum[i] - cum[sb])/(cum[se] - cum[sb]);
  
  //prepocitej t z (0,1) cele krivky na (0,1) segmentu
  t = (t - cum[sb]/cum[cum_c-1]) / (cum[se]/cum[cum_c-1] - cum[sb]/cum[cum_c-1]);

  //linearni interpolace - urceni polohy tt (0,1) mezi t1 a t2
  float tt = (t-t1)/(t2-t1);
  //
  *seg = s;
  return (i - 1 - sb + tt) / (seg_c - 1); 
}

void TJoinHermiteanCubic::evalSecDerivation(float t, P3F* p){
  int seg;
  t = arcParam(t, &seg); 
  P4F tt, pp;
  SETV4(tt.p, 6*t, 2, 0, 0);
  
  ApplyVectorMX4(tt.p, mg[seg], pp.p);
  CopyV3(p->p, pp.p);
}

void TJoinHermiteanCubic::evalDerivation(float t, P3F* p){
  int seg;
  t = arcParam(t, &seg); 
  P4F tt, pp;
  SETV4(tt.p, 3*t*t, 2*t, 1, 0);
  
  ApplyVectorMX4(tt.p, mg[seg], pp.p);
  CopyV3(p->p, pp.p);

}

void TJoinHermiteanCubic::eval(float t, P3F* p){
  int seg;
  t = arcParam(t, &seg); 
  
  P4F tt, pp;
  SETV4(tt.p, t*t*t, t*t, t, 1);
  
  ApplyVectorMX4(tt.p, mg[seg], pp.p);
  CopyV4toV3(p->p, pp.p);
}

void TJoinHermiteanCubic::eval(float t, MX4 m){
  P3F pos, dir, nor, bnor;
  //
  eval(t, &pos);
  evalDerivation(t, &dir);
  evalSecDerivation(t, &nor);
  //
  VectMulV3(bnor.p, dir.p, nor.p);
  SetMX4( m,  
          bnor.x, bnor.y, bnor.z, 0,
          dir.x, dir.y, dir.z, 0,
          nor.x, nor.y, nor.z, 0,
          pos.x, pos.y, pos.z, 1);
}

void TJoinHermiteanCubic::build(){
  MX4 g;
  float t;
  P4F tt;
  P4F p1,p2;

  int cc = curveCount();
  if(!cc) THROW(E_8K_GUI, "TJoinHermiteanCubic::build - no path to build");

  //vyrob matice
  if(mg)KMemFree(mg);
  mg = (MX4*)KMemAlloc(sizeof(MX4) * cc);
  for(int j = 0; j < cc; j++){
    SetMX4(g, 
          controlPoints[j*2].x,controlPoints[j*2].y, controlPoints[j*2].z, 1,
          controlPoints[j*2+2].x,controlPoints[j*2+2].y, controlPoints[j*2+2].z, 1,
          controlPoints[j*2+1].x,controlPoints[j*2+1].y, controlPoints[j*2+1].z, 0,
          controlPoints[j*2+3].x,controlPoints[j*2+3].y, controlPoints[j*2+3].z, 0);
    MulMatrixMX4(mg[j], m, g);
  }

  //alokuj misto pro cumulacni buffer
  if(cum)KMemFree(cum);
  cum = (float*)KMemAlloc(sizeof(float)* (cc * (seg_c-1) + 1));
  cum_c = cc * (seg_c-1) + 1;

  
  //napln kumulacni buffer
  SETV4(p1.p, controlPoints[0].x, controlPoints[0].y, controlPoints[0].z, 1);
  cum[0] = 0;
  for(int k = 0; k < cc; k++){
    for(int i=1; i < seg_c; i++){
      t = ((float)i)/(seg_c-1);
      SETV4(tt.p, t*t*t, t*t, t, 1);
      //
      ApplyVectorMX4(tt.p, mg[k], p2.p);
      MulV4(p2.p, (1.0f/p2.p[3]));
      cum[(seg_c-1)*k + i] = cum[(seg_c-1)*k + i - 1] + DISTV3(p1.p, p2.p);
      //
      CopyV4(p1.p, p2.p);      
    }
  }
}

float TJoinHermiteanCubic::length(){
  return cum[cum_c-1];
}

TJoinHermiteanCubic::TJoinHermiteanCubic(int seg){
  //
  SetMX4(m, 
         2,-2, 1, 1,
        -3, 3,-2,-1,
         0, 0, 1, 0,
         1, 0, 0, 0);

  mg = NULL;
  seg_c = seg;
  cum = NULL;
  cum_c = 0;
}

TJoinHermiteanCubic::~TJoinHermiteanCubic(){
  if(mg)KMemFree(mg);
  if(cum)KMemFree(cum);
}


}//namespace
/*****************************************************************************/