#include <Servo.h>
#include "sensors.h"
#include "Wheels.h"//

#define TRIG_HIGH_DURATION   10 // from specification
#define TIME_OF_TRAVEL_TO_CM 58 // from specification

// ===================

Wheels w{};

Servo serwo;

int distanceSeenInCm(void) {
  digitalWrite(TRIG, HIGH);
  delay(TRIG_HIGH_DURATION);
  digitalWrite(TRIG, LOW);
  
  const unsigned long timeOfTravel = pulseIn(ECHO, HIGH);
  const unsigned int distanceInCm = timeOfTravel / TIME_OF_TRAVEL_TO_CM;

  return distanceInCm;
}

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
    
  Serial.begin(9600);

  serwo.attach(SERVO);
  
  w.attach(RIGHT_FRONT,
           RIGHT_BACK,
           RIGHT_PWM,
           LEFT_FRONT,
           LEFT_BACK,
           LEFT_PWM);

  // Give some time for board to initialize
  delay(1000);
  Serial.println("Car is ready");

  serwo.write(81);

  w.addMovement(
    (Movement) {
      .direction = FRONT,
      .value = 50
    }
  );

  w.addMovement(
    (Movement) {
      .direction = RIGHT,
      .value = 90
    }
  );

  w.addMovement(
    (Movement) {
      .direction = FRONT,
      .value = 50
    }
  );

  w.addMovement(
    (Movement) {
      .direction = LEFT,
      .value = 90
    }
  );
}

void loop() {
//  const int distanceInCm = distanceSeenInCm();
//
//  if (distanceInCm < 25) {
//    w.stop();
//    w.turnLeft(90);
//
//    delay(2500);
//
//    w.goForward(1000);
//  }
//
//  delay(10);

  if (w.leftDistanceToGoNegative()) {
    w.resetLeftDistanceToGo();
    w.stopLeft();
  }

  if (w.rightDistanceToGoNegative()) {
    w.resetRightDistanceToGo();
    w.stopRight();
  }

  if (w.awaitingNextMovement()) {
    const Movement nextMovementInstance = w.takeMovement();

    switch (nextMovementInstance.direction) {
      case FRONT: {
        w.goForward(nextMovementInstance.value);
      } break;
      case BACK: {
        w.goBack(nextMovementInstance.value);
      } break;
      case LEFT: {
        w.turnLeft(nextMovementInstance.value);
      } break;
      case RIGHT: {
        w.turnRight(nextMovementInstance.value);
      } break;
      case NONE:
      default:
      break;
    }
  }
}