/****************************************************************************
Copyright (C) 2012 HidraVFX development team
            <https://launchpad.net/~hidravfx-dev-team>

This file is part of the HidraVFX project.

HidraVFX is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

HidraVFX is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with HidraVFX. If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/

#include <math.h>
#include <mathplus.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include "vertex.h"
#include "spline.h"



tcubicspline tcubicspline_of_tpoint(tpoint p0,tpoint p1,tpoint p2,tpoint p3)
{
  tcubicspline s;
  s.point[0] = tpoint_copy(&p0);
  s.point[1] = tpoint_copy(&p1);
  s.point[2] = tpoint_copy(&p2);
  s.point[3] = tpoint_copy(&p3);
  return s;
}

tcubicspline tcubicspline_of_double(double p0x,double p0y,double p1x,double p1y,double p2x,double p2y,double p3x,double p3y)
{
  tcubicspline s;
  s.point[0].x = p0x;
  s.point[0].y = p0y;
  s.point[0].z = 0.0;
  s.point[1].x = p1x;
  s.point[1].y = p1y;
  s.point[1].z = 0.0;
  s.point[2].x = p2x;
  s.point[2].y = p2y;
  s.point[2].z = 0.0;
  s.point[3].x = p3x;
  s.point[3].y = p3y;
  s.point[3].z = 0.0;
  return s;
}

/* 0 <== u <== 1 */
tpoint bspline_of_tpoint(tpoint p0, tpoint p1,tpoint p2, double u)
{
  tpoint p;
  double a1,a2,a3;

  a1 = u*u-2*u+1;
  a2 = -2*u*u+2*u+1;
  a3 = u*u;
  p.x = 0.5*(a1*p0.x + a2*p1.x + a3*p2.x);
  p.y = 0.5*(a1*p0.y + a2*p1.y + a3*p2.y);
  p.z = 0.0;
  return p;
}

/* 0 <== u <== 1 */
tpoint bspline_of_tcubicspline(tcubicspline *p, double u)
{
  tpoint result;
  double a1,a2,a3,a4;

  a1 = - 1*u*u*u + 3*u*u - 3*u +1;
  a2 =   3*u*u*u - 6*u*u +     +4;
  a3 = - 3*u*u*u + 3*u*u + 3*u +1;
  a4 =   1*u*u*u;
  result.x = (1.0/6.0)*(a1*p->point[0].x + a2*p->point[1].x + a3*p->point[2].x + a4*p->point[3].x);
  result.y = (1.0/6.0)*(a1*p->point[0].y + a2*p->point[1].y + a3*p->point[2].y + a4*p->point[3].y);
  result.z = 0.0;
  return result;
}

/* catmull-rom curve in 2d */
/* u futó változó, értéke: [1] */
/* the curve is drawn between p1 & p2 */
/* p0 & p3 are extra interval control points */
tpoint catmullrom_of_tcubicspline(tcubicspline p, double u)
{
  tpoint result;
  double u2,u3;

  u2 = u * u;
  u3 = u2 * u;
  result.x = 0.5 * ( ( 2.0 * p.point[1].x ) +
                ( -p.point[0].x + p.point[2].x ) * u +
                ( 2.0 * p.point[0].x - 5.0 * p.point[1].x + 4.0 * p.point[2].x - p.point[3].x ) * u2 +
                ( -p.point[0].x + 3.0 * p.point[1].x - 3.0 * p.point[2].x + p.point[3].x ) * u3 );
   result.y = 0.5 * ( ( 2.0 * p.point[1].y ) +
              ( -p.point[0].y + p.point[2].y ) * u +
                ( 2.0 * p.point[0].y - 5.0 * p.point[1].y + 4 * p.point[2].y - p.point[3].y ) * u2 +
                ( -p.point[0].y + 3.0 * p.point[1].y - 3.0 * p.point[2].y + p.point[3].y ) * u3 );
   result.z = 0.0;
   return result;
}

