/*
drive.c
This file contains all functions relevant to driving and turning.
*/

/*calibrating variables*/
int right90ticks = 77;
int left90ticks = 77;
int right180ticks = 165;
int left180ticks = 170;
int straight = 1970;
long turnTimeOut = (long)5000;

void forward(int speed) {
  int maxLeftSpeed = speed;
  int maxRightSpeed = speed*7/10;
  int minLeftSpeed = 0;
  int minRightSpeed = 0;
  int rightSpeed;
  int leftSpeed;
  int leftEncoder = 0;
  int rightEncoder = 0;
  int errorOK = 0;
  int countChange = 0;

  servoStraight(straight);
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);

  rightSpeed = 0;
  leftSpeed = 0;

  while(1) {
 
    if (rightSpeed < maxRightSpeed) {
      rightSpeed = rightSpeed + 35;
    }
    else {
      rightSpeed = maxRightSpeed;
    }

    if (leftSpeed < maxLeftSpeed) {
      leftSpeed = leftSpeed + 35;
    }
    else {
      leftSpeed = maxLeftSpeed;
    }

    printf("\nforw l=%d r=%d change=%d", leftEncoder, rightEncoder, countChange);
    setRightSpeed(rightSpeed);
    setLeftSpeed(leftSpeed);
    leftEncoder = read_encoder(7);
    rightEncoder = read_encoder(8);
     if (rightEncoder > leftEncoder + errorOK) {
       setRightSpeed((rightSpeed*4/10));
       setLeftSpeed(leftSpeed);
       servoStraight(straight-10);
       countChange++;
     }
     else if (leftEncoder > rightEncoder + errorOK) {
       setRightSpeed(rightSpeed);
       setLeftSpeed((leftSpeed*4/10));
       servoStraight(straight+10);
       countChange++;
     }
     else {
       setRightSpeed(rightSpeed);
       setLeftSpeed(leftSpeed);
       servoStraight(straight);
     }
     /* sleep(.05);*/
  }
}

int forwardUntilHitTimeout(long timeout_ms) {
  int pid, timedOut=0;
  long initial_time = mseconds();
  pid = start_process(forward(100));
  while (!frontLeftHit() && !frontRightHit()) {
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else sleep(.01);
  }
  kill_process(pid);
  stop();
  return timedOut;
}

int forwardIntoWall(long timeout_ms) {
  int timedOut = 0;
  int lStop = 0;
  int rStop = 0;
  long initial_time;
  initial_time = mseconds();
  servoStraight(straight);
  sleep(.3);
  setLeftSpeed(100);
  setRightSpeed(100);
  while (!lStop || !rStop) {
    if (mseconds() - initial_time > timeout_ms) {
      timedOut = 1;
      break;
    }
    if (frontLeftHit() && !lStop) {
      lStop = 1;
      leftStop();
    }
    if (frontRightHit() && !rStop) {
      rStop = 1;
      rightStop();
    }
  }
  return timedOut;
}

int forwardCountTicksUntilTimeout(int ticks, long timeout_ms, int hit) {
  /*hit - 0 if don't want to do hit detection
          1 if you want hit detection
  */
  int pid, timedOut=0;
  int right, left=0;
  long initial_time = mseconds();
  reset_encoder(7);
  reset_encoder(8);
  pid = start_process(forward(100));
  while (right < ticks && left < ticks) {
    if (hit) {
      if (frontLeftHit() || frontRightHit()) {
	break;
      }
    }
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else {
      sleep(.01);
      left = read_encoder(7);
      right = read_encoder(8);
    }
  }
  kill_process(pid);
  stop();
  return timedOut;
}

/* replaced by forwardCountTicksUntilTimeout with hit variable
int forwardCountTicksUntilHitOrTimeout(int ticks, long timeout_ms) {
  int pid, timedOut=0;
  int right, left=0;
  long initial_time = mseconds();
  reset_encoder(7);
  reset_encoder(8);
  pid = start_process(forward(100));
  while (right < ticks && left < ticks && !frontLeftHit() && !frontRightHit()) {
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else {
      sleep(.01);
      left = read_encoder(7);
      right = read_encoder(8);
    }
  }
  kill_process(pid);
  stop();
  return timedOut;
}
*/

