#include "rover.h"

//keep this with 2 dots - it's how the java code recognizes the new versions
const char VERSION [] = "2.0.1";

Reliagram * rgram;
WebCam * webCam;

//mutexes for the legs, pan, and tilt.  This makes sure that two threads won't
//clobber each other by using the same resource simulatneously.
bool legsLocked = false, panLocked=false, tiltLocked=false;
//this mutex must be locked before any thread changes the state of one of 
//the previous 3 variables.  It is OK to not have the mutex for reading.
pthread_mutex_t resourceLockMutex = PTHREAD_MUTEX_INITIALIZER;

bool exiting = false, showDebug=false, doDutyTest = false, autoStart = false; 
bool doDutyTest2 = false, doDutyTest3 = false;
// Global Variables for Single Thread

void changeError(char *msg) {
  FILE *error = fopen("/root/error.txt", "w");
  fprintf(error, msg);
  fclose(error);
}

int main(int argc, char **argv) {
  Datapack * pack;
  parseArgs(argc, argv);
  if(autoStart) //If started automatically, sleep to prevent race condition
    sleep(3);

  changeError("starting up\r\n");
  printf("Loading calibration... "); fflush(stdout);
  loadCalibration();    //load in the calibration constants.
  loadScans();

  changeError("starting camera\r\n");
  printf("camera... "); fflush(stdout);
  webCam = new WebCam();

  changeError("starting cereb\r\n");
  printf("cerebellum "); fflush(stdout);
  initCerebellum();

  changeError("starting network\r\n");
  printf("network..."); fflush(stdout);
  rgram = new Reliagram(10000);

  if(!rgram->bindTo(1701)) {
    changeError("can't bind\n");
    printf("Can't bind to port 1701\n");
    return -1;
  }

  signal(SIGINT, sigIntHandler);

  if(doDutyTest) 
    runDutyTest();
  else if(doDutyTest2) 
    runDutyTest2();
  else if(doDutyTest3) 
    runDutyTest3();

  headMove(true, 0, true, 30);
  usleep(1000*timeToArrive(LOOK_MASK));
  headMove(true, 0, true, -30);
  usleep(1000*timeToArrive(LOOK_MASK));
  headMove(true, 0, true, 0);

  runDiagnostic();

  printf("\nInitialization complete\n"); fflush(stdout);
  changeError("all done\r\n");


  //int lastSeqNum = 0;
  while (!exiting) {
    if((pack = rgram->receiveData())) 
      parsePacket(pack);
    else
      headMove(false, 0, false, 0);
  } // end while (true)  
  return 0;
}