/* bezier curve in 2d */
tpoint bezier_of_2tpoints(tpoint p0,tpoint p1, double u)
{
  tpoint result;

  result.x = (1.0 - u) * p0.x + u * p1.x;
  result.y = (1.0 - u) * p0.y + u * p1.y;
  result.z = 0.0;
  return result;
}

/* bezier curve in 2d */
/* u futó változó, értéke: [1] */
tpoint bezier_of_3tpoints(tpoint p0,tpoint p1,tpoint p2,double u)
{
  tpoint result;

  result.x = sqr(1-u) * p0.x + 2.0 * (1.0-u) * u * p1.x + sqr(u) * p2.x;
  result.y = sqr(1-u) * p0.y + 2.0 * (1.0-u) * u * p1.y + sqr(u) * p2.y;
  result.z = 0.0;
  return result;
}

/* bezier curve in 2d */
/* u futó változó, értéke: [1] */
tpoint bezier_of_tcubicspline(tcubicspline *p,double u)
{
  tpoint result;

  result.x = sqr3(1.0-u)*p->point[0].x + 3.0*sqr(1.0-u)*u*p->point[1].x + 3.0*(1.0-u)*sqr(u)*p->point[2].x + sqr3(u)*p->point[3].x;
  result.y = sqr3(1.0-u)*p->point[0].y + 3.0*sqr(1.0-u)*u*p->point[1].y + 3.0*(1.0-u)*sqr(u)*p->point[2].y + sqr3(u)*p->point[3].y;
  result.z = 0.0;
  return result;
}

// first derivate of a bezier
tpoint beziertangent_of_tcubicspline(tcubicspline *bezier, double u)
{
  tpoint result;

  result.x = - 3.0*sqr(1.0-u)*bezier->point[0].x + 3.0*sqr(1.0-u)*bezier->point[1].x
               - 6.0*(1.0-u)*u*bezier->point[1].x + 6.0*(1.0-u)*u*bezier->point[2].x
               - 3.0*u*u*bezier->point[2].x + 3.0*u*u*bezier->point[3].x;
  result.y = - 3.0*sqr(1.0-u)*bezier->point[0].y + 3.0*sqr(1.0-u)*bezier->point[1].y
               - 6.0*(1.0-u)*u*bezier->point[1].y + 6.0*(1.0-u)*u*bezier->point[2].y
               - 3.0*u*u*bezier->point[2].y + 3.0*u*u*bezier->point[3].y;
  result.z = 0.0;
  return result;
}

// second derivate of a bezier
tpoint bezierradius_of_tcubicspline(tcubicspline *bezier, double u)
{
  tpoint result;

  result.x = 6.0*(1.0-u)*bezier->point[0].x + 6.0*u*bezier->point[3].x
               + (-6.0*(1.0-u)+6.0*u-6.0*(1.0-u))*bezier->point[1].x
               + (-6.0*u+6.0*(1.0-u)-6.0*u)*bezier->point[2].x;
  result.y = 6.0*(1.0-u)*bezier->point[0].y + 6.0*u*bezier->point[3].y
               + (-6.0*(1.0-u)+6.0*u-6.0*(1.0-u))*bezier->point[1].y
               + (-6.0*u+6.0*(1.0-u)-6.0*u)*bezier->point[2].y;
  result.z = 0.0;
  return result;
}

// cut a bezier to 2 bezier in point in u
void decasteljau(tcubicspline *b, tcubicspline *b1, tcubicspline *b2, double u)
{
  int i,j,n;
  tpoint p[3][3];
  n = 3;
  for( i = 0 ; i <= 3 ; i ++) {
    p[0][i] = b->point[i];
  }
  for( j = 1 ; j <= n ; j ++)
    for( i = 0 ; i <= n-j ; i ++) {
      p[j][i].x = (1-u)*p[j-1][i].x + u*p[j-1][i+1].x;
      p[j][i].y = (1-u)*p[j-1][i].y + u*p[j-1][i+1].y;
   }
   b1->point[0] = p[0][0];
   b1->point[1] = p[1][0];
   b1->point[2] = p[2][0];
   b1->point[3] = p[3][0];
   b2->point[0] = p[3][0];
   b2->point[1] = p[2][1];
   b2->point[2] = p[1][2];
   b2->point[3] = p[0][3];
}

