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

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

// ======================= rc Wiring ===========================
#define CH1_LR A0 //Channel1
#define CH2_FWD A1 //Channel2
#define CH3_TPBTTN A2 //Channel3
#define CH4_BTMBTTN A3 //Channel4
// =============================================================

// ===================== Servo Object  ========================
Servo servo1;
// =============================================================

// ===================== Motor Objects ========================
const int fwdDirectionA = 1;
const int fwdDirectionB = 1;

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

// ============= GLOBAL VARIABLES ==============================

// ==================== Servo ==================================
int angle1 = 90;

// ==================== RC Controller ==========================
long hori_pulse = 0;
long vert_pulse = 0;
long top_pulse = 0;
long bottom_pulse = 0;
long cur_top_pulse = 0;
long cur_bottom_pulse = 0;
int control_mode = 1;


// Check if the input value it is in thge acceptable range for the input signal that
// comes from the RC receiver. Acceptable values are between 999 and 2000.
boolean valueCheck(long signal_length){
  if(signal_length >= 999 && signal_length <= 2000 ){
    return true;
  }else{
    return false;
  }
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


void setup()
{
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

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

  //Set pins as input
  pinMode(CH1_LR, INPUT_PULLUP);
  pinMode(CH2_FWD, INPUT_PULLUP);
  pinMode(CH3_TPBTTN, INPUT_PULLUP);
  pinMode(CH4_BTMBTTN, INPUT_PULLUP);

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

void loop()
{
  //Reads signal from RC controller, specifically when the signal is high
  hori_pulse = pulseIn(CH1_LR,HIGH);
  vert_pulse = pulseIn(CH2_FWD,HIGH);
  cur_top_pulse = pulseIn(CH3_TPBTTN,HIGH);
  cur_bottom_pulse = pulseIn(CH4_BTMBTTN,HIGH);

  if(abs(cur_top_pulse - top_pulse) > 500 && valueCheck(cur_top_pulse)){
    control_mode = 1;
  }else if(abs(cur_bottom_pulse - bottom_pulse) > 500 && valueCheck(cur_bottom_pulse)){
    control_mode = 2;
  }

  top_pulse = cur_top_pulse;
  bottom_pulse = cur_bottom_pulse;

if (control_mode == 1){
    //Check if signal received is within proper range, then convert to a readable speed
    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);

      // Set exact speed for each motor, accounting for direction
      int left_speed  = constrain(speed + direction, -255, 255);
      int right_speed = constrain(speed - direction, -255, 255);
      motor1.drive(right_speed);
      motor2.drive(left_speed);
    }
  }else if(control_mode == 2){
    if(valueCheck(vert_pulse)){
      angle1 += int(mapFloat(vert_pulse,999,2000,-5,5));
      angle1 = constrain(angle1, 0, 180);
      servo1.write(angle1);
    }
  }


}