int forwardToBallTimeout(int ticks, long timeout_ms) {
  /*ticks - number of ticks before deceleration*/
  int pid, timedOut=0;
  int right, left=0;
  long initial_time = mseconds();
  reset_encoder(7);
  reset_encoder(8);
  pid = start_process(forward(100));
  while (right < ticks && left < ticks && !frontLeftHit() && !frontRightHit()) {
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else {
      sleep(.01);
      left = read_encoder(7);
      right = read_encoder(8);
    }
  }
  kill_process(pid);

  if (!timedOut) {
    pid = start_process(forward(70));  
    while (!frontLeftHit() && !frontRightHit()) {
      if ((mseconds()-initial_time)>timeout_ms) {
	timedOut=1;
	break;
      }
      else {
	sleep(.01);
      }
    }
    kill_process(pid);
  }
  stop();
  return timedOut;
}

int forwardToDropBallTimeout(long timeout_ms) {
  int pid, timedOut=0;
  long initial_time = mseconds();
  int to;
  pid = start_process(forward(100));
  while (!frontLeftHit() || !frontRightHit()) {
    if (frontLeftHit() || frontRightHit()) {
      stop();
      rampDown();
      kill_process(pid);
      return 0;
    }
/*    if (frontLeftHit()) {
      
      kill_process(pid);  
      stop();

      
      if (gotBall()) {
	to = forwardIntoWall((long) 1500);
	if (to) {
	
	   backwardCountTicksUntilTimeout(30, (long) 1000);
	   right90(-right90ticks*2/3);
	   pid = start_process(forward(100));
	} else {
	
	  rampDown();
	  sleep(.5);
	  rampUp();
	  if (!gotBall()) {
	
	    return 0;
	     
	  } else {
	
	    backwardCountTicksUntilTimeout(30, (long) 1000);
	    right90(-right90ticks*2/3);
	    pid = start_process(forward(100));
	  }
	}
      } else {
	backwardCountTicksUntilTimeout(30, (long) 1000);
	right90(-right90ticks*2/3);
	pid = start_process(forward(100));
      }
    }
    else if (frontRightHit()) {
     
      kill_process(pid);  
      stop();
      
      if (gotBall()) {
	to = forwardIntoWall((long) 1500);
	if (to) {
	
	  backwardCountTicksUntilTimeout(30, (long) 1000);
	  left90(-left90ticks*2/3);
	  pid = start_process(forward(100));
	} else {
	
	  rampDown();
	  sleep(.5);
	  rampUp();
	  sleep(.5);
	  if (!gotBall()) {
	
	    return 0;
	     
	  } else {
	
	    backwardCountTicksUntilTimeout(30, (long) 1000);
	    left90(-left90ticks*2/3);
	    pid = start_process(forward(100));
	  }
	}
      } else {
	backwardCountTicksUntilTimeout(30, (long) 1000);
	left90(-left90ticks*2/3);
	pid = start_process(forward(100));
      }
      }*/
    else if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else sleep(.01);
  }
  kill_process(pid);
  stop();
  return timedOut;
}

void backward() {
  int maxLeftSpeed = 0;
  int maxRightSpeed = 0;
  int minLeftSpeed = -100;
  int minRightSpeed = -80;
  int leftSpeed;
  int rightSpeed;
  int leftEncoder = 0;
  int rightEncoder = 0;
  int errorOK = 0;
  int countChange = 0;

  servoStraight(2000);
  sleep(.3); /*wait for servo to turn*/

  setLeftSpeed(leftSpeed);
  setRightSpeed(rightSpeed);

  reset_encoder(7);
  reset_encoder(8);

  rightSpeed = 0;
  leftSpeed = 0;

  while(1) {
    leftEncoder = read_encoder(7);
    rightEncoder = read_encoder(8);

    if (rightSpeed > minRightSpeed) {
      rightSpeed = rightSpeed - 35;
    }
    else {
      rightSpeed = minRightSpeed;
    }

    if (leftSpeed > minLeftSpeed) {
      leftSpeed = leftSpeed - 35;
    }
    else {
      leftSpeed = minLeftSpeed;
    }

    printf("\nback l=%d r=%d change=%d", leftEncoder, rightEncoder, countChange);
     if (rightEncoder > leftEncoder + errorOK) {
       countChange++;
       setRightSpeed((rightSpeed*4/10));
       setLeftSpeed(leftSpeed);
     } else if (leftEncoder > rightEncoder + errorOK) {
       countChange++;
       setLeftSpeed((leftSpeed*4/10));
       setRightSpeed(rightSpeed);
     } else {
       setRightSpeed(rightSpeed);
       setLeftSpeed(leftSpeed);
     }
     sleep(.05);
  }  
}

