#include <Arduino.h>

/*
  ESP32 PCA9685 Servo Control
  esp32-pca9685.ino
  Driving multiple servo motors with ESP32 and PCA9685 PWM module
  Use I2C Bus

  DroneBot Workshop 2020
  https://dronebotworkshop.com
*/

// Include Wire Library for I2C
#include <Wire.h>

// Include Adafruit PCA9685 Servo Library
#include <Adafruit_PWMServoDriver.h>

// Creat object to represent PCA9685 at default I2C address
Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver(0x40);

// Define maximum and minimum number of "ticks" for the servo motors
// Range from 0 to 4095
// This determines the pulse width

const int potPin = 34;
const int buttonPin = 4; // the number of the pushbutton pin
const int ledPin = 9;    // the number of the LED pin
#define timeSeconds 2
unsigned long now = millis();
unsigned long lastTrigger = 0;
boolean startTimer = false;

int last_potValue = 0;
int potValue = 0;
int pwm = 0;

#define SERVOMIN 20  // 80  // Minimum value
#define SERVOMAX 600 // Maximum value

// Define servo motor connections (0-indexed)
#define MAX_MOTORS 2
u_int8_t last_motor = 0;
u_int8_t active_motor = 0;
typedef enum
{
  forward,
  backward
} direction;

// Checks if motion was detected, sets LED HIGH and starts a timer
void IRAM_ATTR detectsButtonPress()
{
  digitalWrite(ledPin, HIGH);
  startTimer = true;
  lastTrigger = millis();
}

void move_180(uint16_t servopin, direction dir)
{
  int pwm = 0;
  if (dir == direction::forward)
  {
    // Move Motor 0 from 0 to 180 degrees
    for (int posDegrees = 0; posDegrees <= 180; posDegrees++)
    {
      // Determine PWM pulse width
      pwm = map(posDegrees, 0, 180, SERVOMIN, SERVOMAX);
      // Write to PCA9685
      pca9685.setPWM(servopin, 0, pwm);
      // Print to serial monitor
      Serial.print("Motor ");
      Serial.print(active_motor);
      Serial.print(" = ");
      Serial.println(posDegrees);
      delay(30);
    }
  }
  else if (dir == direction::backward)
  {
    for (int posDegrees = 180; posDegrees >= 0; posDegrees--)
    {
      // Determine PWM pulse width
      pwm = map(posDegrees, 0, 180, SERVOMIN, SERVOMAX);
      // Write to PCA9685
      pca9685.setPWM(servopin, 0, pwm);
      // Print to serial monitor
      Serial.print("Motor ");
      Serial.print(active_motor);
      Serial.print(" = ");
      Serial.println(posDegrees);
      delay(30);
    }
  }
}

void move_by_poti()
{

  potValue = analogRead(potPin); // reads the value of the potentiometer (value between 0 and 1023)
  if (last_potValue != potValue)
  {
    last_potValue = potValue;
    // Serial.println(potValue);
    // val = map(val, 0, 1023, 0, 180); // scale it to use it with the servo (value between 0 and 180)
    pwm = map(potValue, 0, 180, SERVOMIN, SERVOMAX);
    // Write to PCA9685
    pca9685.setPWM(active_motor, 0, pwm);
    // Print to serial monitor
    Serial.print("Motor ");
    Serial.print(active_motor);
    Serial.print(" = ");
    Serial.print(potValue);
    Serial.print(" / ");
    Serial.println(pwm);
    delay(30);
  }
}

void handle_motor_btn()
{
  // Turn off the LED after the number of seconds defined in the timeSeconds variable
  now = millis();
  if (startTimer && (now - lastTrigger > (timeSeconds * 1000)))
  {
    last_motor = active_motor;
    if (++active_motor >= 3)
      active_motor = 0;
    Serial.print("Motor ");
    Serial.print(active_motor);
    Serial.println(" selected");
    digitalWrite(ledPin, LOW);
    startTimer = false;
  }
}

void setup()
{

  // Serial monitor setup
  Serial.begin(115200);

  pinMode(buttonPin, INPUT);
  attachInterrupt(digitalPinToInterrupt(buttonPin), detectsButtonPress, RISING);
  pinMode(potPin, INPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);

  // Print to monitor
  Serial.println("PCA9685 Servo Test");

  // Initialize PCA9685
  pca9685.begin();

  // Set PWM Frequency to 50Hz
  pca9685.setPWMFreq(50);
}

void loop()
{

  handle_motor_btn();

  move_by_poti();

  // // Move Motor 0 from 0 to 180 degrees
  // move_180(SER0, direction::forward);
  // // Move Motor 1 from 0 to 180 degrees
  // move_180(SER1, direction::forward);
  // // Move Motor 2 from 0 to 180 degrees
  // move_180(SER2, direction::forward);

  // // Move Motor 0 from 180 to 0 degrees
  // move_180(SER0, direction::backward);
  // // Move Motor 1 from 0 to 180 degrees
  // move_180(SER1, direction::backward);
  // // Move Motor 1 from 0 to 180 degrees
  // move_180(SER2, direction::backward);
}