#include <BaseCar.h>

// log
const boolean VERBOSE = true;
const int SERIAL_SPEED = 9600;

// for Car
const int MOTO1_PIN = 10;
const int MOTO2_PIN = 11;
const int I11_PIN = 6;
const int I12_PIN = 7;
const int I21_PIN = 8;
const int I22_PIN = 9;

// for Trace Sensor
const int LEFT_PIN = 3;
const int RIGHT_PIN = 4;

// speed
const int SPEED_VAL = 150;

// delay
const int DELAY_TIME = 300;

// car control
BaseCar car(MOTO1_PIN, MOTO2_PIN, I11_PIN, I12_PIN, I21_PIN, I22_PIN);

const int adj = -19;

void setup() {
  pinMode(LEFT_PIN, INPUT);
  pinMode(RIGHT_PIN, INPUT);
  if (VERBOSE) {
    Serial.begin(SERIAL_SPEED);
  }
}

void loop() {
  int left_flag = digitalRead(LEFT_PIN);
  int right_flag = digitalRead(RIGHT_PIN);
  if (VERBOSE) {
    Serial.print("left=");
    Serial.print(left_flag);
    Serial.print(", right=");
    Serial.println(right_flag);
  }
  if ((left_flag == LOW) && (right_flag == LOW)) {
    // on the way
    car.forward(SPEED_VAL, adj);
  } else if ((left_flag == HIGH) && (right_flag == HIGH)) {
    // no way, stop
    car.standBy();
  } else if (left_flag == HIGH) {
    // turn left
    car.rotateLeft(SPEED_VAL, adj);
  } else if (right_flag == HIGH) {
    // turn right
    car.rotateRight(SPEED_VAL, adj);
  }
}