// PCA9685 with Millis
// https://forum.arduino.cc/t/pca9685-with-millis/1299343


/* PCA9685ServoV4
    26/12/23

  This sketch is controlling 2 servos, one on pin 0 the 2nd on pin 3

   PCA9685 Board with MG90 servo



  SDA/SCL connections

  Arduino UNO: A4 (SDA), A5 (SCL)
  Arduino Mega 2560: 20 (SDA), 21 (SCL)
  ESP32: 21(SDA), 22 (SCL)

*/


#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
int IRhorizAllowedPin = 8;
int IRhorizCountPin = 9;
int obstaclePin = 10;
const long eventTime_1_LDR = 1000; //in ms

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

enum { Stopped, Pause_before_Left, Pan_Left, Stop_Going_Left, Too_Far_Left, Pause_before_Right, Pan_Right, Stop_Going_Right, Too_Far_Right };

const char * StateStr [] =  { "Stopped", "Pause_before_Left", "Pan_Left", "Stop_Going_Left", "Too_Far_Left",
                              "Pause_before_Right", "Pan_Right", "Stop_Going_Right", "Too_Far_Right"
                            };


uint32_t currentMillis;//Holds the current board time in milliseconds


#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

#define BLACK LOW
#define WHITE HIGH


// our servos
const int servo0 = 0;
const int servo3 = 3;
// uint32_t previousTime_1 = 0;
// uint32_t previousTime_2 = 0;

void setServoPos(int servo, int pos) {
  //This first bit of code makes sure we are not trying to set the servo outside of limits
  int sendPos;
  if (pos > 179) {
    pos = 179;
  }
  if (pos < 0) {
    pos = 0;
  }
  sendPos = USMIN + ((USMAX - USMIN) / 180 * pos);
  if (-1 < servo && servo < 16) { //only try to move valid servo addresses
    pwm.writeMicroseconds(servo, sendPos);

    // uint32_t previousTime_1 = 0;
    // uint32_t previousTime_2 = 0;

  }
}

void setup() {
  Serial.begin(115200);
  //Serial.println("PCA9685ServoV4");
  pinMode(signalPin, INPUT);// set signalPin as input

  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  delay(10);
}

void loop() {

  /* Updates frequently */
  uint32_t currentTime = millis();

  static uint8_t state = Stopped;
  uint8_t IRhorizAllowed = digitalRead(IRhorizallowedPin);
  uint8_t IRhorizCount = digitalRead(IRhorizCountPin);
  ldxfkjg

  // if ( currentTime - previousTime_1 >= 5000UL) {
  // setServoPos(servo0,179);//0 Degrees
  // Update the timing for the next event*/
  // previousTime_1 = currentTime;}

  // if ( currentTime - previousTime_1>= 500UL) {
  // setServoPos(servo3,0);//0 Degrees
  // Update the timing for the next event*/
  // previousTime_1 = currentTime;}



  // if(currentMillis > 7000){setServoPos(servo0,0);}
  // if(currentMillis > 10000){setServoPos(servo0,179);}
  // currentMillis = millis();
  // setServoPos(servo0,90);//90 degrees
  // delay(500);
  // setServoPos(servo0,);//179 degrees
  // delay(6000);
  // setServoPos(servo0,90);//90 degrees
  // delay(500);


  int detect = digitalRead(signalPin);// read obstacle status and store it into "detect"
  if (detect == LOW) {

    Serial.println("Obastacle on the way");
  } else {

    Serial.println("All clear");
  }
  {
    // setServoPos(servo0,0);//0 Degrees
    // delay(5000);
    // setServoPos(servo0,90);//90 degrees
    // delay(2000);
    // setServoPos(servo0,179);//179 degrees
    // delay(5000);}

  }
}


// -----------------------------------------------------------------------------
byte PinButs [] = { A1, A2, A3 };
byte butState [sizeof(PinButs)];

enum { B_0, B_1, B_2 };

// -------------------------------------
const int NoBut = -1;
int chkButtons ()
{
  for (unsigned n = 0; n < sizeof(PinButs); n++)  {
    byte but = digitalRead (PinButs [n]);
    if (butState [n] != but)  {
      butState [n] = but;
      delay (10);     // debounce
      if (LOW == but)
        return n;
    }
  }
  return NoBut;
}

// -----------------------------------------------------------------------------
byte PinLeds [] = { 9, 10, 11, 12 };

enum { L_0, L_1, L_2, L_3 };
enum { Off = HIGH, On = LOW };

// -------------------------------------
void ledSet (int idx )
{
  for (unsigned n = 0; n < sizeof(PinLeds); n++)
    digitalWrite (PinLeds [n], Off);

  digitalWrite (PinLeds [idx], On);
}

// -------------------------------------
void ledBlink ( int idx, uint32_t msecPeriod )
{
  static uint32_t msec0;
  uint32_t msec = millis ();

  if (msec - msec0 >= msecPeriod)  {
    msec0 += msecPeriod;
    digitalWrite (PinLeds [idx], !  digitalRead (PinLeds [idx]));
  }
}

// -----------------------------------------------------------------------------

enum { Stopped, Pan_Left, Pan_Right, Too_Far_Left, Too_Far_Right, State_5 };

const char * StateStr [] =  { "Stopped", "Pan_Left", "Pan_Right", "Too_Far_Left", "Too_Far_Right", "State_5" };

int state;

// -------------------------------------
void stateMach (int stim)
{
  switch (state)  {
    case Stopped:
      state = Pan_Left;
      ledSet (L_1);
      break;

    case Pan_Left:
      if (B_1 == stim)  {
        state = Pan_Right;
        ledSet (L_2);
      }
      else if (B_2 == stim) {
        state = Too_Far_Left;
        ledSet (L_3);
      }
      break;

    case Pan_Right:
      if (B_0 == stim)  {
        state = Pan_Left;
        ledSet (L_1);
      }
      else if (B_1 == stim) {
        state = Pan_Right;
        ledSet (L_2);
      }
      else if (B_2 == stim)
        state = Too_Far_Right;
      break;

    case Too_Far_Left:
      if (B_0 == stim)  {
        state = Pan_Left;
        ledSet (L_1);
      }
      else if (B_1 == stim) {
        state = Pan_Right;
        ledSet (L_2);
      }
      else if (B_2 == stim)
        state = State_5;
      break;

    case Too_Far_Right:
      ledBlink (L_0, 500);

      if (B_0 == stim)  {
        state = Pan_Left;
        ledSet (L_1);
      }
      else if (B_1 == stim)  {
        state = Pan_Right;
        ledSet (L_2);
      }
      else if (B_2 == stim)  {
        state = Too_Far_Left;
        ledSet (L_3);
      }
      break;

    case State_5:
      ledBlink (L_0, 100);

      if (B_0 == stim)  {
        state = Pan_Left;
        ledSet (L_1);
      }
      break;
  }

  if (NoBut != stim)
    Serial.println (StateStr [state]);
}

// -----------------------------------------------------------------------------
void loop (void)
{
  stateMach (chkButtons ());
}

// -----------------------------------------------------------------------------
void setup ()
{
  Serial.begin (115200);

  for (unsigned n = 0; n < sizeof(PinButs); n++)  {
    pinMode (PinButs [n], INPUT_PULLUP);
    butState [n] = digitalRead (PinButs [n]);
  }

  for (unsigned n = 0; n < sizeof(PinLeds); n++)  {
    digitalWrite (PinLeds [n], Off);
    pinMode      (PinLeds [n], OUTPUT);
  }
}