// This is going to be my first completely from scratch bot. It's going to be just a basic object avoidance bot.
// First run will just turn lights on and off on either side to prove that I can do it. 
// Then we'll reverse that. They will be on unless a button is pressed. This will be functional code that will switch off the opposite motor to get the bot to turn. Then i'll have to figure out how to reverse the motor when that happens.
// Then i'll want to have it reverse for a few seconds then go straight again.

// Alright, roughly working. I need to write a schematic for this. 


// TODO 
// Reverse a motor instead of just stopping it. Probably should stop the motor on the side we hit on and reverse the other. Gotta figure out how to do such a thing.
// handle what happens if the button is pressed while a different method is still running.
// Handle both buttons pressed

#define LEFTBUTTON 7    // The left bumper
#define RIGHTBUTTON 8  // The right bumper
#define LEFTMOTOR   12   // Left hand side of the bot
#define RIGHTMOTOR  11    // Riht hand side of the bot





int rval = 0;        // val will be used to store the state of the input pin
int rold_val = 0;    // The previous value of val
int rstate= 0;      // Store the state of our circuit. 0 = off 1 = on

int lval = 0;        // val will be used to store the state of the input pin
int lold_val = 0;    // The previous value of val
int lstate= 0;      // Store the state of our circuit. 0 = off 1 = on
boolean debug=true;



void setup() {
    delay(1000); // Wait one second just in case I've screwed something up.
    pinMode(LEFTMOTOR, OUTPUT);    // Pin 13 will be an ouput pin
    pinMode(RIGHTMOTOR, OUTPUT);    // Pin 13 will be an ouput pin
    pinMode(LEFTBUTTON, INPUT);  // Pin 7 will be input
    pinMode(RIGHTBUTTON, INPUT);  // Pin 7 will be input
    if (debug == true) {   
      Serial.begin(9600);
    }
}

void loop() {
  lval = digitalRead(LEFTBUTTON); // SRead input value and store it
  rval = digitalRead(RIGHTBUTTON); // SRead input value and store it
  
  // Check whether the left input Has transitioned
  if ((lval == HIGH) && (lold_val == LOW)) {
    lstate = 1 - lstate;
    delay(10);
  } 
  
  // Check whether the right input Has transitioned
  if ((rval == HIGH) && (rold_val == LOW)) {
    rstate = 1 - rstate;
    delay(10);
  } 
    
  lold_val = lval; // val is now old so let's store it
  rold_val = rval; // val is now old so let's store it
 
    
  if (lstate == 1) {
    if (debug == true) {
      Serial.println("Left bumper triggered");
      Serial.print("values are lval ");
      Serial.print(lval);
      Serial.print(" rval ");
      Serial.print(rval);
      Serial.print(" lstate ");
      Serial.print(lstate);
      Serial.print(" rstate ");
      Serial.println(rstate);
    }
    leftbumper(); 
    //lstate = 0;
  }
  
  if (rstate == 1) {
    if (debug == true) {
      Serial.println("Right bumper triggered");
      Serial.print("values are lval ");
      Serial.print(lval);
      Serial.print(" rval ");
      Serial.print(rval);
      Serial.print(" lstate ");
      Serial.print(lstate);
      Serial.print(" rstate ");
      Serial.println(rstate);
    }
    rightbumper(); 
    //rstate = 0; // zero rstate
  }
  // If nothing has changed both motors are on and we should be going straight. ish
  digitalWrite(RIGHTMOTOR, LOW);
  digitalWrite(LEFTMOTOR, LOW);
      lstate = 0; // zero rstate
      rstate = 0; // zero rstate
}


void rightbumper() {
    // Turn left
    if (debug == true) {
      Serial.println("Turning left");
    }
    digitalWrite(RIGHTMOTOR, LOW);
    digitalWrite(LEFTMOTOR, HIGH);  
    delay(3000); // keep going like this for three seconds. This time period will need to be tuned down vor varios motors.
}

void leftbumper() {
    // Turn right
    if (debug == true) {
      Serial.println("Turning right");
    }
    digitalWrite(LEFTMOTOR, LOW);
    digitalWrite(RIGHTMOTOR, HIGH);  
    delay(3000); // keep going like this for three seconds. This time period will need to be tuned down vor varios motors.
}

void bothbumpers() {
    // Hit something head on, back up a sec then turn some other direction
    // not yet implemented
    if (debug == true) {
      Serial.println("Both bumpers triggered, drastic actions required");
    }
    // something like this to cause us to back up.
    digitalWrite(LEFTMOTOR, LOW);
    digitalWrite(RIGHTMOTOR, LOW);  
    delay(100); // But just for a second
    // then we will randomly select a direction and turn for just a second.
}