// bezier felület
//u,v futó változók, értéke:[1]
 tpoint bezier_of_t3dpointgrid(t3dpointgrid p, double u,double v)
{
  double u1,u2,u3,u4,v1,v2,v3,v4;
  tpoint result;

  u1 = (1-u)*(1-u)*(1-u);
  u2 = 3*u*(1-u)*(1-u);
  u3 = 3*u*u*(1-u);
  u4 = u*u*u;
  v1 = (1-v)*(1-v)*(1-v);
  v2 = 3*v*(1-v)*(1-v);
  v3 = 3*v*v*(1-v);
  v4 = v*v*v;
  result.x = (u1*p[0][0].x + u2*p[1][0].x + u3*p[2][0].x + u4*p[3][0].x) * v1 +
             (u1*p[0][1].x + u2*p[1][1].x + u3*p[2][1].x + u4*p[3][1].x) * v2 +
             (u1*p[0][2].x + u2*p[1][2].x + u3*p[2][2].x + u4*p[3][2].x) * v3 +
             (u1*p[0][3].x + u2*p[1][3].x + u3*p[2][3].x + u4*p[3][3].x) * v4;
  result.y = (u1*p[0][0].y + u2*p[1][0].y + u3*p[2][0].y + u4*p[3][0].y) * v1 +
             (u1*p[0][1].y + u2*p[1][1].y + u3*p[2][1].y + u4*p[3][1].y) * v2 +
             (u1*p[0][2].y + u2*p[1][2].y + u3*p[2][2].y + u4*p[3][2].y) * v3 +
             (u1*p[0][3].y + u2*p[1][3].y + u3*p[2][3].y + u4*p[3][3].y) * v4;
  result.z = (u1*p[0][0].z + u2*p[1][0].z + u3*p[2][0].z + u4*p[3][0].z) * v1 +
             (u1*p[0][1].z + u2*p[1][1].z + u3*p[2][1].z + u4*p[3][1].z) * v2 +
             (u1*p[0][2].z + u2*p[1][2].z + u3*p[2][2].z + u4*p[3][2].z) * v3 +
             (u1*p[0][3].z + u2*p[1][3].z + u3*p[2][3].z + u4*p[3][3].z) * v4;
  return result;
}

void assign_bezier(tcubicspline *base, tcubicspline *ne, double l)
{
  ne->point[0].x = base->point[3].x;
  ne->point[1].x = ne->point[0].x + l * (base->point[3].x-base->point[2].x);
  ne->point[0].y = base->point[3].y;
  ne->point[1].y = ne->point[0].y + l * (base->point[3].y-base->point[2].y);
}

void assign_bezier_2(tcubicspline *base, tcubicspline *ne, double l, double n)
{
   ne->point[0].x = base->point[3].x;
   ne->point[1].x = ne->point[0].x + l * (base->point[3].x-base->point[2].x);
   ne->point[0].y = base->point[3].y;
   ne->point[1].y = ne->point[0].y + l * (base->point[3].y-base->point[2].y);
   ne->point[2].x = l*l*base->point[1].x - (2*l*l+2*l+n/2)*base->point[2].x + (l*l+2*l+1+n/2)*base->point[3].x;
   ne->point[2].y = l*l*base->point[1].y - (2*l*l+2*l+n/2)*base->point[2].y + (l*l+2*l+1+n/2)*base->point[3].y;
}

