#include <Arduino.h>

#include "config.h"

#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>


// Functions
void setMotorDirection(uint8_t motor ,uint8_t direction = FORWARD);
void setMotorSpeed(uint8_t motor, uint8_t speed);

void setup() {
  Serial.begin(115200);      // make sure your Serial Monitor is also set at this baud rate.
  Dabble.begin("Kleiax-Rover");       //set bluetooth name of your device

  //SETUP PINS
  pinMode(M_DIR_11, OUTPUT);
  pinMode(M_DIR_12, OUTPUT);
  pinMode(M_DIR_21, OUTPUT);
  pinMode(M_DIR_22, OUTPUT);

  digitalWrite(M_DIR_11, LOW);
  digitalWrite(M_DIR_12, LOW);
  digitalWrite(M_DIR_21, LOW);
  digitalWrite(M_DIR_22, LOW);

  ledcSetup(PWM_CHANNEL_M1, PWM_FREQ, PWM_RES);
  ledcSetup(PWM_CHANNEL_M2, PWM_FREQ, PWM_RES);
  ledcAttachPin(M_PWM_1, PWM_CHANNEL_M1);
  ledcAttachPin(M_PWM_2, PWM_CHANNEL_M2);
  ledcWrite(M_PWM_1, 0);
  ledcWrite(M_PWM_2, 0);


  //Start speed
  setMotorSpeed(BOTH_MOTOR, 255);

}

void loop() {
  Dabble.processInput();             //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile.
  //Serial.print("KeyPressed: ");
  if (GamePad.isUpPressed()) {
    Serial.print("Up\n");
    setMotorDirection(BOTH_MOTOR, FORWARD);
  } else if (GamePad.isDownPressed()) {
    Serial.print("Down\n");
    setMotorDirection(BOTH_MOTOR, BACKWARD);
  } else if (GamePad.isLeftPressed()) {
    Serial.print("Left\n");
    setMotorDirection(LEFT_MOTOR, BACKWARD);
    setMotorDirection(RIGHT_MOTOR, FORWARD);
  } else if (GamePad.isRightPressed()) {
    Serial.print("Right\n");
    setMotorDirection(LEFT_MOTOR, FORWARD);
    setMotorDirection(RIGHT_MOTOR, BACKWARD);

  } else if (GamePad.isSquarePressed()) {
    setMotorDirection(LEFT_MOTOR, FORWARD);
    Serial.print("1\n");
  } else if (GamePad.isTrianglePressed()) {
    setMotorDirection(LEFT_MOTOR, BACKWARD);
    Serial.print("2\n");
  } else if (GamePad.isCirclePressed()) {
    setMotorDirection(RIGHT_MOTOR, FORWARD);
    Serial.print("3\n");
  } else if (GamePad.isCrossPressed()) {
    setMotorDirection(RIGHT_MOTOR, BACKWARD);
    Serial.print("4\n");
  



  } else {
    setMotorDirection(BOTH_MOTOR, STOP);
  }
}

void setMotorDirection(uint8_t motor ,uint8_t direction) {
  if (motor == LEFT_MOTOR) {
    if (direction == FORWARD) {
      digitalWrite(M_DIR_11, HIGH);
      digitalWrite(M_DIR_12, LOW);
    } else if (direction == BACKWARD) {
      digitalWrite(M_DIR_11, LOW);
      digitalWrite(M_DIR_12, HIGH);
    } else if (direction == STOP) {
      digitalWrite(M_DIR_11, LOW);
      digitalWrite(M_DIR_12, LOW);
    } else {
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, LOW);
      Serial.print("setDirection: undefined direction\n");
    }
  } else if (motor == RIGHT_MOTOR) {
    if (direction == FORWARD) {
      digitalWrite(M_DIR_21, HIGH);
      digitalWrite(M_DIR_22, LOW);
    } else if (direction == BACKWARD) {
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, HIGH);
    } else if (direction == STOP) {
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, LOW);
    } else {
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, LOW);
      Serial.print("setDirection: undefined direction\n");
    }
  } else if (motor == BOTH_MOTOR) {
    if (direction == FORWARD) {
      digitalWrite(M_DIR_11, HIGH);
      digitalWrite(M_DIR_12, LOW);
      digitalWrite(M_DIR_21, HIGH);
      digitalWrite(M_DIR_22, LOW);
    } else if (direction == BACKWARD) {
      digitalWrite(M_DIR_11, LOW);
      digitalWrite(M_DIR_12, HIGH);
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, HIGH);
    } else if (direction == STOP) {
      digitalWrite(M_DIR_11, LOW);
      digitalWrite(M_DIR_12, LOW);
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, LOW);
    }else {
      digitalWrite(M_DIR_11, LOW);
      digitalWrite(M_DIR_12, LOW);
      digitalWrite(M_DIR_21, LOW);
      digitalWrite(M_DIR_22, LOW);
      Serial.print("setDirection: undefined direction\n");
    }
  } else {
    Serial.print("setDirection: undefined motor\n");
  }
}

void setMotorSpeed(uint8_t motor, uint8_t speed) {
  if (motor == LEFT_MOTOR) {
    ledcWrite(PWM_CHANNEL_M1, speed);
  } else if (motor == RIGHT_MOTOR) {
    ledcWrite(PWM_CHANNEL_M2, speed);
  } else if (motor == BOTH_MOTOR) {
    ledcWrite(PWM_CHANNEL_M1, speed);
    ledcWrite(PWM_CHANNEL_M2, speed);
  } else {
    ledcWrite(PWM_CHANNEL_M1, 0);
    ledcWrite(PWM_CHANNEL_M2, 0);
    Serial.print("setSpeed: undefined motor \n");
  }
}