#include <Servo.h>

// common settings
#define DEBUG 0

// steer wheel settings
#define STEER_MIN_W 615 //90 degrees CW
#define STEER_MAX_W 2450 //90 degrees CCW
#define STEER_PIN 9

// BW sensors settings
#define SENSOR_THRESHOLD 750
byte sensorPins[6] = {A2, A3, A4, A5, A0, A1};
byte sensorLedPin = 4;
//whole sensors coordinates: -9 -7 -5 -3 -1 1 3 5 7 9
//used sensors coordinates: -7 -5 -3 3 5 7
int minCoord[6] = {-8, -6, -4, 0, 4, 6};
int maxCoord[6] = {-6, -4, 0, 4, 6, 8};

// back wheels settings
#define POWER 20 //percentage
byte wheelsPins[4] = {5, 7, 6, 8};

Servo steer;

void setup() {
  steer.attach(STEER_PIN, STEER_MIN_W, STEER_MAX_W);
  pinMode(2, INPUT);
  pinMode(sensorLedPin, OUTPUT);
  digitalWrite(sensorLedPin, HIGH);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  analogReference(DEFAULT);
#if DEBUG
  Serial.begin(9600);
#endif
}

int prevAngle = 90;

int loopCount = 0;
int state = 1;
void loop() {
  if (state == 0) {
    if (digitalRead(2))
      state = 1;
  } else if (state == 1) {
    delay(2000);
    state = 2;
  } else if (state == 2) {
  loopCount++;

  int vals[6];
  for (byte i = 0; i < 6; i++) {
    pinMode(sensorPins[i], OUTPUT);
    analogWrite(sensorPins[i], 1023);
    delayMicroseconds(500);
    pinMode(sensorPins[i], INPUT);
    delayMicroseconds(500);
    vals[i] = analogRead(sensorPins[i]);
  }

  int thresholds[6] = {SENSOR_THRESHOLD, SENSOR_THRESHOLD, SENSOR_THRESHOLD, SENSOR_THRESHOLD, SENSOR_THRESHOLD, SENSOR_THRESHOLD};
  byte mini, maxi;
  mini = 5;
  maxi = 0;
  for (byte i = 0; i < 6; i++) {
    if (vals[i] > thresholds[i]) {
      if (mini > i) mini = i;
      if (maxi < i) maxi = i;
    }
  }

  int resultCoordinate = minCoord[mini] + maxCoord[maxi];
  int angle;
  if (prevAngle < 90)
    angle = 55;
  else if (prevAngle > 90)
    angle = 115;
  else
    angle = 90;
  if (mini <= maxi) {
    angle = prevAngle - resultCoordinate / 2;
    if (angle > 115) angle = 115;
    if (angle < 55) angle = 55;
  }

  steer.write(angle + 5);
  go(angle, POWER);
  if (angle != prevAngle) {
    digitalWrite(13, HIGH);
  } else
    digitalWrite(13, LOW);

#if DEBUG
  if (loopCount % 15 == 0) {
    Serial.print("Values:");
    for (byte i = 0; i < 6; i++) {
      Serial.print(" ");
      Serial.print(vals[i], DEC);
    }
    Serial.println();
    Serial.print("Result coordinate: ");
    Serial.println(resultCoordinate, DEC);
    Serial.print("Angle: ");
    Serial.print(angle, DEC);
    Serial.print(" ");
    Serial.println(angle - prevAngle, DEC);
  }
#endif

  prevAngle = angle;
  delay(5);
  }
}

void wheel(int power, byte dirPin, byte pwmPin) {
  digitalWrite(dirPin, power < 0 ? HIGH : LOW);
  analogWrite(pwmPin, map(abs(power), 0, 100, 0, 255));
}

void wheels(int powerLeft, int powerRight) {
  wheel(powerLeft, wheelsPins[1], wheelsPins[0]);
  wheel(powerRight, wheelsPins[3], wheelsPins[2]);
}

void go(int angle, int power) {
  if (angle == 90) {
    wheels(power, power);
  } else if (angle < 90) {
    wheels(power, map(angle, 0, 90, 0, power));
  } else {
    wheels(map(angle, 90, 180, power, 0), power);
  }
}

