/***************************************************************************** 
 *locomotion.c                                                               *
 *                                                                           *
 *Tom Stepleton and Stephanie Wojtkowski                                     *
 *                                                                           *
 *This module controls the movement of the robot as it follows the path      *
 *discovered by A*.                                                          *
 ****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <tiffio.h>
#include <time.h>
#include "Nclient.h"
#include "utilities.h"
#include "astar.h"
#include "scoutmap.h"
#include "graphmaster.h"

#define UNLESS(item) if(!( (item) ))
#define STATE_INTO_POINT(item) get_rc(); get_sn(); (item).x = State[STATE_CONF_X]; (item).y = State[STATE_CONF_Y]

#define XLATSPEED 400       //Maximum forward speed
#define TURNSPEED 450       //Maximum turning speed

/* This code is borrowed from our last lab. Information about its inner
   workings is available there, but for our purposes this description should
   suffice: */
//This function laboriously calculates the difference between the robot's 
//heading and the relative heading of the goal.  It returns a value between -1 
//and 1, corresponding to -180 and 180 degrees, respectively.  This 
//correspondence, however, is not important to the robot's navigation, which 
//just uses it as a general indication of which way to turn.  

double HeadingDifference(point goal);
double HeadingDifference(point goal)
{
  static double lastangle = 0;
  double dx, dy;
  double goalangle, headingangle;
  double relativeangle;

  dx = goal.x - State[STATE_CONF_X];
  dy = goal.y - State[STATE_CONF_Y];

  goalangle = atan2(dy, dx)/M_PI;
  if(State[STATE_CONF_STEER] < 1800)
    headingangle = (double)State[STATE_CONF_STEER] / 1800;
  else
    headingangle = -((double)(3600 - State[STATE_CONF_STEER]) / 1800);

  relativeangle = goalangle - headingangle;
  if(relativeangle > 1) relativeangle -= 2;
  else if(relativeangle < -1) relativeangle += 2;

  if(fabs(relativeangle - lastangle) > 1) { puts("boing"); relativeangle = lastangle; }

  lastangle = relativeangle;

  if(fabs(relativeangle > 0.1)) return(relativeangle > 0 ? 1 : -1);
    else return (10 * relativeangle);
}

/*
 * If the robot gets stuck in loop-de-loops, increase PRETTY_CLOSE by
 * a few hundred units
 */
#define PRETTY_CLOSE 700
#define CLOSE_ENOUGH 200
#define DISTANCE_FACTOR ((2*M_PI) / CLOSE_ENOUGH)
/* Every good C program needs GoTo, right? */
/*This function guides the robot to the next node in the path, slowing down
  as it approaches the goal.*/
void GoTo(point p);
void GoTo(point p)
{
  point BotLocation, StartPoint;
  double GoalDistance, DistanceFactor, Distance;
  int ForwardSpeed, TurnSpeed;

  STATE_INTO_POINT(BotLocation);
  StartPoint = BotLocation;

  GoalDistance = GetEuclDist(BotLocation, p);
  if(GoalDistance == 0) return;

  while(GoalDistance > CLOSE_ENOUGH) {
    TurnSpeed = HeadingDifference(p) * TURNSPEED;

    GoalDistance = GetEuclDist(BotLocation, p);
    Distance = GetEuclDist(p, StartPoint);

    if((Distance < PRETTY_CLOSE) || (GoalDistance < PRETTY_CLOSE))
      ForwardSpeed = (1 - 0.7 * cos(Distance * DistanceFactor)) * (XLATSPEED/2);
    else ForwardSpeed = XLATSPEED/2;

    vm(ForwardSpeed, TurnSpeed, TurnSpeed);

    STATE_INTO_POINT(BotLocation);
  }
}