#include <WiFi.h>
#include <WiFiUdp.h>
#include <Wire.h>

#define MOTORLATCH 12
#define MOTORCLK 4
#define MOTORENABLE 7
#define MOTORDATA 8
#define MOTOR3_A 5
#define MOTOR3_B 7
#define MOTOR4_A 0
#define MOTOR4_B 6
#define MOTOR3_PWM 6
#define MOTOR4_PWM 5
#define FORWARD 1
#define BACKWARD 2
#define BRAKE 3
#define RELEASE 4
String message = "";
bool isBlowingHorn = false;
bool isBraking = false;
bool isDrivingForward = false;
bool isDrivingBackward = false;
bool isTurningLeft = false;
bool isTurningRight = false;
int interval = 50;
int extraSpeedPerInterval = 5;
int slowDownPerInterval = 1;
int currentSpeed3 = 0;
int currentSpeed4 = 0;
int maxSpeed3 = 255;
int maxSpeed4 = 240;
long lastMillis = 0;
int outputSpeed3 = 0;
int outputSpeed4 = 0;

void setup() {
  Wire.begin(8);
  Wire.onReceive(receiveEvent);
  Serial.begin(9600);
}

void loop() {
  CalculateSpeed();
  ApplySpeed();
  String speed3 = "3:" + currentSpeed3;
  sendWireMessage(speed3);
  String speed4 = "4:" + currentSpeed4;
  sendWireMessage(speed4);
}

void CalculateSpeed() {
  if (isDrivingForward && !isDrivingBackward) {
    if(millis() - lastMillis > interval) {
      if (currentSpeed3 < maxSpeed3)
        currentSpeed3 += (int) ((maxSpeed3 / 100) * extraSpeedPerInterval);
      if(currentSpeed4 < maxSpeed4)
       currentSpeed4 += (int) ((maxSpeed4 / 100) * extraSpeedPerInterval);
    }
  } else if (isDrivingBackward && !isDrivingForward) {
    if(millis() - lastMillis > interval) {
      if (currentSpeed3 > -maxSpeed3)
        currentSpeed3 -= (int) ((maxSpeed3 / 100) * extraSpeedPerInterval);
      if(currentSpeed4 > -maxSpeed4) {
        currentSpeed4 -= (int) ((maxSpeed4 / 100) * extraSpeedPerInterval);
      }
    }
  } else {
    if (currentSpeed3  > 0)
      currentSpeed3 -= (int) (maxSpeed3 / 100 * slowDownPerInterval);
    if (currentSpeed3  < 0)
      currentSpeed3 += (int) ((maxSpeed3 / 100) * slowDownPerInterval);
    if (currentSpeed4  > 0)
      currentSpeed4 -= (int) ((maxSpeed4 / 100) * slowDownPerInterval);
    if (currentSpeed4  < 0)
      currentSpeed4 += (int) ((maxSpeed4 / 100) * slowDownPerInterval);
  }
  if(isTurningRight && !isTurningLeft) {
    if(millis() - lastMillis > interval)
      outputSpeed4 = currentSpeed4 / 2;
      outputSpeed3 = currentSpeed3 ;
  } else if (isTurningLeft && !isTurningRight) {
    if(millis() - lastMillis > interval)
      outputSpeed3 = currentSpeed3 / 2 ;
      outputSpeed4 = currentSpeed4 ;
  }  else{
    outputSpeed3 = currentSpeed3 ;
    outputSpeed4 = currentSpeed4 ;
  }

  ApplySpeed();
  if(isBraking) {
    motor(3, BRAKE, 0);
    motor(4, BRAKE, 0);
  }

  if(millis() - lastMillis > interval)
    lastMillis = millis();
}

void ApplySpeed() {
  if (outputSpeed3 > 0 )
    motor(3, FORWARD, outputSpeed3);
  else if (outputSpeed3 < 0 )
    motor(3, BACKWARD, -outputSpeed3);
  else
    motor(3, RELEASE, 0);
  if (outputSpeed4 > 0 )
    motor(4, FORWARD, outputSpeed4);
  else if (outputSpeed4 < 0 )
    motor(4, BACKWARD, -outputSpeed4);
  else
    motor(4, RELEASE, 0);
}

void sendWireMessage(String message) {
  Wire.beginTransmission(8);
  Wire.write('#');
  for(int i=0; i<message.length(); i++)
    Wire.write(message[i]);
  Wire.write('%');
  Wire.endTransmission();
}

void receiveEvent() {
  while (Wire.available() > 0) {
    char readChar = Wire.read();
    Serial.println(readChar);
    switch (readChar) {
      case '#':
        message = "";
        break;
      case '%':
        ProcessReceivedMessage(message);
        break;
      default:
        message += readChar;
    }
  }
}

void ProcessReceivedMessage(String message) {
  Serial.print("RECEIVED:");
  Serial.println(message);
  if(message.equals("DRIVE_FORWARD")) {
    isDrivingForward = true;
  } else if(message.equals("DRIVE_BACKWARD")) {
    isDrivingBackward = true;
  } else if(message.equals("STOP_DRIVING_FORWARD")) {
    isDrivingForward = false;
  } else if(message.equals("STOP_DRIVING_BACKWARD")) {
    isDrivingBackward = false;
  } else if(message.equals("TURN_LEFT")) {
    isTurningLeft = true;
  } else if(message.equals("TURN_RIGHT")) {
    isTurningRight = true;
  } else if(message.equals("STOP_TURNING_LEFT")) {
    isTurningLeft = false;
  } else if(message.equals("STOP_TURNING_RIGHT")) {
    isTurningRight = false;
  } else if(message.equals("START_BRAKING")) {
    isBraking = true;
  } else if(message.equals("STOP_BRAKING")) {
    isBraking = false;
  } else if(message.equals("BLOW_HORN")) {
    isBlowingHorn = true;
  } else if(message.equals("STOP_HORN")) {
    isBlowingHorn = false;
  } else if(message.equals("TOGGLE_LIGHTS")) {
  } else if(message.equals("REMOTE_CONTROL")) {
  }
}