#include <Bounce.h>
//Pin numbers for bump sensors
int RR_button = 3;
int FR_button = 10;
int RL_button = 9;
int FL_button = 11;

//Pin numbers for motors (direction and PWM)
int LM = 6;
int LMdir = 7;
int RM = 5;
int RMdir = 4;

//Creation of the bounce objects (bump sensors)
Bounce BRbump = Bounce(RR_button,10);
Bounce FRbump = Bounce(FR_button,10);
Bounce BLbump = Bounce(RL_button,10);
Bounce FLbump = Bounce(FL_button,10);

//Testing LED
int LED = 13;

//Forward and reverse for writing motor direction
int f = 1;
int r = 0;
int pressed = 0;
int unpressed = 1;
int loopDelay = 50;
int spiralDelay = 100;
int LMspeed = 0;
int RMspeed = 0;
int accelConst = 1;
int minSpeed = 130;

//--------------------REFERENCE FUNCTIONS---------------------
void resetMotors(int dir){
  updateBumps();
  LMspeed = 0;
  RMspeed = 0;
  digitalWrite(LMdir,dir);
  digitalWrite(RMdir,dir);
  analogWrite(LM,0);
  analogWrite(RM,0);
  delay(300);
  
  return;
}


void rotateR2L(int accelConst){
  Serial.println("rotateR2L");
  //Rotates from a bump made to FRONT RIGHT until FRONT LEFT
  //hits aswell
  //OR
  //Rotates from a bump made to BACK LEFT until BACK RIGHT
  //hits aswell
  resetMotors(f);
  unsigned int startTime = millis();
  unsigned int errorTime = 7 * 1000;
  unsigned int inputTime = 2 * 1000;
  unsigned int runTime = 0;
  
  //Start Values
  LMspeed = minSpeed;
  RMspeed = -minSpeed;
  while(true){
    //Add const to Left, subtract from Right(reverse)
    delay(loopDelay);
    LMspeed += accelConst;
    RMspeed -= accelConst;
    
    //If temp exceeds 200, set back
    if(LMspeed >= 200){
      LMspeed = 200;
    }
    if(RMspeed <= -200){
         RMspeed = -200;
    }
 
    //Write temp to motors
    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    updateBumps();
    
    runTime = millis() - startTime;
    
    //If left hits, exit function
    if((FLbump.read() == pressed || BRbump.read()== pressed) && runTime >= inputTime){
      Serial.println("Front Left Pressed");
      return;
    }
    if(runTime>=errorTime){
      Serial.println("rotateR2L too long - Break");
      return;
    }   
  } 
}

void rotateL2R(int accelConst){
  Serial.println("rotateL2R");
  //Rotates from a bump made by FRONT LEFT, until the FRONT RIGHT
  //hits as well.
  //OR Rotates from a bump made by BACK RIGHT, until BACK LEFT
  //hits as well.
  
  resetMotors(f);
  unsigned int startTime = millis();
  unsigned int errorTime = 10 * 1000;
  unsigned int inputTime = 2 * 1000;
  unsigned int runTime = 0;

  //Start Values
  LMspeed = -minSpeed;
  RMspeed = minSpeed;
  while(true){
    //Add const to Left, subtract from Right(reverse)
    LMspeed -= accelConst;
    RMspeed += accelConst;
        
    //If temp exceeds 200, set back
    if(LMspeed <= -200){
      LMspeed = -200;
    }
    if(RMspeed >= 200){
         RMspeed = 200;
    }
 
    //Write temp to motors
    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    updateBumps();
    
    runTime = millis() - startTime;
    //If left hits, exit function
    if((FRbump.read() == pressed || BLbump.read() == pressed) && runTime >= inputTime){
      Serial.println("Front Right Pressed");
      return;
    }
    
    //Error check
    if(runTime>=errorTime){
      Serial.println("rotateL2R too long - Break");
      return;
    }
  } 
}

void updateBumps(){
  FRbump.update();
  FLbump.update();
  BRbump.update();
  BLbump.update();
  return;
}
      
    
void runMotor(int m,int mSpeed){
  //Runs motor m, at speed mSpeed. If mSpeed is negative,
  //run at that speed in reverse.
  if(m==5){
    Serial.print("Right Motor: ");
  }
  else{
    Serial.print("Left Motor: ");
  }
  
  Serial.println(mSpeed,DEC);
  int dir = 0;
  if(mSpeed >= 0){
    dir = f;
  }
  else{
    dir = r;
    mSpeed = mSpeed * -1;
  }
  
  if(m == LM){
    digitalWrite(LMdir,dir);
    analogWrite(LM,mSpeed);
  }
  else{
    digitalWrite(RMdir,dir);
    analogWrite(RM,mSpeed);
  }
 return;
}


//--------------------SEARCH ALGORITHMS-----------------------
void check(){
  Serial.println("check");
  resetMotors(f);
  while(true){
    updateBumps();  
    if(FRbump.read()== pressed || FLbump.read()==pressed){
      return;
    }
  }
}

void forwardHalf(){
  //STARTS FROM: edge of tank, rear touching tank
  //ENDS AS:  Middle of tank, facing same dir
  Serial.println("forwardHalf");
  int accelConst = 2;
  unsigned int halfPool = 10 * 1000;
  unsigned int startTime = millis();
  unsigned int runTime = 0;
  resetMotors(f);
  
  //Start Values
  LMspeed = minSpeed;
  RMspeed = minSpeed;
  while(true){
    delay(loopDelay);
    LMspeed += accelConst;
    RMspeed += accelConst;
    
    if(LMspeed >= 254){
      LMspeed = 255;
    }
    if(RMspeed >= 254){
      RMspeed = 255;
    }
   
    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    
    runTime = millis() - startTime;
    if (runTime >= halfPool){
      resetMotors(f);
      return;
    }
  }
}

