#include <Servo.h>
#include <Thread.h>

#define VRX_PIN A0  // left/right
#define VRY_PIN A1  // drive
#define P_PIN A2
#define SERVO_X_PIN 13  // Arduino pin connected to Servo motor 1
#define DRIVE_PIN 6
#define L_LED_PIN 3
#define R_LED_PIN 2
#define BUZZER_PIN 4
#define HWL_BT_PIN A3  // Hazard Warning Light Switch
#define HORN_BT_PIN A4
#define HORN_BUZZ_PIN 5

Servo xServo;  // create servo object to control a servo 1
const int turnSignalDelay = 100;
const int range = 25;

//Turn signal Thread
Thread turnSignalThread = Thread();
//Drive Thread
Thread driveThread = Thread();
//Horn Thread
Thread hornThread = Thread();

int xValue = 0;
int acceleratorValue = 0;
int pValue = 0;
int zero_point = 0;
int xAngle = 0;
int hornBtnValue = 0;
bool leftTurnStatus = false;
bool rightTurnStatus = false;
bool hwlButtonState = false;
bool isDrive = false;
bool isHorn = false;

// callback for turnSignalThread
void turnSignalCallback() {
  if (hwlButtonState || leftTurnStatus || rightTurnStatus) {
    tone(BUZZER_PIN, 500, 20);
    if (hwlButtonState || leftTurnStatus) {
      digitalWrite(L_LED_PIN, HIGH);
    }
    if (hwlButtonState || rightTurnStatus) {
      digitalWrite(R_LED_PIN, HIGH);
    }
    delay(turnSignalDelay);
    tone(BUZZER_PIN, 50, 30);
  } else {
    noTone(BUZZER_PIN);
  }

  digitalWrite(L_LED_PIN, LOW);
  digitalWrite(R_LED_PIN, LOW);
}

void driveCallback() {
  if (isDrive) {
      Serial.print(">>>>>>>>>>>>>>>>>>>> GO GO GO <<<<<<<<<<<<<<<<<<<<<<<<<<<<");
      Serial.println();
    digitalWrite(DRIVE_PIN, LOW);
  } else {
    digitalWrite(DRIVE_PIN, HIGH);
  }
}

void hornCallback() {
  if (isHorn) {
    tone(HORN_BUZZ_PIN, 440);
  } else {
    noTone(HORN_BUZZ_PIN);
  }
}


void setup() {
  Serial.begin(9600);
  xServo.attach(SERVO_X_PIN);

  pinMode(DRIVE_PIN, OUTPUT);
  digitalWrite(DRIVE_PIN, HIGH);

  pinMode(L_LED_PIN, OUTPUT);
  digitalWrite(L_LED_PIN, LOW);

  pinMode(R_LED_PIN, OUTPUT);
  digitalWrite(R_LED_PIN, LOW);

  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(HORN_BUZZ_PIN, OUTPUT);

  pinMode(HWL_BT_PIN, INPUT);

  turnSignalThread.onRun(turnSignalCallback);
  turnSignalThread.setInterval(500);

  driveThread.onRun(driveCallback);
  driveThread.setInterval(1);

  hornThread.onRun(hornCallback);
  hornThread.setInterval(10);
}

void setUpThreads() {
  if (turnSignalThread.shouldRun()) {
    turnSignalThread.run();
  }

  if (driveThread.shouldRun()) {
    driveThread.run();
  }

  if (hornThread.shouldRun()) {
    hornThread.run();
  }
}

void loop() {

  setUpThreads();

  xValue = analogRead(VRX_PIN);
  acceleratorValue = (1024 - analogRead(VRY_PIN)) / 10;
  pValue = analogRead(P_PIN);
  zero_point = map(pValue, 0, 1023, 60, 120);

  xAngle = map(xValue, 0, 1023, zero_point - range, zero_point + range);

  isDrive = acceleratorValue > 60;

  if (abs(xAngle) > 2) {
    xServo.write(xAngle);
  }
  rightTurnStatus = xAngle > zero_point + 2;
  leftTurnStatus = xAngle < zero_point - 2;

  hwlButtonState = digitalRead(HWL_BT_PIN);

  hornBtnValue = analogRead(HORN_BT_PIN);
  isHorn = hornBtnValue < 50;

  log();

  // delay(1);
}

void log() {
  // print data to Serial Monitor on Arduino IDE
  Serial.print("Horisontal: ");
  Serial.print(xValue);
  Serial.print(", acceleratorValue: ");
  Serial.print(acceleratorValue);
  Serial.print(", P: ");
  Serial.print(pValue);
  Serial.print(" => zeroP: ");
  Serial.print(zero_point);
  Serial.print("°, xAngle diff: ");
  Serial.print(xAngle - zero_point);
  Serial.print("°, isDrive: ");
  Serial.print(isDrive);
  Serial.print(", hwlButtonState: ");
  Serial.print(hwlButtonState);
  Serial.print(", leftTurnStatus: ");
  Serial.print(leftTurnStatus);
  Serial.print(", rightTurnStatus: ");
  Serial.print(rightTurnStatus);
  Serial.print(", Horn/hornBtnValue: ");
  Serial.print(isHorn);
  Serial.print("/");
  Serial.print(hornBtnValue);
  Serial.println();
}