tcubicspline assignbezier_to_point(tcubicspline *base, tpoint *p, double l, double n)
{
  tcubicspline result;

  result.point[0].x = base->point[3].x;
  result.point[1].x = result.point[0].x + l * (base->point[3].x-base->point[2].x);
  result.point[0].y = base->point[3].y;
  result.point[1].y = result.point[0].y + l * (base->point[3].y-base->point[2].y);
  result.point[2].x = l*l*base->point[1].x - (2*l*l+2*l+n/2)*base->point[2].x + (l*l+2*l+1+n/2)*base->point[3].x;
  result.point[2].y = l*l*base->point[1].y - (2*l*l+2*l+n/2)*base->point[2].y + (l*l+2*l+1+n/2)*base->point[3].y;
  result.point[3].x = p->x;
  result.point[3].y = p->y;
  return result;
}

void assignbezier_to_t3dpointgrid(t3dpointgrid base,  t3dpointgrid ne, double l, int orient)
{
  int i;
  int ii[3];

  for( i = 0 ; i <= 3 ; i++)
    if( orient < 2 )
      {  ii[i]=i ; }
    else
      { ii[i]=3-i; }
   for( i = 0 ; i <= 3 ; i ++) {
     if( orient%2 == 0 ) {
       ne[ ii[0] ][i].x = base[ ii[3] ][i].x;
       ne[ ii[1] ][i].x = ne[ ii[0] ][i].x+l*(base[ ii[3] ][i].x-base[ ii[2] ][i].x);
       ne[ ii[0] ][i].y = base[ ii[3] ][i].y;
       ne[ ii[1] ][i].y = ne[ ii[0] ][i].y+l*(base[ ii[3] ][i].y-base[ ii[2] ][i].z);
       ne[ ii[0] ][i].z = base[ ii[3] ][i].z;
       ne[ ii[1] ][i].z = ne[ ii[0] ][i].z+l*(base[ ii[3] ][i].z-base[ ii[2] ][i].z);
     } else {
       ne[i][ii[0]].x = base[i][ii[3]].x;
       ne[i][ii[1]].x = ne[i][ii[0]].x+l*(base[i][ii[3]].x-base[i][ii[2]].x);
       ne[i][ii[0]].y = base[i][ii[3]].y;
       ne[i][ii[1]].y = ne[i][ii[0]].y+l*(base[i][ii[3]].y-base[i][ii[2]].y);
       ne[i][ii[0]].z = base[i][ii[3]].z;
       ne[i][ii[1]].z = ne[i][ii[0]].z+l*(base[i][ii[3]].z-base[i][ii[2]].z);
     }
   }
}


// fergusson interpolation
// p0,p1 - talppontok, p0d,p1d - derivaltak a talppontban
tpoint fergusson(tpoint *p0,tpoint *p1,tpoint *p0d,tpoint *p1d,double u)
{
  tpoint result;
  double f1u,f2u,f3u,f4u;

  f1u = (1-u)*(1-u)*(1+2*u);
  f2u = u*u*(3-2*u);
  f3u = (1-u)*(1-u)*u;
  f4u = -u*u*(1-u);
  result.x = f1u * p0->x + f2u * p1->x + f3u * p0d->x + f4u * p1d->x;
  result.y = f1u * p0->y + f2u * p1->y + f3u * p0d->y + f4u * p1d->y;
  return result;
}

// hermite interpolaccio
// x - x ertekek, f - f(x) fuggv ertekek, fd - f`(x) derivaltak, xs - futovaltozo
double hermite(double x[],double f[],double fd[],double xs)
{
  int i, last_index_x;
  double result,hi,t,ki,li;

  last_index_x = sizeof x / sizeof *x - 1;
  if( (xs<x[0]) || (xs>x[last_index_x]) ) {
    result = NAN;
    exit(0);
  }
  for( i = 0 ; i <= last_index_x ; i ++) {
    if( (xs == x[i]) ) {
      result = x[i];
      exit(0);
    }
    if( (i != last_index_x) && (xs > x[i]) && (xs < x[i+1]) ) {  break; }
  }
  hi = x[i+1]-x[i];
  t = (xs-x[i])/hi;
  ki = -2*(f[i+1]-f[i])/hi;
  li = -ki+(f[i+1]-f[i])/hi-fd[i];
  result = f[i]+(xs-x[i])*(fd[i]+t*(li+t*ki));
  return result;
}