void spiral(){
  //STARTS FROM: middle of the tank, facing any direction
  //ENDS AS: flush against wall of the tank (facing wall)
  Serial.println("spiral");
  resetMotors(f);
  int RMaccelConst = 2;
  int LMaccelConst = -1;
  int TaccelConst = 3;
  unsigned int errorTime = 40 * 1000;
  unsigned int startTime = millis();
  unsigned int inputTime = 5 * 1000;
  unsigned int runTime = 0;
  
  //Start Values
  LMspeed = -minSpeed;
  RMspeed = minSpeed;
  
  while(true){    
    //Add constants to temp speed
    delay(spiralDelay);
    LMspeed += LMaccelConst;
    RMspeed += RMaccelConst;
    
    //If temps exceed 255, set them back
    if(RMspeed >= 254){
      RMspeed = 255;
    }
    
    if(LMspeed <= -154){
      LMspeed = -155;
    }
   
    //Write temps to the motors
    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    
    updateBumps();
    
    //If front right hits first
    //Rotate till front left hits
    runTime = millis() - startTime;
    
    if(FRbump.read() == pressed && runTime >= inputTime){
       Serial.println("Front Right Pressed");
       rotateR2L(TaccelConst);
       return;
    }
    //If front left hits first
    //Rotate till front right hits
    if(FLbump.read() == pressed && runTime >= inputTime){
      Serial.println("Front Left Pressed");
      rotateL2R(TaccelConst);
      return;
    }
    
    //If the function is running too long, go forward to a wall
    //and break out
    if(runTime>=errorTime){
      Serial.println("Spiral Too Long - Break");
      fullForward();
      return;
    }
  }
}

void reverseCurve(){
  //STARTS FROM: edge of tank, facing wall
  //ENDS AS: edge of tank, facing away from wall
  Serial.println("reverseCurve");
  resetMotors(r);
  int RMconst = -3;
  int LMconst = -1;
  int Tconst = 2;
  unsigned int startTime = millis();
  unsigned int errorTime = 20 * 1000;
  unsigned int inputTime = 5 * 1000;
  unsigned int runTime = 0;
  
  //Start Values
  LMspeed = -minSpeed;
  RMspeed = -minSpeed;
  while(true){
    delay(loopDelay);
    LMspeed += LMconst;
    RMspeed += RMconst;
    
    if(LMspeed <= -150){
      LMspeed = -150;
    }
    if(RMspeed <= -255){
      RMspeed = -255;
    }
    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    
    updateBumps();
    runTime = millis() - startTime;
    if (BLbump.read() == pressed && runTime>=inputTime){
      Serial.println("Back Left Pressed");
      rotateR2L(Tconst);
      return;
    }
    if (BRbump.read() == pressed && runTime>=inputTime){
      Serial.println("Back Right Pressed");
      rotateL2R(Tconst);
      return;
    }
    
    if(runTime>=errorTime){
      Serial.println("Reverse Curve Too Long - run fullForward");
      fullForward();
      return;
    }
  }
}

void fullForward(){
  //STARTS AS: From any tank position
  //ENDS AS: Flush against the tank wall (facing wall)
  Serial.println("fullForward");
  resetMotors(f);
  int RMconst = 2;
  int LMconst = 2;
  int Tconst = 2;
  unsigned int startTime = millis();
  unsigned int errorTime = 20 * 1000;
  unsigned int runTime = 0;
  unsigned int inputTime = 5 * 1000;
  
  //Start Values
  LMspeed = minSpeed;
  RMspeed = minSpeed;
  while(true){
    delay(loopDelay);
    LMspeed += LMconst;
    RMspeed += RMconst;
    
    if(LMspeed >= 254){
      LMspeed = 255;
    }
    if(RMspeed >= 254){
      RMspeed = 255;
    }

    runMotor(LM,LMspeed);
    runMotor(RM,RMspeed);
    
    updateBumps();
    runTime = millis() - startTime;
    if(FRbump.read()==pressed && runTime>=inputTime){
      Serial.println("Front Right Pressed");
      rotateR2L(Tconst);
      return;
    }
    if(FLbump.read()==pressed && runTime>=inputTime){
      Serial.println("Front Left Pressed");
      rotateL2R(Tconst);
      return;
    }
    
    
    if(runTime>=errorTime){
      return;
    }
  }
}
    
    
  
//-----------------ARDUINO FUNCTIONS------------------------
void setup() {
  Serial.begin(9600); //Initialise serial for testing  
  //Initialise bump sensors (resistor and input)
  pinMode(RR_button, INPUT);
  digitalWrite(RR_button,HIGH);
  pinMode(FR_button, INPUT);
  digitalWrite(FR_button,HIGH);
  pinMode(RL_button, INPUT);
  digitalWrite(RL_button,HIGH);
  pinMode(FL_button, INPUT);
  digitalWrite(FL_button,HIGH);
  
  //Initialise motors and outputs(LED)
  pinMode(LMdir,OUTPUT);
  pinMode(RMdir,OUTPUT);  
  pinMode(LED,OUTPUT); //pin 13 (LED) as output
}

void loop() {
  check();
  forwardHalf();
  spiral();
  while(true){
    reverseCurve();
    fullForward();
  }
}