/* PCA9685RailCrossingV3
   08/01/2024

   Sound and Button Control Added

   Sound provide by DY-SV5W MP3 player tutorial at:
   https://www.digitaltown.co.uk/components18DYSV5W.php

   Railway Crossing Sketch
   PCA9685 board for servos...2 gate or 4 gate
   PCA9685 board for LED's.... orange static, red flashing


  Software Serial pins 10-11
  Pin 9 Button   press to start/release to stop

*/
#include "SoftwareSerial.h"
#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"

//PCA 9685 settings
//Using the address as this sketch uses multiple PCA9685 boards
Adafruit_PWMServoDriver servoBoard = Adafruit_PWMServoDriver(0x40);//Address found from I2C scanner
Adafruit_PWMServoDriver LEDBoard = Adafruit_PWMServoDriver(0x46);//Address found from I2C scanner
#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
//end pca9685 settings

unsigned long currentMillis;//Keeps track of time in milliseconds for all timing
unsigned long triggerMillis;

unsigned long ledTimer;//stores time in millis for tracking events
byte ledState = 0;//Stores the current state of the system
byte currentledState = 0;// what is the current state
byte flashState;//is flash on or off

unsigned long gateServoTimer;
byte gateState;

unsigned long servoMovementTimer;
int servoPos[4][2];//[4 servos 0-3 on servo board][target pos, actual pos]

//software serial
SoftwareSerial mySerial(10, 11); // RX, TX

//DY-SV5W MP3 player
byte commandLength;
byte command[5];
int checkSum = 0;
int trackNum = 6;//track number on SD card
//Button
const int buttonPin = 9;//button on this pin with pull down resistor 4.7k

//sets the position of the servo in degrees.
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 (servo > -1 && servo < 16) { //only try to move valid servo addresses
    servoBoard.writeMicroseconds(servo, sendPos);
  }
}

//This would normally be triggered by sensors/buttons/dcc control
//Using a timer just for demo purposes.
void gateTrigger() {
  int buttonState = digitalRead(buttonPin);

  //gate closing trigger when button is pressed
  if (buttonState > 0 && ledState < 1) {
    ledState = 1;
    Serial.println("Trigger Close");
  }


  //gate opening trigger


  //when button is released gates open.
  if (gateState < 2 && ledState == 5 && buttonState == 0) { //
    Serial.println("Trigger Open");
    // triggerMillis = currentMillis + 10000;//reset timer
    gateState = 2;
  }
}

//controls movement and LEDs depending on led state
void ledControl() {
  int q;
  switch (ledState) {
    case 1://system starting...set lights to orange for 3 seconds
      if (currentledState < 1) {
        currentledState = 1;//set the current state
        Serial.println("Orange On");
        for (int q = 0; q < 4; q++) {
          LEDBoard.setPWM(q, 0, 4095);//turn Orange on full brightness
        }
        for (int q = 12; q < 16; q++) {
          LEDBoard.setPWM(q, 0, 4095);//turn red barrier lights on
        }
        ledTimer = currentMillis;
        ledState = 2;
        Serial.println(ledState);
        //start sound
        playTrack(trackNum);//Crossing sound is track 6 on my SD Card
      }

      break;
    case 2://Reds on orange off
      if (currentMillis - ledTimer >= 3000) {
        Serial.println("Reds on orange off 2");
        for (q = 4; q < 12; q++) {
          LEDBoard.setPWM(q, 0, 4095);//turn Reds on full brightness
        }
        for (q = 0; q < 4; q++) {
          LEDBoard.setPWM(q, 0, 4096);//turn Orange off
        }
        ledTimer = currentMillis;
        ledState = 3;
        Serial.println(ledState);
      }
      break;
    case 3://Reds off

      if (currentMillis - ledTimer >= 1500) { //0.5 second on before turning off
        Serial.println("Reds off 3");
        for (q = 4; q < 12; q++) {
          LEDBoard.setPWM(q, 0, 4096);//turn Reds off
        }
        ledTimer = currentMillis;
        ledState = 4;

      }

      break;
    case 4://Reds on
      if (currentMillis - ledTimer >= 500) { //0.5 second on before turning on
        Serial.println("Reds on 4");
        for (q = 4; q < 12; q++) {
          LEDBoard.setPWM(q, 0, 4095);//turn Reds on full brightness
        }
        ledState = 5;
        gateState = 1;
        gateServoTimer = currentMillis;
        triggerMillis = currentMillis;
      }
      break;
    case 5://Reds Flashing
      if (currentMillis - ledTimer >= 500) { //0.5 secs on before turning on
        ledTimer = currentMillis;
        if (flashState > 0) {
          for (q = 4; q < 8; q++) {
            LEDBoard.setPWM(q, 0, 4095);//turn Reds on full brightness

          }
          for (q = 8; q < 12; q++) {
            LEDBoard.setPWM(q, 0, 4096);//turn Reds off
          }
          flashState = 0;
        } else {
          for (q = 4; q < 8; q++) {
            LEDBoard.setPWM(q, 0, 4096);//turn Reds off
          }
          for (q = 8; q < 12; q++) {
            LEDBoard.setPWM(q, 0, 4095);//turn Reds on full brightness
          }
          flashState = 1;
        }
      }
      break;
    case 6://turn all LED's off... before barrier is fully down
      for (q = 0; q < 16; q++) {
        LEDBoard.setPWM(q, 0, 4096);//everything off;
      }
      currentledState = 0;
      ledState = 0;
      break;
    default:
      break;
  }
}

