#include <sparki.h>

#define STATE_FIRST_ONE 0
#define STATE_SECOND_ONE 1

// Set up some global variables with default values to be replaced during operation
int current_state = 0;
const int threshold = 500; // IR reading threshold to detect whether there's a black line under the sensor
int cm_distance = 1000;
int distance = 0;
int line_left = 1000;
int line_center = 1000;
int line_right = 1000;


void setup() {
  // put your setup code here, to run once:
  sparki.RGB(RGB_RED); // Turn on the red LED
  sparki.servo(SERVO_CENTER); // Center the ultrasonic sensor
  delay(1000); // Give the motor time to turn
  sparki.gripperOpen(); // Open the gripper
  delay(5000); // Give the motor time to open the griper
  sparki.gripperStop(); // 5 seconds should be long enough
  sparki.RGB(RGB_GREEN); // Change LED to green so we know the robot's setup is done!
}

void readSensors() {
  cm_distance = sparki.ping(); // Replace with code to read the distance sensor
  line_left = sparki.lineLeft(); // Replace with code to read the left IR sensor
  line_right = sparki.lineRight(); // Replace with code to read the right IR sensor
  line_center = sparki.lineCenter(); // Replace with code to read the center IR sensor
}

void loop() {
  // put your main code here, to run repeatedly:
  readSensors(); // Read sensors once per loop() call

  sparki.clearLCD();
  sparki.print("STATE: ");
  sparki.println(current_state);

  // Your state machine code goes here
  switch (current_state) {
    case 0:
      //rotate sparki around its center
      sparki.moveRight();

      //when sparki detects the object is within 30cm
      if (cm_distance != -1) {
       // sparki.println(cm_distance);


        if (cm_distance <= 30) {
          distance = cm_distance;
          sparki.moveStop();
          //sparki.println(cm_distance);
          current_state = 1;
        }
      }
      break;
    case 1:
      sparki.moveForward(distance); //move to the object
      sparki.moveStop();
      current_state = 2;
      break;
    case 2:
      sparki.gripperClose(); //grab the object
      delay(5000);
      sparki.gripperStop();
      sparki.moveRight(180); //rotate 180 degrees
      sparki.moveStop();
      current_state = 3;
      break;
    case 3:
      sparki.moveForward(); //move towards the line
      if (line_center < threshold) { //once sparki detects the line stop
        sparki.moveStop();
        current_state = 4;
      }
      break;
    case 4:
      if ( line_left < threshold ) // if line is below left line sensor
      {
        sparki.moveLeft(); // turn left
      }

      if ( line_right < threshold ) // if line is below right line sensor
      {
        sparki.moveRight(); // turn right
      }

      // if the center line sensor is the only one reading a line
      if ( (line_center < threshold) && (line_left > threshold) && (line_right > threshold) )
      {
        sparki.moveForward(); // move forward
      }

      if ((line_left < threshold) && (line_center < threshold) && (line_right < threshold)) {
        sparki.moveStop();
        current_state = 5;
      }
      break;
    case 5:
      sparki.beep();
      sparki.gripperOpen();
      delay(5000);
      sparki.gripperStop();
      current_state = 6;
      break;
    case 6:
      sparki.moveStop();
  }

  sparki.updateLCD();
  delay(100); // Only run controller at 10Hz
}