/************************************************************************************
libsj
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/
#include <stdio.h>
#include <math.h>
#include "curve.h"



double createcurveA(double spx, double spy, double svx, double svy,
		   double epx, double epy, double evx, double evy,
		   double dt)
{
  double length = 0.0;

  for (double curt = 0.0; curt <= 1.0; curt += dt) {
    double x0 = curve_fergusoncoons(spx, svx, epx, evx, curt);
    double y0 = curve_fergusoncoons(spy, svy, epy, evy, curt);
    double x1 = curve_fergusoncoons(spx, svx, epx, evx, curt+dt);
    double y1 = curve_fergusoncoons(spy, svy, epy, evy, curt+dt);
    length += sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
    printf("%f %f\n", x0, y0);
  }
  return length;
}

double createcurveB(double spx, double spy, double svx, double svy,
		   double epx, double epy, double evx, double evy,
		   double dt)
{
  double length = 0.0;

  for (double curt = 0.0; curt < 1.0; curt += dt) {
    double x0 = curve_fergusoncoons(spx, svx, epx, evx, curt);
    double y0 = curve_fergusoncoons(spy, svy, epy, evy, curt);
    double x1 = curve_fergusoncoons(spx, svx, epx, evx, curt+dt);
    double y1 = curve_fergusoncoons(spy, svy, epy, evy, curt+dt);
    length += sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
  }

  double lenconst = length * dt;

  double addt = dt;
  for (double curt = 0.0; curt < 1.0; curt += addt) {
    double x0 = curve_fergusoncoons(spx, svx, epx, evx, curt);
    double y0 = curve_fergusoncoons(spy, svy, epy, evy, curt);
    double x1 = curve_fergusoncoons(spx, svx, epx, evx, curt+dt);
    double y1 = curve_fergusoncoons(spy, svy, epy, evy, curt+dt);
    length = sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));

    double k = length / lenconst;
    addt = dt / k;

    printf("%f %f\n", x0, y0);
  }
  printf("%f %f\n", epx, epy);

  return length;
}



int main(int argc, char **argv)
{  
#if 1
  //coord x y vel vx, vy
  struct curvepoint 
  {
    double x, y;    //point 
    double vx, vy;  //vector
  };
  curvepoint points[] = {
    { -1.0,  -1.0, 0.0, 20.0 }, //start point&vector 
    {  1.0,  1.0, 0.0, 20.0 },  //end   point&vector
  };


  //-------------------------------
  // NORMAL CURVE
  //-------------------------------
  
  double dt = 0.01;
  for (double t = 0.0; t < 1.0; t += dt) {
    double x = curve_fergusoncoons(points[0].x, points[0].vx, points[1].x, points[1].vx, t);
    double y = curve_fergusoncoons(points[0].y, points[0].vy, points[1].y, points[1].vy, t);
    printf("%f %f\n", x, y);
  }

  printf("\n\n");



  //--------------------------------
  // 
  //--------------------------------
  double length = 0.0;
  dt = 0.01;
  for (double curt = 0.0; curt < 1.0; curt += dt) {
    double x0 = curve_fergusoncoons(points[0].x, points[0].vx, points[1].x, points[1].vx, curt);
    double y0 = curve_fergusoncoons(points[0].y, points[0].vy, points[1].y, points[1].vy, curt);
    double x1 = curve_fergusoncoons(points[0].x, points[0].vx, points[1].x, points[1].vx, curt+dt);
    double y1 = curve_fergusoncoons(points[0].y, points[0].vy, points[1].y, points[1].vy, curt+dt);
    length += sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
  }
  
  dt = 0.01;

  double vel = 1.0f;
  bool constvel = true;
  if (!constvel) {
    double time = 1.0;
    vel = length / time;
  }
  double lenconst = length * dt;

  double addt = dt;
  for (double curt = 0.0; curt <= 1.0; curt += addt) {

    double x0 = curve_fergusoncoons(points[0].x, points[0].vx, points[1].x, points[1].vx, curt);
    double y0 = curve_fergusoncoons(points[0].y, points[0].vy, points[1].y, points[1].vy, curt);
    double x1 = curve_fergusoncoons(points[0].x, points[0].vx, points[1].x, points[1].vx, curt+dt);
    double y1 = curve_fergusoncoons(points[0].y, points[0].vy, points[1].y, points[1].vy, curt+dt);
    length = sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));

    double k = length / lenconst;
    addt = dt / k;

    printf("%f %f %f %f  k=%f/%f=%f addt=%f/%f=%f\n", x0, y0, x1, y1, length,lenconst,k, dt,k,addt);
  }

#else
  struct curvepoint 
  {
    double x, y;
  };
  curvepoint points[4] = {
    { 0.0, 0.0 }, 
    { 10.0, 10.0 }, 
    { 20.0, -10.0 }, 
    { 0.0, 0.0 }, 
  };
  for (double t = 0.0; t < 1.0; t += 0.01) {
    double x = curve_cutmullrom(points[0].x, points[1].x, points[2].x, points[3].x, t);
    double y = curve_cutmullrom(points[0].y, points[1].y, points[2].y, points[3].y, t);
    printf("%f %f\n", x, y);
  }
#endif
  return 0;
}
