/***************************************
 * Name   : Lab 01Demo                 *
 * Author : IamN0tRobot                *
 ***************************************/
#include "3DoTConfig.h"

void setup() {
  Serial.begin(9600);
  init3DoT();
}

void loop() {
    takeAStep();
}

void takeAStep() {
  /* Finite State Machine */
  static int state = 0;    // fsm state

  /* Read the inside IR sensors */
  static uint8_t sensorLeftInner = LOW;
  static uint8_t sensorRightInner = LOW;
  sensorLeftInner  = sensorRead(IR_L_I,sensorLeftInner);  // White = 0, LOW
  sensorRightInner = sensorRead(IR_R_I,sensorRightInner); // Black = 1, HIGH
  /* Run the path following algorithm */ 
    switch(state) {
      case 0:        // start
        delay(2000); // robot moved to maze
        state = 1;   // continue to state 1
        break;
      case 1:        // walk
        if(sensorLeftInner == BLACK || sensorRightInner == BLACK) {
          // start hopping
          setMotor(forward);
          state = 2;
        } else {
          walk();
        }    
        break;
      /* continue over the line until both sensor leave the intersection */
      case 2:  //hop
        if(sensorLeftInner == WHITE && sensorRightInner == WHITE) {
          setMotor(brake);    
          state = 0;
        }
        break;
     } // end switch w/o default state
}

void walk() {
  /* Initialization */
  static uint8_t sensorLeft = LOW;
  static uint8_t sensorRight = LOW;
  /* Read IR sensors */
  sensorLeft  = sensorRead(IR_L_O,sensorLeft);  // White = 0, LOW
  sensorRight = sensorRead(IR_R_O,sensorRight); // Black = 1, HIGH
  // To follow a path the ir sensor controls the opposite motor
  analogWrite(AIN1, 0);     // left motor
  analogWrite(AIN2, touchAWall(sensorRight,motorLeftHIGH));
  analogWrite(BIN1, 0);     // right motor
  analogWrite(BIN2, touchAWall(sensorLeft,motorRightHIGH));
}

uint8_t sensorRead(uint8_t sensor_pin, uint8_t current_level) {
  uint8_t  logic_level = current_level;   // initial value white, motor ON 
  uint16_t analog_value = analogRead(sensor_pin);
  if (analog_value >= trigger1) logic_level = HIGH;
  if (analog_value <= trigger0) logic_level = LOW;
  /* else do not change */
  return logic_level;
}

uint8_t touchAWall(uint8_t sensor_reading, uint8_t motorHIGH) {
  uint8_t motorSpeed;
  if (sensor_reading == LOW){
    // white, set motor to max value
    motorSpeed = motorHIGH;  
  } else {
    // black, set motor left to min value
    motorSpeed = motorHIGH/4;  
  }
  return motorSpeed; 
}