#include <Arduino.h>
#include "driver/gpio.h"
#include "driver/dac.h"
#include "driver/adc.h"

#define motorA 32
#define motorB 33
#define steer_servo 25

const uint16_t pwm_freq_1 = 500;
const uint8_t pwm_freq_2 = 50;
const uint8_t pwm_res_1 = 12;
const uint8_t pwm_res_2 = 16;
const uint16_t forw_lim = 2194;
const uint16_t back_lim = 2000;
const uint8_t speed_delay = 5;
const uint8_t speed_step = 20;

uint32_t timer;

uint16_t reduction(uint16_t);
void engine(uint16_t);

void setup()
{
  pinMode(motorA, OUTPUT);
  pinMode(motorB, OUTPUT);
  pinMode(steer_servo, OUTPUT);
  pinMode(34, OUTPUT);

  ledcAttach(motorA, pwm_freq_1, pwm_res_1);
  ledcAttach(motorB, pwm_freq_1, pwm_res_1);

  ledcAttach(steer_servo, pwm_freq_2, pwm_res_2);
  timer = millis();
  Serial.begin(115200);
}


void loop()
{
  uint16_t test = analogRead(34);
  uint16_t motor = reduction(test);
  engine(motor);
  Serial.println(motor);

}


uint16_t reduction(uint16_t speed)
{
  static uint16_t current_speed = 2047;
  if(speed > current_speed){
    if(millis()-timer > speed_delay && abs(current_speed - speed) > speed_step){
      current_speed += speed_step;
      timer = millis();
    }
  }
  else if(speed < current_speed){
    if(millis()-timer > speed_delay && abs(current_speed - speed) > speed_step){
      current_speed -= speed_step;
      timer = millis();
    }
  }
  return current_speed;
}

void engine(uint16_t rpm)
{
  if(rpm >= forw_lim){
    ledcWrite(motorA, rpm);
    ledcWrite(motorB, 0);
  }
  else if(rpm <= back_lim){
    ledcWrite(motorA, 0);
    ledcWrite(motorB, rpm);
  }
  else if(rpm > back_lim && rpm < forw_lim){
    ledcWrite(motorA, 0);
    ledcWrite(motorB, 0);
  }
}