int backwardUntilTimeout(long timeout_ms) {
  int pid, timedOut=0;
  long initial_time = mseconds();
  pid = start_process(backward());
  while (!((mseconds()-initial_time)>timeout_ms)) {
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else sleep(.01);
  }
  
  kill_process(pid);
  stop();
  return timedOut;
}

int backwardUntilHitTimeout(long timeout_ms) {
  int pid, timedOut=0;
  long initial_time = mseconds();
  pid = start_process(backward());
  while (!backLeftHit() && !backRightHit()) {
    if ((mseconds()-initial_time)>timeout_ms) {
      timedOut=1;
      break;
    }
    else sleep(.01);
  }
  kill_process(pid);
  stop();
  return timedOut;
}

int backwardIntoWall(long timeout_ms) {
  int timedOut = 0;
  int lStop = 0;
  int rStop = 0;
  long initial_time = mseconds();
  servoStraight(2000);
  sleep(.3);
  setLeftSpeed(-100);
  setRightSpeed(-100);
  
  while (!lStop || !rStop) {
    if (mseconds() - initial_time > timeout_ms) {
      timedOut = 1;
      break;
    }
    if (backLeftHit() && !lStop) {
      lStop = 1;
      leftStop();
    }
    if (backRightHit() && !rStop) {
      rStop = 1;
      rightStop();
    }
  }
  return timedOut;
}

int backwardCountTicksUntilTimeout(int ticks, long timeout_ms) {
  int pid, timedOut=0;
  int right, left=0;
  long initial_time = mseconds();
  reset_encoder(7);
  reset_encoder(8);
  pid = start_process(backward());
  while (right < ticks && left < ticks) {
    if ((mseconds()-initial_time) > timeout_ms) {
      timedOut=1;
      break;
    } else {
      sleep(.01);
      left = read_encoder(7);
      right = read_encoder(8);
    }
  }

  kill_process(pid);
  stop();
  return timedOut;

}

int right90(int dev) {
  /*dev - # of ticks + or - from calibrated setting*/
  int right = 0; /*ticks counted on right side*/
  int left = 0; /*ticks counted on left side*/
  int rStop = 0; /*boolean indicates whether right side has stopped*/
  int lStop = 0; /*boolean indicates whether left side has stopped*/
  int rightSpeed = 0; /*speed we want on right side*/
  int leftSpeed = 0; /*speed we want left side*/
  int timedOut = 0;
  int turnSpeed = 30; /*max speed of the turn*/
  long initial_time = mseconds();
  int rOld, lOld;

  servoTurn();
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);
  setLeftSpeed(30);
  setRightSpeed(-50);

  while (left < (right90ticks + dev) && right < (right90ticks + dev)) {
  
    if ((mseconds()-initial_time) > turnTimeOut) {
      timedOut=1;
      break;
    }
    /*if (rightSpeed > -turnSpeed) {
      rightSpeed = rightSpeed - 15;
    }
    else {
      rightSpeed = -turnSpeed;
    }
    
    if (leftSpeed < turnSpeed) {
      leftSpeed = leftSpeed + 15;
    }
    else {
      leftSpeed = turnSpeed;
      }*/
    /* if (!rStop) {
      setRightSpeed(-60);
    }
    if (!lStop) {
      setLeftSpeed(30);
    }
    */
    right = read_encoder(8);
    if (right > (right90ticks + dev) && !rStop) {
      rightStop();
      rStop = 1;
      /*      brakeRightBackward();
       rOld = right;
      */
    }
      left = read_encoder(7);
    if (left > (right90ticks + dev) && !lStop) {
       leftStop();
       lStop = 1;
       /*  brakeLeftForward();
       lOld = left;
       */
    }
      printf("\nr=%d l=%d ro=%d lo=%d", right, left, right-rOld, left-lOld);
     
     
  }

  stop();
  return timedOut;
}

