/*
 *wallfollow.cc
 *by Patrick Vinograd and Andrew McDonnell
 *CS 154, Project A
 *Due February 27, 2001
 *
 *Wall-following code for the Nomad simulator.
 */

#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <sys/ddi.h>
#include <stream.h>
#include <fstream.h>
#include "KbdInput.hh"

extern "C" {
#include "Nclient.h"
}

//debugging output
ofstream outf("output");


/*
 *Find the smallest of the three front sensors, and pay attention to it.
 */
int leastFront() {
  return min(min(State[17], State[18]), State[32]);
}

/*
 *Find the smallest of the three left sensors, plus the one to the rear of 
 *those, since we're doing left turns all the time. 
 */
int leastLeft() {
  return min(min(min(State[23], State[21]), 
		 State[20]), State[22]);
}

/*
 *Find the smallest of the three right sensors. 
 */
int leastRight() {
  return min(min(State[29], State[30]), State[28]);
}

/*
 *Find the smallest of the five rear sensors.  Don't think I'll use this.
 */
int leastRear() {
  int temp1 = min(State[23], State[24]);
  int temp2 = min(State[27], State[26]);
  int temp3 = min(temp1, temp2);
  return min(temp3, State[25]);
}


//some useful constants
int THRESHOLD = 11;
int DANGER_THRESHOLD = 10;
int CRUISE_SPEED = 300;

/*
 *Indicate whether we have a wall in our perimeter.
 */
bool noWall() {
  return ((leastLeft() > THRESHOLD)  && 
	  (leastRight() > THRESHOLD) && 
	  (leastFront() > THRESHOLD));
}

/*
 *Check for impending doom.
 */
bool frontDanger() {
  return (leastFront() < DANGER_THRESHOLD);
}

/*
 *Are the first two parameters within the third parameter of one another?
 */
bool proxim(int real, int target, int thresh) {
  if (target == 3600) {
    return ((abs(target - real) < thresh) || real < thresh);
  }
  if (target == 0) {
    return ((abs(target - real) < thresh) || real > (3600-thresh));
  }
  return (abs(target - real) < thresh);
}

/*
 *Perform the actual calibration referred to below.  Kludgy. 
 */
void fineTune(int target) {
  int delta = (target - State[36]);
  if (target = 3600) {
    if (State[36] < 20)
      delta = -1 * State[36];
  }
  pr(0, delta, delta);
  for (int i = 0; i < 10; i++)
    gs();
}

/*
 *Since we know we always want to be at right angles, align to the nearest
 *right angle after a turn.  
 */
void recalibrate() {
  if (proxim(State[36], 3600, 10)) {
    fineTune(3600);
  }
  else if (proxim(State[36], 900, 10)) {
    fineTune(900);
  }
  else if (proxim(State[36], 1800, 10)) {
    fineTune(1800);
  }
  else if (proxim(State[36], 2700, 10)) {
    fineTune(2700);
  }  
}

/*
 *Turn the number of degrees specified.  Do some checking around angle zero.
 *Modular arithmetic hurts my head.  3600 is the same as 0?!  What the hell
 *is that?!
 */
void turnRel(int degrees) {
  gs();
  int cheading = State[36];
  pr(0, 10*degrees, 10*degrees);
  int target = (cheading + (10 * degrees) + 3600) % 3600;
  if (target == 0) {
    if (degrees > 0) {
      while (State[36] < 3600 && State[36] > 0)
	gs();
    }
    else {
      while (State[36] > target)
	gs();
    }
  }
  else {
    if (degrees > 0) {
      while (State[36] < target)
	gs();
    }
    else {
      while (State[36] > target)
	gs();
    }
  }
  recalibrate();
}

/*
 *We don't have a wall around, so turn left, go forward until there's 
 *something in our perimeter, and then get it on our left side.  
 */
void acquireWall() {
  turnRel(90);
  while (noWall()) {
    vm(CRUISE_SPEED, 0, 0);
  }
  if (leastRight() < THRESHOLD) {
    vm(0, 0, 0);
    turnRel(-180);
  }
  else if (leastFront() < THRESHOLD) {
    vm(0, 0, 0);
    turnRel(-90);
  }
  vm(CRUISE_SPEED, 0, 0);
  return;
}


/****************************
 *
 *  The usual main method
 *
 ****************************/

int
main(int argc, char** argv)
{

  SERV_TCP_PORT = 20039;    

  // connect to the simulator
  if (connect_robot(1)) 
	printf("Connected to robot\n");
  else {
	printf("Failed to connect to robot\n");
	exit(0);
  }

  /* 
   * zero the robot
   */

  zr();

  //activate infrared and sonar sensors
  for (int i = 1; i < 33; i++) Smask[i] = 1;
  //make the position and heading information available
  Smask[36] = Smask[34] = Smask[35] = 1;
  
  ct();
  gs();                     /* update the robot's state   */

  KbdInput KI; 
  char c; 
  KI.on();                  /* turn on raw input/output   */
  
  if (noWall()) {
    acquireWall();
  }
  
  while (1) {
    c = KI.nextChar();     
    if ( c == 'q' )    break;
    
    if (noWall()) {
      acquireWall();      
    }
    //If we're tracking a wall, go forward
    while (!noWall()) {
      vm(CRUISE_SPEED, 0, 0);
      //If we lose the wall, find one
      if (noWall()) {
	acquireWall();      
      }
      //If we're heading for something, stop, turn, and proceed
      if (frontDanger()) {
	vm(0, 0, 0);
	if (leastRight() < 20)
	  turnRel(-180);
	else
	  turnRel(-90);
	vm(CRUISE_SPEED, 0, 0);
	continue;
      }
      
    }
    
  }

  KI.off();                 /* reset terminal settings              */
  disconnect_robot(1);      /* gracefully disconnect from the robot */

}