/*************************************
 Drive: mid level movement functions
*************************************/

int moveState = MOV_STOP;   // what robot is doing
 
int  moveSpeed   = 0;     // move speed stored here (0-100%)
int  speedIncrement = 10; // percent to increase or decrease speed

void moveBegin()
{
   motorBegin(MOTOR_LEFT);
   motorBegin(MOTOR_RIGHT);
   moveStop();
}

void moveLeft()
{
  changeMoveState(MOV_LEFT);
  motorForward(MOTOR_LEFT,  0);
  motorForward(MOTOR_RIGHT, moveSpeed);
  Serial.println("were going left... maybe????");
}

void moveRight()
{
  changeMoveState(MOV_RIGHT);
  motorForward(MOTOR_LEFT,  moveSpeed);
  motorForward(MOTOR_RIGHT, 0);
  Serial.println("were going right... maybe????");
}

void moveForward()
{
  changeMoveState(MOV_FORWARD);
  motorForward(MOTOR_LEFT,  moveSpeed);
  motorForward(MOTOR_RIGHT, moveSpeed);
}

void moveBackward()
{
  changeMoveState(MOV_BACK);
  motorReverse(MOTOR_LEFT, moveSpeed);
  motorReverse(MOTOR_RIGHT, moveSpeed);  
}

void moveRotate(int angle)
{
  changeMoveState(MOV_ROTATE);
  Serial.print("Rotating ");  Serial.print(angle);
  if(angle < 0)
  {
    Serial.println(" (left)"); 
    motorReverse(MOTOR_LEFT,  moveSpeed); 
    motorForward(MOTOR_RIGHT, moveSpeed);  
    angle = -angle;
  }
  else if(angle > 0)
  {
    Serial.println(" (right)");
    motorForward(MOTOR_LEFT,  moveSpeed);
    motorReverse(MOTOR_RIGHT, moveSpeed);
  }  
  int ms = rotationAngleToTime(angle, moveSpeed);
  movingDelay(ms); 
  moveBrake();   
}

void moveStop()
{
  changeMoveState(MOV_STOP);
  motorStop(MOTOR_LEFT);
  motorStop(MOTOR_RIGHT);
  Serial.println("stop lmao");
}

void moveBrake()
{
  changeMoveState(MOV_STOP);
  motorBrake(MOTOR_LEFT);
  motorBrake(MOTOR_RIGHT);
}

void moveSetSpeed(int speed)
{
  motorSetSpeed(MOTOR_LEFT, speed) ;
  motorSetSpeed(MOTOR_RIGHT, speed) ;
  moveSpeed = speed; // save the value
}

void moveSlower(int decrement)
{
   Serial.print(" Slower: "); 
   if( moveSpeed >= speedIncrement + MIN_SPEED)
     moveSpeed -= speedIncrement;     
   else moveSpeed = MIN_SPEED;
   moveSetSpeed(moveSpeed); 
}


void moveFaster(int increment)
{
  Serial.print(" Faster: ");
  moveSpeed += speedIncrement; 
  if(moveSpeed > 100)
     moveSpeed = 100;
  moveSetSpeed(moveSpeed);      
}

int moveGetState()
{
 return moveState;  
}

/*************************************
 functions to rotate the robot
*************************************/

// return the time in milliseconds to turn the given angle at the given speed
long rotationAngleToTime( int angle, int speed)
{
int fullRotationTime; // time to rotate 360 degrees at given speed

  if(speed < MIN_SPEED)
    return 0; // ignore speeds slower then the first table entry

  angle = abs(angle);

  if(speed >= 100)
    fullRotationTime = rotationTime[NBR_SPEEDS-1]; // the last entry is 100%
  else
 { 
    int index = (speed - MIN_SPEED) / SPEED_TABLE_INTERVAL ; // index into speed and time tables
    int t0 =  rotationTime[index];
    int t1 = rotationTime[index+1];    // time of the next higher speed 
    fullRotationTime = map(speed,  speedTable[index],  speedTable[index+1], t0, t1);  
   // Serial.print("index= ");  Serial.print(index); Serial.print(", t0 = ");  Serial.print(t0);  Serial.print(", t1 = ");  Serial.print(t1); 
 }
 // Serial.print(" full rotation time = ");  Serial.println(fullRotationTime);
  long result = map(angle, 0,360, 0, fullRotationTime);
  return result; 
}


// rotate the robot from MIN_SPEED to 100% increasing by SPEED_TABLE_INTERVAL
void calibrateRotationRate(int direction, int angle)
{  
  Serial.print(locationString[direction]);
  Serial.println(" calibration" );    
  for(int speed = MIN_SPEED; speed <= 100; speed += SPEED_TABLE_INTERVAL)
  {
 
    delay(1000);
    //blinkNumber(speed/10);   
 
    if( direction == DIR_LEFT)
    {    // rotate left
      motorReverse(MOTOR_LEFT,  speed); 
      motorForward(MOTOR_RIGHT, speed);  
    }
    else if( direction == DIR_RIGHT)
    {    // rotate right
      motorForward(MOTOR_LEFT,  speed);
      motorReverse(MOTOR_RIGHT, speed);
    }
    else
       Serial.println("Invalid direction");
    

    int time = rotationAngleToTime(angle, speed);

    Serial.print(locationString[direction]);
    Serial.print(": rotate ");
    Serial.print(angle);
    Serial.print(" degrees at speed ");
    Serial.print(speed);
    Serial.print(" for ");
    Serial.print(time);
    Serial.println("ms");
    delay(time); 
    motorStop(MOTOR_LEFT);
    motorStop(MOTOR_RIGHT);  
    delay(2000); // two second delay between speeds
  }    
}

// this is the low level movement state.
// it will differ from the command state when the robot is avoiding obstacles
void changeMoveState(int newState)
{
  if(newState != moveState)
  {
    Serial.print("Changing move state from "); Serial.print( states[moveState]);
    Serial.print(" to "); Serial.println(states[newState]);
    moveState = newState;
  } 
}

/************* high level movement functions ****************/

//moves in the given direction at the current speed for the given duration in milliseconds
void timedMove(int direction, int duration)
{
  Serial.print("Timed move ");
  if(direction == MOV_FORWARD) {
    Serial.println("forward");
    moveForward();    
  }
  else if(direction == MOV_BACK) {
    Serial.println("back");
    moveBackward();     
  }
  else
    Serial.println("?");
    
  movingDelay(duration);
  moveStop();
}

// check for obstacles while delaying the given duration in ms

void movingDelay(long duration)
{
  long startTime = millis();
  while(millis() - startTime < duration) {
     // function in Look module checks for obstacle in direction of movement 
     if(checkMovement() == false) { 
        if( moveState != MOV_ROTATE) // rotate is only valid movement
        {
            Serial.println("Stopping in moving Delay()"); 
            moveBrake(); 
        }
     }  
  }  
}

boolean checkMovement() {
  return true;
}