#include "opengl.h"
#include "track.h"


void drawBezier(void)
{
  float i;

  sVertex dots[4];

  sVertex f;
  sVertex derive;
  sVertex derive2;
  sVertex *temp;
  sVertex *normal;

  dots[0].x = -2;
  dots[0].y = -1;
  dots[0].z = -2;

  dots[1].x = 1;
  dots[1].y = 1;
  dots[1].z = 2;

  dots[2].x = 2;
  dots[2].y = 2;
  dots[2].z = 2;

  dots[3].x = 2;
  dots[3].y = 1;
  dots[3].z = 1;

  glPointSize(5.0f);

  /* Starting point */
  glBegin(GL_POINTS);
  glColor3f(0.0f,1.0f,0.0f);
  glVertex3f(dots[0].x, dots[0].y, dots[0].z);
  glEnd();

  /* Ending point */
  glBegin(GL_POINTS);
  glColor3f(0.0f,0.0f,1.0f);
  glVertex3f(dots[3].x, dots[3].y, dots[3].z);
  glEnd();

  
  /* Control point #1 */
  glBegin(GL_POINTS);
  glColor3f(1.0f,0.0f,0.0f);
  glVertex3f(dots[1].x, dots[1].y, dots[1].z);
  glEnd();
  glBegin(GL_LINES);
  glColor3f(0.5f,0.5f,0.5f);
  glVertex3f(dots[0].x, dots[0].y, dots[0].z);
  glVertex3f(dots[1].x, dots[1].y, dots[1].z);
  glEnd();

  /* Control point #2 */
  glBegin(GL_POINTS);
  glColor3f(1.0f,0.0f,0.0f);
  glVertex3f(dots[2].x, dots[2].y, dots[2].z);
  glEnd();
  glBegin(GL_LINES);
  glColor3f(0.5f,0.5f,0.5f);
  glVertex3f(dots[3].x, dots[3].y, dots[3].z);
  glVertex3f(dots[2].x, dots[2].y, dots[2].z);
  glEnd();




  glColor3f(0.0,0.0,0.0);
  glBegin(GL_LINE_STRIP);
  for(i=0;i<1.0f;i+=0.01)
    {
      glVertex3f( bezier(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i), 
		  bezier(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i), 
		  bezier(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i));

    }
  glVertex3f(dots[3].x, dots[3].y, dots[3].z);
  glEnd();

#define COEF 1.0f




  glColor3f(0.5,0.5,1.0);
  for(i=0;i<1.0f;i+=0.01)
    {
f.x = bezier(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i);
f.y = bezier(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i);
f.z = bezier(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i);

  derive.x = bezier_derive(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i);
  derive.y = bezier_derive(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i);
  derive.z = bezier_derive(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i);

  derive2.x = bezier_2nd_derive(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i);
  derive2.y = bezier_2nd_derive(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i);
  derive2.z = bezier_2nd_derive(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i);
  
  temp = cross_product(derive.x, derive.y, derive.z,
			 derive2.x, derive2.y, derive2.z);
  normal = cross_product(temp->x, temp->y, temp->z,
			 derive.x, derive.y, derive.z);
normalise(temp, 0.5);
normalise(normal, 0.5);



  glBegin(GL_LINE_STRIP);
      //glVertex3f(f.x,f.y,f.z); 
      glVertex3f(f.x+normal->x, f.y+normal->y, f.z+normal->z);
      glVertex3f(f.x+temp->x, f.y+temp->y, f.z+temp->z);
      glVertex3f(f.x-normal->x, f.y-normal->y, f.z-normal->z);
      glVertex3f(f.x-temp->x, f.y-temp->y, f.z-temp->z);
      glVertex3f(f.x+normal->x, f.y+normal->y, f.z+normal->z);
  glEnd();

      /* glVertex3f( bezier(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i) 
		  + COEF * bezier_derive(dots[0].x,dots[1].x,dots[2].x,dots[3].x,i), 
		  bezier(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i) 
		  + COEF * bezier_derive(dots[0].y,dots[1].y,dots[2].y,dots[3].y,i), 
		  bezier(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i) 
		  + COEF * bezier_derive(dots[0].z,dots[1].z,dots[2].z,dots[3].z,i));
      */

      // printf("%f %f %f\n", normal->x, normal->y, normal->z);
    }
}


float bezier(float x0, float x1, float x2, float x3, float t)
{
    float a = 0.0f;
    float b = 0.0f;
    float c = 0.0f;
    float t2 = 0.0f;
    float t3 = 0.0f;

    t2 = t*t;
    t3 = t2*t;

    c = 3.0f*(x1-x0);
    b = 3.0f*(x2-x1)-c;
    a = x3-x0-c-b;

    return a*t3+b*t2+c*t+x0;
}

float bezier_derive(float x0, float x1, float x2, float x3, float t)
{
    float a = 0.0f;
    float b = 0.0f;
    float c = 0.0f;
    float t2 = 0.0f;
    float t3 = 0.0f;

    t2 = t*t;
    t3 = t2*t;

    c = 3.0f*(x1-x0);
    b = 3.0f*(x2-x1)-c;
    a = x3-x0-c-b;

    return (3*a)*t2+(2*b)*t+c;
}
float bezier_2nd_derive(float x0, float x1, float x2, float x3, float t)
{
    float a = 0.0f;
    float b = 0.0f;
    float c = 0.0f;
    float t2 = 0.0f;
    float t3 = 0.0f;

    t2 = t*t;
    t3 = t2*t;

    c = 3.0f*(x1-x0);
    b = 3.0f*(x2-x1)-c;
    a = x3-x0-c-b;

    return 6*a*t + 2*b;
}


 

sVertex *cross_product(float x1, float y1, float z1,
		       float x2, float y2, float z2)
{
  sVertex *w;
  
  w = malloc(sizeof(sVertex));
  w->x = y1*z2 - z1*y2;
  w->y = z1*x2 - x1*z2;
  w->z = x1*y2 - y1*x2;
  
  return w;
}

void normalise(sVertex *v, float n)
{
    float m = sqrt((v->x)*(v->x) + (v->y)*(v->y) + (v->z)*(v->z));
    if(!m) return;
    n = m / n;
    v->x /= n; v->y /= n; v->z /= n;
}