void parsePacket(Datapack * pack) {
  char *buf = pack->getData();
  int len = pack->getLength();

  //check for a low battery.  If so, then reject most commands.
  if(voltageBuffer->getVoltage() < LOW_BATTERY_THRESHOLD) {
    headMove(true, 0, true, 90);
    //char type = *buf;
    reply(BATTERY_LOW, pack);
    //allow only spin, turn, and crab commands when voltage is low
    //if(type != 'p' && type !='c' && type != 'q') {
    delete pack;
    return;
      //}
  }

  switch(*buf) {
  case 'i':
    len == 1 ? reply(initRobot(), pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'v': //get version
    len == 1 ? rgram->sendResponse((char *)VERSION, strlen(VERSION), pack) :
      reply(INVALID_LENGTH, pack);
    break;
  case 'g': // 'g' goto
    if(len == 12){ 
      addCerebRequest(pack); 
      return; 
    }else
      reply(INVALID_LENGTH, pack);
    break;
  case 't': // turnto
    if(len == 8){
      addCerebRequest(pack);
      return;
    }else
      reply(INVALID_LENGTH, pack);
    break;
  case 'k': // kill
    if(len == 1) {
      kill(KILLED);
      reply(highLevelStatus, pack);
    }else
      reply(INVALID_LENGTH, pack);
    break;
  case 'u': // get an update on the high level motion command
    len == 1 ? 
      reply(cerebThreadState==CEREB_IDLE?highLevelStatus:HL_CONTINUE, pack) 
      : reply(INVALID_LENGTH, pack);
    break;
  case 'l': 
    len == 40 ? RoverSetAll(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'h':
    len == 12 ? roverHeadMove(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'p':
    len == 8 ? RoverSpin(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'c':
    len == 12 ? RoverCrab(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'q':
    len == 12 ? RoverTurn(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'a': //take picture 
    if(len == 20) { 
      webCam->takePicture(pack);
      return;//return so that the packet is not deleted
    }else
      reply(INVALID_LENGTH, pack);
  case 'n': //scan for rock
    if(len == 20) {
      addCerebRequest(pack);
      return;
    }
    reply(INVALID_LENGTH, pack);
    break;
  case 'y':
    reply(setCal(pack->getData() + 1, pack->getLength() - 1), pack);
    break;
  case 'z':
    len == 1 ? sendCal(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 11:
    reply(setScan(pack->getData() + 1, pack->getLength() - 1), pack);
    break;
  case 12:
    len == 1 ? sendScan(pack) : reply(INVALID_LENGTH, pack);
    break;
  case 'x':
    if(len == 2) {
      UVon = (buf[1] > 0);
      reply(headMove(false, 0, false, 0), pack);
    }
    else
      reply(INVALID_LENGTH, pack);
    break;
  case 'r':
    webCam->doSpecial(WC_TRACK, pack);
    return;
  case 'b': //get camera properties
    if(len == 1) {webCam->doSpecial(WC_PROPERTIES, pack); return; }
    else {reply(INVALID_LENGTH, pack);  break;}
  case 13: //get mean
    if(len == 2) {webCam->doSpecial(WC_MEAN, pack); return; }
    else {reply(INVALID_LENGTH, pack);  break;}
  case 14: //get motion
    if(len == 1) {webCam->doSpecial(WC_MOTION, pack); return; }
    else {reply(INVALID_LENGTH, pack);  break;}
  case 'o': //stop streaming
    if(len == 1) {
      webCam->doSpecial(WC_STOP, pack);
      return;
    }else
      reply(INVALID_LENGTH, pack);
    break;
  default:
    reply(UNKNOWN_TYPE, pack);
  }
  if(pack != NULL)
    delete pack;
}

/*****************************************************************************/

int roverHeadMove(Datapack * pack) {
  char *buf = pack->getData();
  int pan = (int)ntohl(*((int *) (buf+4)));
  int tilt = (int)ntohl(*((int *) (buf+8)));
  bool movePan = buf[1]!=0, moveTilt = buf[2]!=0;
  //give an error if what they need is already in use.
  if((movePan && panLocked) || (moveTilt && tiltLocked))
    return reply(RESOURCE_CONFLICT, pack);

  int status = headMove(movePan, pan, moveTilt, tilt);
  return reply(status, pack);
}

int RoverSetAll(Datapack * pack) {
  char *buf = pack->getData();
  int *arr = (int *) (buf+4);
  for(int i=0; i<9; i++) {
    arr[i] = ntohl(arr[i]);
    if(arr[i] == 0 && i > SERVO_START) //if zero, turn off mask
      arr[0] &= 255 - (1 << (i-1));
  }
  //printf("received set all %d %d %d\n", arr[0], arr[1], arr[2]);
  int status = setAll(arr[0], arr[1], arr[2], servoPos2Ang(SERVO0, arr[3]), servoPos2Ang(SERVO1, arr[4]), servoPos2Ang(SERVO2, arr[5]), servoPos2Ang(SERVO3, arr[6]), servoPos2Ang(PAN_SERVO, arr[7]), servoPos2Ang(TILT_SERVO, arr[8]), true);
  return reply(status, pack);
}

int RoverSpin(Datapack * pack) {
  char *buf = pack->getData();
  int speed = (int)ntohl(*((uint32_t *) (buf+4)));
  int status;
  if(legsLocked)
    status = RESOURCE_CONFLICT;
  else
    status = spin(speed);
  return reply(status, pack);
}

int RoverCrab(Datapack * pack) {
  char *buf = pack->getData();
  int speed = (int)ntohl(*((int *) (buf+4)));
  int angle = (int)ntohl(*((int *) (buf+8)));
  int status;
  if(legsLocked)
    status = RESOURCE_CONFLICT;
  else
    status = crab(speed, angle);
  return reply(status, pack);
}

int RoverTurn(Datapack * pack) {
  char *buf = pack->getData();
  int speed = (int)ntohl(*((int *) (buf+4)));
  double radius = (int)ntohl(*((int *) (buf+8)));
  int status;
  if(legsLocked)
    status = RESOURCE_CONFLICT;
  else
    status = quadTurn(speed, radius);
  return reply(status, pack);
}

int reply(int status, Datapack *pack) {
  unsigned char response[16];

  //I'm allowing certain commands to work with low batteries.  I set pack to 
  //NULL and respond with BATTERY_LOW
  if(pack == NULL) 
    return -1; 
   
  response[0] = status;//status byte
  response[1] = range;  //range reading
  response[2] = (motorPos[PAN_SERVO] >> 8) & 255;
  response[3] = motorPos[PAN_SERVO] & 255;
  response[4] = (motorPos[TILT_SERVO] >> 8) & 255;
  response[5] = motorPos[TILT_SERVO] & 255;
  response[6] = (distTraveled >> 8) & 255;
  response[7] = distTraveled & 255;
  response[8] = voltageBuffer->getVoltage();
  response[9] = (legsLocked ? 1 : 0)+(panLocked ? 2 : 0)+(tiltLocked ? 4 : 1) +
    (UVon ? 8 : 0);
  response[10] = cerebThreadState;
  response[11] = wcThreadStatus;
  
  return rgram->sendResponse((char *)response, 16, pack);
} // returnRoverResponse() //

void sigIntHandler(int sig) {
  if(exiting) //for some reason, crtl+C sends more than 1 sigint
    return;   //this keeps me from trying to delete things twice.
  exiting = true;
  printf("Exiting. . .\n");
  //delete camSerial;
  delete cerebSerial;
  delete webCam;
  //pthread_mutex_destroy(&cerebSerialMutex);
  exit(0);
}

void debug(int level, char * msg) {
  if(level < DEBUG_LEVEL && showDebug)
    fprintf(stderr, msg);
}

void parseArgs(int argc, char **argv) {
  for(int i=1; i<argc; i++) {
    if(!strcmp(argv[i], "debug"))
      showDebug = true;
    else if(!strcmp(argv[i], "duty"))
      doDutyTest = true;
    else if(!strcmp(argv[i], "duty2"))
      doDutyTest2 = true;
    else if(!strcmp(argv[i], "duty3"))
      doDutyTest3 = true;
    else if(!strcmp(argv[i], "auto"))
      autoStart = true;
    else
      printf("unrecognized command line argument: %s\n", argv[i]);
  }
}

void simulatePanorama() {
  for(int i=0; i<8; i++) 
    for(int j=0; j<2; j++) {
      headMove(true, 157-i*45, true, -30+j*15);
      usleep(timeToArrive(LOOK_MASK)*1000+ 400000);
    }
}

void reportCyclesRun(int cycles, struct timeb start) {
  FILE *duty = fopen("/root/duty.txt", "w");
  struct timeb now;
  ftime(&now);
  int secs = now.time - start.time;
  int minutes = secs / 60;
  secs = secs % 60;
  int hours = minutes / 60;
  minutes = minutes % 60;
  fprintf(duty, "Duty test ran for %d cycles.\n", cycles);
  fprintf(duty, "That's %d hours, %d minutes and %d seconds\n", hours, minutes, secs);
  fclose(duty);
}

void runDutyTest() {
  int timesRun = 0;
  struct timeb start;
  ftime(&start);
  printf("\nstarting duty test\n");
  while(voltageBuffer->getVoltage() >= LOW_BATTERY_THRESHOLD) {
    simulatePanorama();
    crab(200 * !(timesRun % 10), 0);
    simulatePanorama();
    spin(200 * !(timesRun % 10));
    timesRun++;
    reportCyclesRun(timesRun, start);
  }
}

void runDutyTest2() {
  int timesRun = 0;
  struct timeb start;
  ftime(&start);
  printf("\nstarting duty test\n");
  while(voltageBuffer->getVoltage() >= LOW_BATTERY_THRESHOLD) {
    crab(0, 0);
    sleep(6);
    spin(0);
    sleep(6);
    timesRun++;
    reportCyclesRun(timesRun, start);
  }
}

void runDutyTest3() {
  int timesRun = 0;
  struct timeb start;
  ftime(&start);
  printf("\nstarting duty test\n");
  while(voltageBuffer->getVoltage() >= LOW_BATTERY_THRESHOLD) {
    headMove(false, 0, true, 0);
    usleep(500000);
    headMove(false, 0, true, -45);
    usleep(500000);
    headMove(false, 0, true, 0);
    usleep(500000);
    headMove(false, 0, true, 45);
    usleep(500000);
    timesRun++;
    reportCyclesRun(timesRun, start);
  }
}

bool cameraWasCovered = false, diagThreadExited = false, *currWheel = NULL;

void * diagnosticThread(void * timeToRun) {
  char buf[100];
  unsigned char *camBuf;
  int runTime = *((int *) timeToRun);
  int avgY, i, lastY = -1;
  struct timeb start, now;
  cameraWasCovered = diagThreadExited = false;
  ftime(&start);
  ftime(&now);
  //printf("runtime is %d\n", runTime);
  //wait for 5 seconds or until someone tries to connect
  while(!rgram->hasNewPacket()) {
    if(webCam->grabImage(176, 144, false)) {
      camBuf = (unsigned char *)webCam->getBuf();
      avgY = 0;
      for(i=0; i<(176*144); i++)
	avgY += camBuf[i];
      avgY /= (176*144);
      if(lastY == -1)
	lastY = avgY;
      sprintf(buf, "the average Y is %d\n", avgY);
      debug(150, buf);
      if(avgY < 35) {
	/* If currWheel isn't NULL, then I'm interested in whether or not the
	   camera is covered and I don't want the thread to quit.  This is how
	   I make the rover reset to the default P2P configuration. */
	if(currWheel != NULL) {
	  *currWheel = true;
	  UVon = !UVon; // change the state of the UV tube 
	  headMove(false, 0, false, 0); 
	}
	else {
	  cameraWasCovered = true;
	  break;
	}
      }
      lastY = avgY;
    }else
      usleep(20000);
    ftime(&now);
    //if timeout is nonzero and it has timed out. . .
    if(runTime > 0 && timebDiff(now, start) >= runTime)
      break;
  }
  UVon = false;
  headMove(false, 0, false, 0); //turn the UV tube off
  diagThreadExited = true;
  pthread_exit(0);
}

void runDiagnostic() {
  pthread_t diagThread;
  int timeToRun = 6500, i, j;
  bool wheelCover[4] = {false, false, false, false};
  if(pthread_create(&diagThread, 0, diagnosticThread, (void *)&timeToRun)) {
    printf("Failed to start diagnostic thread\n");
    return;
  }
  if(pthread_join(diagThread, NULL))
    return;
  if(cameraWasCovered) {
    //move steering servos so that you know you've covered the camera
    for(i=0; i<=25; i++)
      setAll(ALL_MASK, 0, 0, i, i, i, i, 0, 0, true);
    for(i=5; i>=-25; i--)
      setAll(ALL_MASK, 0, 0, i, i, i, i, 0, 0, true);
    for(i=-5; i<=0; i++)
      setAll(ALL_MASK, 0, 0, i, i, i, i, 0, 0, true);
    sleep(1);
    UVon = true; //turn uv on
    //move pan around
    headMove(true, 90, true, 0);
    usleep(timeToArrive(LOOK_MASK)*1000);
    headMove(true, -90, true, 0);
    usleep(timeToArrive(LOOK_MASK)*1000);
    headMove(true, 0, true, 0);
    usleep(timeToArrive(LOOK_MASK)*1000);
    //move tilt around
    headMove(true, 0, true, -45);
    usleep(timeToArrive(LOOK_MASK)*1000);
    headMove(true, 0, true, 45);
    usleep(timeToArrive(LOOK_MASK)*1000);
    headMove(true, 0, true, 0);
    usleep(timeToArrive(LOOK_MASK)*1000);
    UVon = false; //turn uv off

    //start up camera again
    timeToRun = 0;
    if(pthread_create(&diagThread, 0, diagnosticThread, &timeToRun)) {
      printf("Failed to start diagnostic thread\n");
      return;
    }
    usleep(50000);
    pthread_detach(diagThread); //free up its memory resources when it exits

    for(j=0; j<4; j++) {
      currWheel = wheelCover + j;
      setAll(DRIVE_MASK, 0, 0, 90*!j, 90*!(j-3), 90*!(j-2), 90*!(j-1), 0, 0, true);
      usleep(timeToArrive(DRIVE_MASK)*1000);
      setAll(DRIVE_MASK, 0, 0, -90*!j, -90*!(j-3), -90*!(j-2), -90*!(j-1), 0, 0, true);
      usleep(timeToArrive(DRIVE_MASK)*1000);
      setAll(DRIVE_MASK, 0, 0, 0, 0, 0, 0, 0, 0, true);
      usleep(timeToArrive(DRIVE_MASK)*1000);
    }
    UVon = true;
    currWheel = NULL;

    int crabAngle = 0;
    /* if the camera was covered while it was moving the front left and front
       right wheels, change back to the default P2P configuration. */
    if(wheelCover[0] && !wheelCover[1] && !wheelCover[2] && wheelCover[3]) {
      crabAngle = restoreP2P();
      crab(0, crabAngle);
      usleep(timeToArrive(DRIVE_MASK)*1000);
    }else if(!wheelCover[0] && wheelCover[1] && wheelCover[2] && !wheelCover[3]) {
      crabAngle = restoreCMU();
      crab(0, crabAngle);
      usleep(timeToArrive(DRIVE_MASK)*1000);
    }
    //drive forward, slow->fast->slow
    for(j=0; j<=256; j++)
      crab(255-abs(128-j), crabAngle);
    //drive backwards, slow->fast->slow
    for(j=0; j<=256; j++)
      crab(abs(128-j)-255, crabAngle);
    crab(0, 0); //straighten out the wheels

    while(!cameraWasCovered && !diagThreadExited) {
      if(range > 30 && range < 60)
	crab(255, 0);
      else if(range > 90 && range < 135)
	crab(-255, 0);
      else {
	crab(0, 0);
      }
      usleep(5000);
    }
    crab(0, 0);
  }
}

//this function changes networks to the default P2P configuration
//it returns the angle at which to turn the motors
int restoreP2P() {
  pid_t pid;
  int status;FILE *error;

  switch(pid = fork()) {
  case -1: //failure
    printf("can't fork!, exiting. . .\n");
	error = fopen("/root/error.txt", "w");
	fprintf(error, "can't fork!, exiting. . .\n");
	fclose(error);
    exit(-1);
  case 0: //we're the child
    if(execl("/bin/bash", "bash", "/root/scripts/restoreP2P", 0)) {
      printf("The exec failed!\n");
	error = fopen("/root/error.txt", "w");
	fprintf(error, "The exec failed!\n");
	fclose(error);
      exit(-1);
    }
  default: //we're the parent
    waitpid(pid, &status, 0); //wait for child to die
    if(status != 0) {
      printf("Restoring default P2P failed!\n");
      error = fopen("/root/error.txt", "w");
      fprintf(error, "Restoring default P2P failed!\n");
      fclose(error);
      return -45;
    }
  }
  return -90;
}

//this function changes networks to CMU with DHCP on
//it returns the angle at which to turn the motors
int restoreCMU() {
  pid_t pid;
  int status;FILE *error;

  switch(pid = fork()) {
  case -1: //failure
    printf("can't fork!, exiting. . .\n");
	error = fopen("/root/error.txt", "w");
	fprintf(error, "can't fork!, exiting. . .\n");
	fclose(error);
    exit(-1);
  case 0: //we're the child
    if(execl("/bin/bash", "bash", "/root/scripts/netStation", 0)) {
      printf("The exec failed!\n");
	error = fopen("/root/error.txt", "w");
	fprintf(error, "The exec failed!\n");
	fclose(error);
      exit(-1);
    }
  default: //we're the parent
    waitpid(pid, &status, 0); //wait for child to die
    if(status != 0) {
      printf("Restoring CMu configuration failed!\n");
      error = fopen("/root/error.txt", "w");
      fprintf(error, "Restoring CMU configuration failed!\n");
      fclose(error);
      return 45;
    }
  }
  return 90;
}