int left90(int dev) {
  /*dev - # of ticks + or - from calibrated setting*/
  int right = 0;
  int left = 0;
  int rStop = 0;
  int lStop = 0;
  int rightSpeed = 0;
  int leftSpeed = 0;
  int timedOut = 0;
  long initial_time = mseconds();

  servoTurn();
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);
  setRightSpeed(50);
  setLeftSpeed(-30);
      
  while (!(right > (left90ticks + dev) && left > (left90ticks + dev))) {
    if ((mseconds()-initial_time)>turnTimeOut) {
      timedOut=1;
      break;
    }
    /*    if (rightSpeed < 20) {
      rightSpeed = rightSpeed + 5;
    }
    else {
      rightSpeed = 20;
    }

    if (leftSpeed > -20) {
      leftSpeed = leftSpeed - 5;
    }
    else {
      leftSpeed = -20;
    }
    if (!rStop) {
      setRightSpeed(rightSpeed);
    }
    if (!lStop) {
      setLeftSpeed(leftSpeed);
      }*/
     right = read_encoder(8);
    if (right > (left90ticks + dev) && !rStop) {
      rStop = 1;
      /*brakeRightForward();*/
      rightStop();
    }
  
    left = read_encoder(7);
    if (left > (left90ticks + dev) && !lStop) {
      lStop = 1;
      /*      brakeLeftBackward();*/
      leftStop();
    }
     printf("\nleft 90 r=%d l=%d", right, left);
  }
  stop();
  return timedOut;
}

int right180(int dev) {
  /*dev - # of ticks + or - from calibrated setting*/
  int right = 0; /*ticks counted on right side*/
  int left = 0; /*ticks counted on left side*/
  int rStop = 0; /*boolean indicates whether right side has stopped*/
  int lStop = 0; /*boolean indicates whether left side has stopped*/
  int rightSpeed = 0; /*speed we want on right side*/
  int leftSpeed = 0; /*speed we want left side*/
  int timedOut = 0;
  long initial_time = mseconds();

  servoTurn();
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);
  setLeftSpeed(30);
  setRightSpeed(-50);
    
  while (!(left > (right180ticks + dev) && right > (right180ticks + dev))) {
    printf("\nright180 r=%d l=%d", right, left);
    if ((mseconds()-initial_time)>turnTimeOut) {
      timedOut=1;
      break;
    }
    /*    if (rightSpeed > -20) {
      rightSpeed = rightSpeed - 5 ;
    }
    else {
      rightSpeed = -20;
    }

    if (leftSpeed < 20) {
      leftSpeed = leftSpeed + 5;
    }
    else {
      leftSpeed = 20;
      }
    if (!rStop) {
      setRightSpeed(rightSpeed);
    }
    if (!lStop) {
      setLeftSpeed(leftSpeed);
      }*/

    left = read_encoder(7);
    right = read_encoder(8);
    if (right > (right180ticks + dev) && !rStop) {
      rStop = 1;
      /*      brakeRightBackward();*/
      rightStop();
    }
    if (left > (right180ticks + dev) && !lStop) {
      lStop = 1;
      /*      brakeLeftForward();*/
      leftStop();
    }
  }
  stop();
  return timedOut;
}

int left180(int dev) {
  /*dev - # of ticks + or - from calibrated setting*/
  int right = 0;
  int left = 0;
  int rStop = 0;
  int lStop = 0;
  int rightSpeed = 0;
  int leftSpeed = 0;
  int timedOut = 0;
  long initial_time = mseconds();

  servoTurn();
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);
  setRightSpeed(50);
  setLeftSpeed(-30);
   
  while (!(right > (left180ticks + dev) && left > (left180ticks + dev))) {
   
    if ((mseconds()-initial_time)>turnTimeOut) {
      timedOut=1;
      break;
    }
    /*    if (rightSpeed < 20) {
      rightSpeed = rightSpeed + 5;
    }
    else {
      rightSpeed = 20;
    }

    if (leftSpeed > -20) {
      leftSpeed = leftSpeed - 5;
    }
    else {
      leftSpeed = -20;
    }
    if (!rStop) {
      setRightSpeed(rightSpeed);
    }
    if (!lStop) {
      setLeftSpeed(leftSpeed);
      }*/
    left = read_encoder(7);
    right = read_encoder(8);
    if (right > (left180ticks + dev) && !rStop) {
      rStop = 1;
      /* brakeRightForward();*/
      rightStop();
    }
    if (left > (left180ticks + dev) && !lStop) {
      lStop = 1;
      /*     brakeLeftBackward();*/
      leftStop();
    }
  }
  stop();
  return timedOut;
}

