// C++ code
//
#include <Bridge.h>
#include <BridgeClient.h>
#include <BridgeServer.h>

#define BTN1 2
#define MOTOR1_IN6 6
#define MOTOR1_IN7 7
#define ENABLE_1 5

#define BTN2 3
#define MOTOR2_IN9 9
#define MOTOR2_IN10 10
#define ENABLE_2 8

#define BTN3 4
#define MOTOR3_IN12 12
#define MOTOR3_IN13 13
#define ENABLE_3 11

#define BTN4 A5

#define INTERVAL 300
#define INTERVAL_MID 50

BridgeServer server;
int globalStateAll = 0;
int globalState_1 = 0;
int globalState_2 = 0;
int globalState_3 = 0;

unsigned long motor_1;
unsigned long motor_2;
unsigned long motor_3;

int state_right_1 = 0;
int state_left_1 = 0;
int state_stop_1 = 0;

int state_right_2 = 0;
int state_left_2 = 0;
int state_stop_2 = 0;

int state_right_3 = 0;
int state_left_3 = 0;
int state_stop_3 = 0;

void setup()
{
  pinMode(BTN1, INPUT);
  pinMode(MOTOR1_IN6, OUTPUT);
  pinMode(MOTOR1_IN7, OUTPUT);
  digitalWrite(ENABLE_1, HIGH);
  
  pinMode(BTN2, INPUT);
  pinMode(MOTOR2_IN9, OUTPUT);
  pinMode(MOTOR2_IN10, OUTPUT);
  digitalWrite(ENABLE_2, HIGH);
  
  pinMode(BTN3, INPUT);
  pinMode(MOTOR3_IN12, OUTPUT);
  pinMode(MOTOR3_IN13, OUTPUT);
  digitalWrite(ENABLE_3, HIGH);  
  
  pinMode(BTN4, INPUT);

  Bridge.begin();
  server.noListenOnLocalhost();
  server.begin();
}

void loop()
{
  checkIncomingRequests();

  int btn1_read = digitalRead(BTN1);
  int btn1 = btn1_read;
  int btn2_read = digitalRead(BTN2);
  int btn2 = btn2_read;
  int btn3_read = digitalRead(BTN3);
  int btn3 = btn3_read;
  int btn4_read = digitalRead(BTN4);
  int btn4 = btn4_read;
  bool motor1_isNotActive = state_right_1 == 0 && state_stop_1 == 0 && state_left_1 == 0;
  bool motor2_isNotActive = state_right_2 == 0 && state_stop_2 == 0 && state_left_2 == 0;
  bool motor3_isNotActive = state_right_3 == 0 && state_stop_3 == 0 && state_left_3 == 0;

  if((btn1 || globalState_1) && !btn4 && motor1_isNotActive)
  {
    globalState_1 = 0;
    right(MOTOR1_IN6, MOTOR1_IN7);
    motor_1 = millis();
    state_right_1 = 1;
  }
  if(millis() - motor_1 >= INTERVAL && state_right_1 == 1) {
    stop(MOTOR1_IN6, MOTOR1_IN7);
    motor_1 = millis();
    state_right_1 = 0;
    state_stop_1 = 1;
  }
  if(millis() - motor_1 >= INTERVAL_MID && state_stop_1 == 1) {
    left(MOTOR1_IN6, MOTOR1_IN7);	
    motor_1 = millis();
    state_stop_1 = 0;
    state_left_1 = 1;
  }
  if(millis() - motor_1 >= INTERVAL && state_left_1 == 1) {
    stop(MOTOR1_IN6, MOTOR1_IN7);
    state_right_1 = 0;
    state_left_1 = 0;
    state_stop_1 = 0;
  }
  
 if((btn2 || globalState_2) && !btn4 && motor2_isNotActive)
  {
    globalState_2 = 0;
    right(MOTOR2_IN9, MOTOR2_IN10);
    motor_2 = millis();
    state_right_2 = 1;
  }
  if(millis() - motor_2 >= INTERVAL && state_right_2 == 1) {
    stop(MOTOR2_IN9, MOTOR2_IN10);
    motor_2 = millis();
    state_right_2 = 0;
    state_stop_2 = 1;
  }
  if(millis() - motor_2 >= INTERVAL_MID && state_stop_2 == 1) {
    left(MOTOR2_IN9, MOTOR2_IN10);	
    motor_2 = millis();
    state_stop_2 = 0;
    state_left_2 = 1;
  }
  if(millis() - motor_2 >= INTERVAL && state_left_2 == 1) {
    stop(MOTOR2_IN9, MOTOR2_IN10);
    state_right_2 = 0;
    state_left_2 = 0;
    state_stop_2 = 0;
  }
  
  if((btn3 || globalState_3) && !btn4 && motor3_isNotActive)
  {
    globalState_3 = 0;
    right(MOTOR3_IN12, MOTOR3_IN13);
    motor_3 = millis();
    state_right_3 = 1;
  }
  if(millis() - motor_3 >= INTERVAL && state_right_3 == 1) {
    stop(MOTOR3_IN12, MOTOR3_IN13);
    motor_3 = millis();
    state_right_3 = 0;
    state_stop_3 = 1;
  }
  if(millis() - motor_3 >= INTERVAL_MID && state_stop_3 == 1) {
    left(MOTOR3_IN12, MOTOR3_IN13);	
    motor_3 = millis();
    state_stop_3 = 0;
    state_left_3 = 1;
  }
  if(millis() - motor_3 >= INTERVAL && state_left_3 == 1) {
    stop(MOTOR3_IN12, MOTOR3_IN13);
    state_right_3 = 0;
    state_left_3 = 0;
    state_stop_3 = 0;
  }
  
  if((btn4 || globalStateAll) && motor1_isNotActive && motor2_isNotActive && motor3_isNotActive)
  {
    globalStateAll = 0;
    right(MOTOR1_IN6, MOTOR1_IN7);
    motor_1 = millis();
    state_right_1 = 1;
    right(MOTOR2_IN9, MOTOR2_IN10);
    motor_2 = millis();
    state_right_2 = 1;
    right(MOTOR3_IN12, MOTOR3_IN13);
    motor_3 = millis();
    state_right_3 = 1;
  }
  
}

void checkIncomingRequests() {
  BridgeClient client = server.accept();
  if (client) {
    String command = client.readString();
    client.println(command);
    int index = command.indexOf("/");
    int length = command.length();
    if (index == -1) {
        String action = command.substring(0, length-2); // length -2 beacuse there is the \n
        if (action == "on") {
        globalStateAll = 1;
      }
    }
    if (index == 2) {
      String action = command.substring(0, index);
      if (action == "on") {
        String motor = command.substring(index+1, length-2); // length -2 beacuse there is the \n
        if(motor == "1") {
          globalState_1 = 1;
        } else if(motor == "2") {
          globalState_2 = 1;
        } else if(motor == "3") {
          globalState_3 = 1;
        }
      }
      
    }
    client.println(globalState_1);
    client.println(globalState_2);
    client.println(globalState_3);
    client.println(globalStateAll);

    client.stop();
  }
}

void right(int in1, int in2) {
	digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);  	
}

void left(int in1, int in2) {
	digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);  	
}

void stop(int in1, int in2) {
	digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);  	
}