/***********************************************************************/
/** @name spline.c 
    Contains functions calculate a spline to a certain posint with a
    certain heading and have a robot drive to it.
        
    @author Birgit Graf, UWA, 1998
*/
/***********************************************************************/

/*@{*/


#include "global.h"
#include "general.h"

#define DEBUG 0

/** factors to change linear/rotational speed */
#define lin_fact 1.5
#define rot_fact 3.0

/** time intervall after which spline function is called again (period
    in 1/100 seconds) */
#define loop_time 15

/** accuracy parameter for robot movement (in m), angle < epsilon ->
    don't move */
#define epsilon 0.01

/** threshold for robot movement (in m), stop if dist < thres_stop */
#define thresh_stop 0.05


/** record to store robot's start and end position when driving
    on a spline */
PositionType startpos, endpos;

/** desired slope at start/end point (x and y value) */
float startpos_d_x, endpos_d_x, startpos_d_y, endpos_d_y; 

/** u in spline formula */
float u;
/** factor to increase fraction of spline in each step */
float fract_step;
/** flag set to true when driving spline */
int spline_flag = FALSE;

/** spline timer */
TimerHandle splinetimer;

meterPerSec lin_old;


/***********************************************************************/
/** Get angle.
    Get angle between robot orientation phi and line to point
    (diffx, diffy).

    Called by drive_spline().
    
    @see drive_spline()
    @param diffx, diffxy difference between coordinates of point to
    current robot position
    @param phi robot heading
    @return diffphi angle between robot heading and point (x, y)
*/
/***********************************************************************/

float get_angle(float diffx, float diffy, float phi)
{
  float diffphi;

  /* angle to desired position */
  if (fabs(diffx) < epsilon && fabs(diffy) < epsilon)
    diffphi = phi;		/* point reached -> no angle difference */
  else
    diffphi = atan2(diffy, diffx);

  /* difference to current heading */
  diffphi -= phi;
  
  /* angle always between 0 and 2*PI */
  if (diffphi >= 2.0 * PI)
    diffphi -= 2.0 * PI;
  if (diffphi <= 0)
    diffphi += 2.0 * PI;
  
  /* angle always between -PI and PI */
  if (diffphi >=  PI)
    diffphi -= 2.0 * PI;
  
/*   if (fabs(diffphi) > PI/2)  */
/*     AUTone(1200,200); */
  return diffphi;
}


/* compute hermite spline */
void herm_spline(float u, PositionType *spline)
{
  float u2, u3;
  float blend0, blend1, blend2, blend3; /* blending fcts for point on spline */

  if (u > 1.0) u = 1.0;
  u2 = u * u;
  u3 = u2 * u;

  blend0 = 2.0 * u3 - 3.0 * u2 + 1.0;
  blend1 = -2.0 * u3 + 3.0 * u2;
  blend2 = u3 - 2.0 * u2 + u;
  blend3 = u3 - u2;

  /* desired position on spline */
  spline->x = startpos.x * blend0 + endpos.x * blend1 +
              startpos_d_x * blend2 + endpos_d_x * blend3;
  spline->y = startpos.y * blend0 + endpos.y * blend1 +
              startpos_d_y * blend2 + endpos_d_y * blend3;
}




/***********************************************************************/
/** Drive Spline IRQ.
    Drive curve from startpos to endpos, considering orientation 

    Called by start_spline(). Executed every loop_time *10 mseconds.

    @see start_spline()
*/
/***********************************************************************/

void drive_spline_IRQ()
{
  radPerSec rot_new;		/* new rotatonal speed */
  meterPerSec lin_new;		/* new linear speed */
  PositionType pos;
  PositionType diff;		/* difference between desired and
				   current position of robot */
  float diff_dist, min_dist;	/* distance between current and desired
				   position */
  float diff_ball;		/* angle between current and ball position */
  PositionType spline;          /* position on spline */


  if (spline_flag)
  {
    if (u == 1.0)
    {
      spline_flag = FALSE;
/*       VWSetSpeed(vw, 0.0, 0.0);  */
    }
    else
    {

      VWGetPosition(vw, &pos);
      diff_dist = 1.0;  /* 1 meter - should never be reached */
      do {
        min_dist = diff_dist;
        u += fract_step;    
        herm_spline(u, &spline);
        /* difference between current and spline position */
        diff.x = spline.x - pos.x;
        diff.y = spline.y - pos.y;
        diff_dist = fabs(diff.x) + fabs(diff.y);
      } while ((diff_dist < min_dist) && (u < 1.0));
      if (u > 1.0)
	u = 1.0;  /* last step */

      if (DEBUG)
      {
	LCDSetPos(1,0); LCDPrintf("u: % 1.2f\n",u);
	LCDSetPos(2,0); LCDPrintf("       x     y\n");
	LCDSetPos(3,0); LCDPrintf("cur % 1.2f % 1.2f\n",pos.x,pos.y);
	LCDSetPos(4,0); LCDPrintf("des % 1.2f % 1.2f\n",spline.x,spline.y);
	LCDSetPos(5,0); LCDPrintf("dif % 1.2f % 1.2f\n",diff.x,diff.y);
      }
 
      /* angle between current and desired position */
      diff.phi = get_angle(diff.x, diff.y, pos.phi);
      /* angle in range [-PI/2 .. +PI/2] */
      rot_new = rot_fact * diff.phi;

      /* distance between current and desired position */
      diff_dist = fabs(diff.x) + fabs(diff.y);
      lin_new = (u + 0.5) * lin_fact * diff_dist;
      
/*       rot_new = 0.1 * fsign(diff.phi) * diff_dist * rot_fact; */

      VWSetSpeed(vw, lin_new, rot_new);
    }
  }
}


/***********************************************************************/
/** Start Spline.
    Start timer function which calls drive_spline every loop_time
    mseconds.
    
    Called by drive_to_ball().

    @see drive_to_ball()
*/
/***********************************************************************/

void start_spline(PositionType start, PositionType end)
{
  float fract_distx, fract_disty, fract_dist, fract_angle;

  startpos = start;
  endpos = end;

  /* factor to increase fraction of spline in each step */
  fract_distx = endpos.x - startpos.x;
  fract_disty = endpos.y - startpos.y;
  fract_dist  = fabs(fract_distx) + fabs(fract_disty);
  fract_angle = fabs(get_angle(fract_distx, fract_disty, startpos.phi));
  fract_step  = 0.005 * LIN_SPEED * (float)loop_time;
  
  /* desired slope at start/end point (x and y value) */
  startpos_d_x = cos(startpos.phi);
  endpos_d_x = cos(endpos.phi);
  startpos_d_y = sin(startpos.phi);
  endpos_d_y = sin(endpos.phi);
  
  u = 0.0;          /* start with next point */
  lin_old = 0.0;
  spline_flag = TRUE;
}



/***********************************************************************/
/** Stop Spline.
/***********************************************************************/

void stop_spline()
{
  spline_flag = FALSE;
/*   VWSetSpeed(vw, 0.0, 0.0); */
/*   OSDetachTimer(splinetimer); */
}



/***********************************************************************/
/** Spline Done.
/***********************************************************************/

int spline_done()
{
  return (u >= 1.0);
}



/***********************************************************************/
/** Attach drive_spline_IRQ to timer.
/***********************************************************************/

void start_spline_IRQ()
{
  spline_flag = FALSE;
  splinetimer = OSAttachTimer(loop_time, drive_spline_IRQ);
}

/***********************************************************************/
/** Detach drive_spline_IRQ to timer.
/***********************************************************************/

void stop_spline_IRQ()
{
  OSDetachTimer(splinetimer);
}

/*@}*/