void spinRight() {
  servoTurn();
  sleep(.2); /*wait for servo to turn*/

  setRightSpeed(-80);
  setLeftSpeed(100);
}

void spinLeft() {
  servoTurn();
  sleep(.2); /*wait for servo to turn*/

  setRightSpeed(80);
  setLeftSpeed(-100);
}

void setLeftSpeed(int speed) {
  motor(0, speed);
  motor(1, speed);
  if (speed > 0) {
    motor(5, 100);
  } else if (speed < 0) {
    motor(5, -100);
  }
}


void setRightSpeed(int speed) {
  motor(2, speed);
  motor(3, speed);
  off(4);
  /*  if (speed > 0) {
    motor(4, 100);
  } else if (speed < 0) {
    motor(4, -100);
    }*/
}
/*
void brakeLeftForward() {
  off(1);
  off(5);
  motor(0, -50);
  sleep(.05);
  off(0);
}

void brakeRightForward() {
  off(3);
  off(4);
  motor(2, -50);
  sleep(.05);
  off(2);
}

void brakeLeftBackward() {
  off(1);
  off(5);
  motor(0, 50);
  sleep(.05);
  off(0);
}

void brakeRightBackward() {
  off(4);
  off(3);
  motor(2, 50);
  sleep(.05);
  off(2);
}
*/
void rightStop() {
  off(2);
  off(3);
  off(4);
}

void leftStop() {
  off(0);
  off(1);
  off(5);
}

void stop() {
  rightStop();
  leftStop();
}

void servoStraight(int angle) {
 servo(0, angle);
}

void servoTurn() {
  servo(0, 3900);
}

int getBall() {
  int count = 0;
  int timedOut;
  long initial_time;
  while(count < 3) {
    rampUp();
    initial_time = mseconds();
    timedOut = 0;
    while(!gotBall()) {
      if ((mseconds()-initial_time)>(long) 1000) {
	timedOut=1;
	printf("\nNO BALL!");
	break;
      }
      sleep(.05);
    }
    
    /*
    sleep(.75);
    if (gotBall()) {
      return 1;
    }
    */

    if (!timedOut) {
      printf("\nGOT BALL!");
      return 1;
    }
    else {
      backwardCountTicksUntilTimeout(20, (long) 1000);
      rampDown();
      sleep(.2);
      forwardUntilHitTimeout((long)1000); 
    }
    count++;
  }
  return 0;
}

int right180wide(int dev) {
  int right = 0; /*ticks counted on right side*/
  int left = 0; /*ticks counted on left side*/
  int rStop = 0; /*boolean indicates whether right side has stopped*/
  int lStop = 0; /*boolean indicates whether left side has stopped*/
  int leftSpeed = 0; /*speed we want left side*/
  int timedOut = 0;
  int ticks = 250; /*ticks to rotate*/
  long timeOutTime = (long) 8000; 
  long initial_time = mseconds();

  servo(0, 800); /*aim towards stationery wheel*/
  sleep(.5); /*wait for servo to turn*/

  reset_encoder(7);
  setRightSpeed(15);
  setLeftSpeed(100);
  while (left <  ticks + dev) {
    printf("\nright180 r=%d l=%d", right, left);
    if ((mseconds()-initial_time) > timeOutTime) {
      timedOut=1;
      break;
    }
    left = read_encoder(7);
  }
 
  return timedOut;  
}


int right180spin(int dev) {
  /*dev - # of ticks + or - from calibrated setting*/
  int right = 0; /*ticks counted on right side*/
  int left = 0; /*ticks counted on left side*/
  int rStop = 0; /*boolean indicates whether right side has stopped*/
  int lStop = 0; /*boolean indicates whether left side has stopped*/
  int rightSpeed = 0; /*speed we want on right side*/
  int leftSpeed = 0; /*speed we want left side*/
  int timedOut = 0;
  int ticks = 120;
  long initial_time = mseconds();

  servo(0, 200);
  sleep(.3); /*wait for servo to turn*/

  reset_encoder(7);
  reset_encoder(8);
  setLeftSpeed(100);
  setRightSpeed(-100);
    
  while (left < ticks+dev  && right < ticks+dev) {
    printf("\nright180 r=%d l=%d", right, left);
    if ((mseconds()-initial_time)>turnTimeOut) {
      timedOut=1;
      break;
    }

    left = read_encoder(7);
    right = read_encoder(8);
  }
  
  return timedOut;
}