//Works through 3 different gate "states"
void servoControl() {
  switch (gateState) {
    case 1://lower barriers...train coming
      if (currentMillis - gateServoTimer >= 2000) {
        for (int q = 0; q < 2; q++) {//clockwise barriers/gates
          servoPos[q][0] = 89;
        }
        for (int q = 2; q < 4; q++) {//anti clockwise barriers gates
          servoPos[q][0] = 89;
        }
        gateState = 0;
      }

      break;
    case 2://raise barriers...train passed
      //no timer, will trigger from external...train
      for (int q = 0; q < 2; q++) {//clockwise barriers/gates
        servoPos[q][0] = 0;
      }
      for (int q = 2; q < 4; q++) {//anti clockwise barriers gates
        servoPos[q][0] = 179;
      }
      gateServoTimer = currentMillis;
      gateState = 3;
      break;
    case 3://Turns the LED's off before the gate has fully lowered as per real thing.
      //Serial.println(gateState);
      if (currentMillis - gateServoTimer >= 2500) { //wait 1 secs while gate raises
        gateState = 0;
        ledState = 6;//turn leds off
        stopTrack();//stop barrier sounds
        triggerMillis = currentMillis;//reset trigger timer
      }
      break;
    default:
      break;
  }
}



//this moves the servos to the value they have been targetted with
//but at a reduced speed to match the real thing
void servoMovement() {
  int q;
  if (currentMillis - servoMovementTimer >= 50) { //0.05 sec timin
    servoMovementTimer = currentMillis;
    for (q = 0; q < 4; q++) { //work through the 4 servos
      if (servoPos[q][0] > servoPos[q][1]) { //if the target is higher angle
        servoPos[q][1]++;//increment by 1
        setServoPos(q, servoPos[q][1]);//send the new value
      }
      if (servoPos[q][0] < servoPos[q][1]) { //if the target is smaller angle
        servoPos[q][1]--;//decrease by 1
        setServoPos(q, servoPos[q][1]);//send the new value
      }
    }
  }

}

//DY-SV5W MP3 player functions
//May need to be selected after putting into random mode
void stopTrack() {
  command[0] = 0xAA;//first byte says it's a command
  command[1] = 0x04;
  command[2] = 0x00;
  command[3] = 0xAE;
  commandLength = 4;
  sendCommand();
}
//play selected track
void playTrack(int soundTrack) {
  //select track
  //Serial.print("soundTrack: ");
  //Serial.println(soundTrack);
  command[0] = 0xAA;//first byte says it's a command
  command[1] = 0x07;
  command[2] = 0x02;
  command[3] = highByte(soundTrack);//snh...track HIGH bit
  command[4] = lowByte(soundTrack);//SNL... track low bit
  checkSum = 0;
  for (int q = 0; q < 5; q++) {
    checkSum +=  command[q];
  }
  command[5] = lowByte(checkSum);//SM check bit... low bit of the sum of all previous values
  commandLength = 6;
  sendCommand();
}

//sets the device volume...0 - 30
void playbackVolume(int vol) {
  if (vol > 30) { //check within limits
    vol = 30;
  }
  command[0] = 0xAA;//first byte says it's a command
  command[1] = 0x13;
  command[2] = 0x01;
  command[3] = vol;//volume
  checkSum = 0;
  for (int q = 0; q < 4; q++) {
    checkSum +=  command[q];
  }
  command[4] = lowByte(checkSum);//SM check bit... low bit of the sum of all previous values
  commandLength = 5;
  sendCommand();
}

//sends the command to the DY-SV5W
void sendCommand() {
  int q;
  for (q = 0; q < commandLength; q++) {
    mySerial.write(command[q]);
    Serial.print(command[q], HEX);
  }
  Serial.println("End");
}

void setup() {
  Serial.begin(9600);
  Serial.println("PCA9685RailCrossingV3");

  servoBoard.begin();
  servoBoard.setOscillatorFrequency(27000000);
  servoBoard.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  LEDBoard.begin();//start the PCA9685 for the station building
  LEDBoard.setPWMFreq(1600);  // This is the maximum PWM frequency and suited to LED's

  delay(10);//let the above values take effect

  //make sure everything is off
  for (int q = 0; q < 16; q++) {
    LEDBoard.setPWM(q , 0, 4096);//turn Orange on full brightness
  }
  //servos in start position
  for (int q = 0; q < 2; q++) {//clockwise barriers/gates
    setServoPos(q, 0);
    servoPos[q][0] = 0;//target
    servoPos[q][1] = 0;//actual
  }
  for (int q = 2; q < 4; q++) {//anti clockwise barriers gates
    setServoPos(q, 179);
    servoPos[q][0] = 179;//target
    servoPos[q][1] = 179;//actual
  }
  //button
  pinMode(buttonPin, INPUT);

  //software serial
  mySerial.begin(9600);
  delay(100);
  //DY-SV5W set up
  playbackVolume(17);//sets volume to lvl 17
  stopTrack();//stop barrier sounds
}





void loop() {
  currentMillis = millis();//get current time since board started in milliseconds

  gateTrigger();//function that checks if the gate should be moving...this would usually be reading sensors/buttons
  ledControl();//controls the leds
  servoControl();//servo movement/timing control

  servoMovement();//deals with speed of movement...sweep at controlled speed


}