/***************************************************************************
 *   Lab 2: Navigation Using Behavior-Based and Path-Planning Strategies   *
 *                  Stephanie Wojtkowski and Tom Stepleton                 *
 *                                                                         *
 *This is the main program that controls the D* search of an unknown       *
 *environment. The robot plans a path given its current information, then  *
 *traverses the path for a short distance and checks to see if it needs to *
 *replan.  The cost for each path is determined by an evidence grid.       *
 ***************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "sonar_head.h"
#include "Nclient.h"
#include "utilities.h"
#include "dstar.h"
#include "graphmaster3.h"
#include "makepic.h"

#define UNLESS(item) if(!( (item) ))

/* grid information */
Node worldGrid[GRIDX][GRIDY];
NodeInfo worldGridInfo[GRIDX][GRIDY];
char worldImage[GRIDY + 1][GRIDX + 1];
/* and the new Evidence Grid */
EvGrid *worldEv;

/* personal information */
char *progname;

/* where's the goal? */
point Goalpoint;
/* where's the robot? */
point Robotpoint;

void InitBot(int ResetAll)
{
  connect_robot(1, MODEL_SCOUT2, "/dev/ttyS0", 38400);
  turn_sonar_on();
  conf_tm(2);

  if(ResetAll) { zr(); dp(0, 0); da(0, 0); }

  get_rc();
  get_sn();

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

//Jordan's revamped rectify_angle
double jordans_rectify_angle (double input_angle) {

  while (input_angle > 360.0)
    input_angle -= 360.0;

  while (input_angle < -360.0)
    input_angle += 360.0;

  while (fabs (input_angle) > 180.0)
    {
      if (input_angle > 0.0)
        {
          input_angle -= 360.0;
        }
      else
        {
          input_angle += 360.0;
        }
    }
  return (input_angle);
}

//Inserts all sonar data into the evidence grid
void SonarScan(void)
{
  int i;
  double range, angle, sx, sy;

  for(i=0;i<16;i++) {
    if(State[STATE_SONAR_0 + i] == 255) continue;

    range = State[STATE_SONAR_0 + i];
    angle = i*22.5 + State[STATE_CONF_STEER]/10;
    angle = jordans_rectify_angle(angle);
    angle *= M_PI / 180.0;
    sx = State[STATE_CONF_X];
    sy = State[STATE_CONF_Y];
    sx /= 10.0;
    sy /= 10.0;

    InsertSonarReading(worldEv, sx, sy, angle, range);
  }
}

//Constructs a list of the first few nodes in the robot's path to create a
//list of initial nodes for D*. This is a cheesy but effective means of
//replanning our path when an obstacle turns up in front of us.
#define FAR_ENOUGH 1000
Node **firstFewPathPts(Node *path, int *numInitial)
{
  int i;
  Node *p, **list = NULL;

  *numInitial = 0;
  UNLESS(p = path) return NULL;

  //for(i=0;i<4;i++) UNLESS(p = (Node *) p->parent) break;

  while(GetEuclDist(*((NodeInfo *) p->nodeInfo), Robotpoint) < FAR_ENOUGH) {
    UNLESS(list = realloc(list, ++*numInitial * sizeof(Node *))) {
      fputs("Can't alloc memory for a node list\n", stderr);
      exit(-1);
    }

    list[*numInitial - 1] = p;
    UNLESS(p = (Node *) p->parent) break;
  }

  return list;
}

void printNodeList(Node * list, char *name, void (*printfunc) (Node *));
Node *Locomote(Node *path);

/*

Arguments to this program:
 -reset: places the robot at 0,0
 -movie: generates a 'movie' of the robot's progress through the map -
         BEWARE: many, many TIFF files!!!

 */
#define STRLEN 256
int main(int argc, char **argv)
{
  Node **initial, *path;
  int numInitial, movie;
  double costR[2];
  long i=0;
  char fn[20];

  /*################# PROGRAM INIT #################*/

  progname = *argv;

  InitBot(argc > 1 ? !strcmp(argv[1], "-reset") : 0);
  movie = argc > 1 ? !strcmp(argv[1], "-movie") : 0;

  /*############### WHERE'S THE GOAL ###############*/

  fputs("Please enter the X coordinate of the goal\n> ", stdout);
  fscanf(stdin, "%lf", &Goalpoint.x);
  fputs("Now enter the Y coordinate of the goal\n> ", stdout);
  fscanf(stdin, "%lf", &Goalpoint.y);
  fputs("Excellent...\n", stdout);

  /*############## THE INITIAL SEARCH ##############*/

  initGraph();
  worldEv = CreateMap(GRIDX, GRIDY,
		      -GRIDX*GRIDSIZE/20, -GRIDY*GRIDSIZE/20, GRIDSIZE/10);

  /* Create initial initial list */
  initial = malloc(sizeof(Node *));
  *initial = fetchGoalNode();
  numInitial = 1;

  costR[0] = costR[1] = HUGE_VAL;

  path = DStarSearch(initial, numInitial, gfunction, hfunction, robot,
		     getNeighbors, cost, costR, printNode, drawArrow);
  plotEvidence();
  plotPath(path);
  drawGrid();

  /*################# LET'S DRIVE ##################*/

  for(;;) {
    path = Locomote(path);  /* _must_ update robotpoint!!! */
    SonarScan();
    //if(SomethingAhead()) {
    if(i++ % 2 == 0) {
      Node *newPath;
      point_int botloc;

      free(initial);
      initial = firstFewPathPts(path, &numInitial);
      st();

      botloc = graph2grid(Robotpoint);

      costR[0] = worldGrid[botloc.x][botloc.y].f;
      costR[1] = worldGrid[botloc.x][botloc.y].g;

      UNLESS(path) break;
      //if(!path || atGoal()) break;

      newPath = DStarSearch(initial, numInitial, gfunction, hfunction, robot,
			    getNeighbors, cost, costR, printNode, drawArrow);

      path = whichPath(path, newPath);

      plotEvidence();
      plotPath(path);
      drawGrid();

    }
    if(movie && (i % 2 == 0)) {
      sprintf(fn, "File%ld.tif", i/2);
      WritePic(fn, worldEv, path);
    }
  }

  /*############### WE'VE MADE IT ##################*/

  puts("We're there!");

  st();
  ws(1,1,0,5);

  // Disconnect from the robot
  disconnect_robot(1);

  plotEvidence();
  drawGrid();

  return 0;
}