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

/*
 NOTE: This code is identical to locomotion2.c except with the addition of
       obstacle avoidance capabilities. This is necessary for the evidence
       grid planner, as it sometimes leads the robot on paths that cross
       obstacles. If the robot stays away from these obstacles long enough,
       it will detect them and D* will avoid them.
*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <tiffio.h>
#include <time.h>
#include "Nclient.h"
#include "utilities.h"
#include "dstar.h"
#include "scoutmap.h"
#include "graphmaster3.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
#define SONAR_RADIUS 200

extern point Robotpoint;

/* 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);
}

#define CLOSE_ENOUGH 200

double LeftSideSonarWeights[]  = {1,.8,.6,.4,.2,0,0,0,0,0,0,0,0,0,0,0};
double RightSideSonarWeights[] = {1,0,0,0,0,0,0,0,0,0,0,0,.2,.4,.6,.8};

#define IR_THRESHOLD 20
double SensorImperative(double *sonarray)
{
  double Importance = 0;
  int i;

  for(i=0;i<16;i++) {
    if(State[i+STATE_SONAR_0] < IR_THRESHOLD)
      if(sonarray[i] > Importance) Importance = sonarray[i];
  }
  return(Importance);
}
  
/*This function guides the robot to the farthest node in its path that 
  is still less than a sonar distance away.  This allows the robot to 
  follow a smoother path while ensuring that the robot will not run into 
  any obstacles along the way.*/
Node *GoTo(Node *pa);
Node *GoTo(Node *pa)
{
  double TurnFactor, GoalDistance;
  double RightImperative, LeftImperative;
  point BotLocation;
  int ForwardSpeed, TurnSpeed, i = 0;
  int SONAR_ORDER[] = {14, 15, 0, 1, 2};
  point p, q;
  
  q = *((point *) pa->nodeInfo);
  p = *((point *) ((Node *)pa->parent)->nodeInfo);
  pa = pa->parent;
  UNLESS(pa->parent) return NULL;
  while(GetEuclDist( *((point *) ((Node *)pa->parent)->nodeInfo), q) < 
	SONAR_RADIUS){
    printf("i=%d\n", i);
    i++;
    p =  *((point *) ((Node *)pa->parent)->nodeInfo);
    pa = pa->parent;
    if(!(pa = (Node*) pa->parent))break;
  }
  
  printf("Goal is (%f, %f)\n", p.x, p.y);

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

  for(i=0; i<5; i++) {
    printf("In loop\n");
    gs();
    get_rc();

    if(GoalDistance < CLOSE_ENOUGH) break;

    if(((RightImperative = SensorImperative(LeftSideSonarWeights)) > 0) ||
       ((LeftImperative = SensorImperative(RightSideSonarWeights)) > 0))
      TurnFactor = RightImperative > LeftImperative ? - RightImperative :
							LeftImperative ;
      else TurnFactor = HeadingDifference(p);

    if(TurnFactor>.20 || TurnFactor<-.20){
      TurnSpeed = TurnFactor * TURNSPEED;
      GoalDistance = GetEuclDist(BotLocation, p);
      vm(100, TurnSpeed, TurnSpeed);
    }
    else{
      TurnSpeed = TurnFactor * TURNSPEED;
      GoalDistance = GetEuclDist(BotLocation, p);
      ForwardSpeed = XLATSPEED * (1 - TurnFactor * 0.98);
      
      vm(ForwardSpeed, TurnSpeed, TurnSpeed);
    }
    STATE_INTO_POINT(BotLocation);
  }
  return pa;
}

Node *Locomote(Node *path);
Node *Locomote(Node *path)
{
  path = GoTo(path);

  Robotpoint.x = State[STATE_CONF_X];
  Robotpoint.y = State[STATE_CONF_Y];

  return(path);
}