double lengthpolyline(tcubicspline *curve)
{
  double result;
  result = sqrt(sqr(curve->point[1].x-curve->point[0].x) + sqr(curve->point[1].y-curve->point[0].y));
  result = result + sqrt(sqr(curve->point[2].x-curve->point[1].x) + sqr(curve->point[2].y-curve->point[1].y));
  result = result + sqrt(sqr(curve->point[3].x-curve->point[2].x) + sqr(curve->point[3].y-curve->point[2].y));
  return result;
}

tcubicspline beziertohermite(tcubicspline *bezier)
{
  tcubicspline result;
  result.point[0].x = bezier->point[0].x;
  result.point[1].x = bezier->point[3].x;
  result.point[2].x = - 3*bezier->point[0].x + 3*bezier->point[1].x;
  result.point[3].x = - 3*bezier->point[2].x + 3*bezier->point[3].x;
  result.point[0].y = bezier->point[0].y;
  result.point[1].y = bezier->point[3].y;
  result.point[2].y = - 3*bezier->point[0].y + 3*bezier->point[1].y;
  result.point[3].y = - 3*bezier->point[2].y + 3*bezier->point[3].y;
  return result;
}

tcubicspline hermitetobezier(tcubicspline *hermite)
{
  tcubicspline result;

  result.point[0].x = hermite->point[0].x;
  result.point[1].x = hermite->point[0].x + hermite->point[2].x/3;
  result.point[2].x = hermite->point[1].x - hermite->point[3].x/3;
  result.point[3].x = hermite->point[1].x;
  result.point[0].y = hermite->point[0].y;
  result.point[1].y = hermite->point[0].y + hermite->point[2].y/3;
  result.point[2].y = hermite->point[1].y - hermite->point[3].y/3;
  result.point[3].y = hermite->point[1].y;
  return result;
}

// draws bezier curve ino (x,y) point array
void rasterizebezier(tcubicspline *bezier, tpointarray *points, int nonuniformspacing)
{
  tpoint p;
  int i,l;
  double u;

  l = round(lengthpolyline(bezier)/nonuniformspacing);

  assert(points == 0);
  points->length = l+1;
  points = malloc((points->length) * sizeof(tpoint));

  for( i = 0 ; i <= l ; i ++) {
    u = i/l;
    p = bezier_of_tcubicspline(bezier,u);
    points->points[i].x = p.x;
    points->points[i].y = p.y;
  }
}

void rasterizebezier_of_double(tcubicspline *bezier, tpointarray *points, double uniformspacing)
{
  tcubicspline *b1,*b2;

  if( lengthpolyline(bezier) <= uniformspacing ) {
    points = realloc(points,(points->length+1)*sizeof(tpoint));
    points->length +=1;
    points->points[ points->length].x = round(bezier->point[0].x);
    points->points[ points->length].y = round(bezier->point[0].y);
  }else {
    decasteljau(bezier,b1,b2,0.5);
    rasterizebezier(b1,points,uniformspacing);
    rasterizebezier(b2,points,uniformspacing);
  }
}

void rasterize_bspline(tcubicspline *bspline, tpointarray *points, int nonuniformspacing)
{
  tpoint p;
  int i,l;
  double u;

  l = round(lengthpolyline(bspline)/nonuniformspacing);
  assert(points == 0);
  points->length = l+1;
  points = malloc((points->length) * sizeof(tpoint));

  for( i = 0 ; i <= l ; i ++) {
    u = i/l;
    p = bspline_of_tcubicspline(bspline,u);
    points->points[i].x = p.x;
    points->points[i].y=p.y;
   }
}