#include <SparkFun_TB6612.h>
#include <ESP32Servo.h>

#define AIN1 D5
#define BIN1 D6
#define AIN2 D4
#define BIN2 D7
#define PWMA D3
#define PWMB D8
#define STBY D2

#define CH1_LR A0
#define CH2_FWD A1
#define CH6_BTTN A7

Servo servo1;

const int fwdDirectionA = 1;
const int fwdDirectionB = 1;

Motor motor1 = Motor(AIN1, AIN2, PWMA, fwdDirectionA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, fwdDirectionB, STBY);

long hori_pulse = 0;
long vert_pulse = 0;
long bttn_pulse = 0;

boolean valueCheck(long signal_length) {
  if (signal_length >= 999 && signal_length <= 2000) {
    return true;
  } else {
    return false;
  }
}

void setup()
{
  Serial.begin(115200);
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  servo1.setPeriodHertz(50);
  servo1.attach(D10);
  servo1.write(90);

  pinMode(CH1_LR, INPUT_PULLUP);
  pinMode(CH2_FWD, INPUT_PULLUP);
  pinMode(CH6_BTTN, INPUT_PULLUP);

  motor1.brake();
  motor2.brake();
}

void loop()
{
  hori_pulse = pulseIn(CH1_LR, HIGH);
  vert_pulse = pulseIn(CH2_FWD, HIGH);
  bttn_pulse = pulseIn(CH6_BTTN, HIGH);

  if (valueCheck(hori_pulse) && valueCheck(vert_pulse)) {
    int speed = map(vert_pulse, 999, 2000, -255, 255);
    int direction = map(hori_pulse, 999, 2000, -255, 255);

    int left_speed  = constrain(speed + direction, -255, 255);
    int right_speed = constrain(speed - direction, -255, 255);
    motor1.drive(right_speed);
    motor2.drive(left_speed);
  }

  if (valueCheck(bttn_pulse)) {
    if (bttn_pulse >= 1500) {
      servo1.write(0);
    } else {
      servo1.write(90);
    }
  }
}