/*  HopeStationV5
    23/12/21 Updated to controller
    Addresses set correctly in the 10-20 range for this board/module

    programmed on a Mega for the extra opins for the servos

    Final Programming against board
    micros() rollover code on turntable move...WORKING :-)
    Turntable adjuster with cap switches...WORKING
    servos coded in...working

     Station DCC Controller
     Turntable
     EEPROM
     2 x touch button turntable adjusters
     5 point servos
     2 x photointerrupter

     Turntable has 4 exits

  Pins
  2 - interrupt pin for DCC

  7 = enablePin...stepper
  8 = stepPin...stepper
  9 = dirPin...stepper

  23 = cap switch
  25 = cap switch
  27  servo
  29 servo
  31 servo
  33 servo
  35 servo

  A0 = photointerrupter....far corner
  A1 = photointerrupter...station end

*/

#include "NmraDcc.h"
NmraDcc  Dcc ;
DCC_MSG  Packet ;

//EEPROM
//system will use built in eeprom as it should not be written to very much...limited lifecycle.
#include "EEPROM.h"

long eepromUpdatePeriod = 10000;//60 secs between updates...also used to turn the power off
unsigned long eepromUpdateTimer;
const int eepromOffset = 200;//position of 1st bit of data...prevents eeprom wear in same place from previous use place
int trackPositions[4] = {247, 10747, 13047, 23547}; //default positions in steps of track exits from end 1



#include "Servo.h"
const int PointAPin = 27;
const int PointBPin = 29;
const int PointCPin = 31;
const int PointDPin = 33;
const int PointEPin = 35;
const int servoLiveTime = 1500;//1.5 secs before reset
unsigned long servoTimer;
byte servoDetached = 0;//1 means attached
Servo PointA;
Servo PointB;
Servo PointC;
Servo PointD;
Servo PointE;


//stepper motor
const int maxSteps = 25600;//number of steps in rotation
const int enablePin = 7;//taken high to disable stepper
const int stepPin = 8;
const int dirPin = 9;
const int poweron = 0;
const int poweroff = 1;
const int clockwise = 0;
const int anticlockwise = 1;

const int capClockPin = 23;
const int capantiClockPin = 25;

int moveToTrackPos = 100; //default when not looking for a track
int currentTrackPos;
byte turntableIndexState = 0; //1 means it has indexed
byte turntableDirection;
byte stepInState = 0;//high or low state for next step
unsigned long stepperTimer;//timer until next step in micros();
int stepperSpeedDelay = 1000;//delay in micros() between steps...controls speed, Good max speed
int stepperStartSpeedDelay = 30000;//nice and slow for start speed...adjustment speed
int stepCounter;//current step position of the turntable
byte photointerruptReadState = 0;//photointyerrupter state...triggers if hit
int currentStepperSpeedDelay;
//counters to deal with acceleration timings
int findDirection;
int stepsTaken;//count up from start

//gets eeprom data and puts in trackpositions[4] array
void geteepromdata() {
  int q;
  byte getbytes[8];//2 bytes per position
  Serial.println("getbytes[]: ");
  //get the data from eeprom and store in array
  for (q = 0; q < 8; q++) {
    getbytes[q] = EEPROM.read(q + eepromOffset);
    Serial.println( getbytes[q]);
  }
  Serial.println("trackPositions[]: ");
  //now assemble the data and put into trackpositions[4]
  for (q = 0; q < 4; q++) {
    trackPositions[q] = (getbytes[q * 2] * 256) + (getbytes[(q * 2) + 1]);
    Serial.println(trackPositions[q]);
  }
}

//update the eeprom with current trackpositions
void updateeeprom() {
  int q;

  for (q = 0; q < 4; q++) {
    EEPROM.update((q * 2) + eepromOffset, highByte(trackPositions[q]));
    EEPROM.update((q * 2) + eepromOffset + 1, lowByte(trackPositions[q]));
  }
  geteepromdata();
}

void moveTurntable() {
  unsigned long currentMicros;
  currentMicros = micros();

  //only do something of a movement has been commanded
  if (moveToTrackPos < 5) {
    //changes here to how timing works, this avoids the micros tollover issue at 70 minutes
    if ((currentMicros - stepperTimer) >= currentStepperSpeedDelay) {
      stepperTimer = currentMicros;
      if (stepInState > 0) {
        stepInState = 0;
      } else {
        stepInState = 1;
      }
      digitalWrite(stepPin, stepInState);
      stepsTaken++;


      if (turntableIndexState > 0) { //don't adjust speed if not indexed
        //acceleration

        if (stepsTaken < 500) {
          currentStepperSpeedDelay = stepperSpeedDelay + 2000 - (stepsTaken * 4);
        }
        if ((findDirection - stepsTaken) < 500) {
          currentStepperSpeedDelay = stepperSpeedDelay + 2000 - ((findDirection - stepsTaken) * 4);
        }

      } else {
        stepsTaken = 0;
      }
      
      if (turntableDirection == clockwise ) {
        stepCounter++;
        if (stepCounter > maxSteps) {
          stepCounter = 0;
        }
      } else {
        stepCounter--;
        if (stepCounter < 0) {
          stepCounter = maxSteps;
        }
      }


      //this resets the index everytime the indexing point is crossed in a clockwise direction
      int photointerruptRead = analogRead(A0);
      if (photointerruptRead < 250 && turntableDirection == clockwise && photointerruptReadState < 1) { //only do this check in clockwise direction
        photointerruptReadState = 1;

        Serial.println(stepCounter);
        stepCounter = 0;
      }
      if (photointerruptRead > 250 && photointerruptReadState > 0) {
        photointerruptReadState = 0;
      }
    }

    //stop the table at preset points
    if (stepCounter == trackPositions[moveToTrackPos]) {
      Serial.print("Target: ");
      Serial.print(trackPositions[moveToTrackPos]);
      Serial.print(" : ");
      Serial.print(stepCounter);
      Serial.println(" ");
      currentTrackPos = moveToTrackPos;
      moveToTrackPos = 100;
      turntableIndexState = 1;
    }
  }
}

//finds out what direction to spin the turntable...shortest route
//turns power on if there is a change of position
void getTurntableDirection() {

  findDirection = trackPositions[moveToTrackPos] - stepCounter;
  if (findDirection < 0) {
    findDirection = findDirection + maxSteps;
  }
  if (findDirection > (maxSteps / 2)) {
    Serial.println("anticlock: ");
    digitalWrite(dirPin, anticlockwise); // LOW for anticlockwise
    turntableDirection = anticlockwise;
  } else {
    Serial.println("clock: ");
    digitalWrite(dirPin, clockwise); // LOW for anticlockwise
    turntableDirection = clockwise;
  }
  Serial.println("findDirection: " + String(findDirection));
  //turn the power on if the stepper needs to move
  if (trackPositions[moveToTrackPos] != stepCounter) {
    stepsTaken = 0;//reset
    currentStepperSpeedDelay = stepperStartSpeedDelay;
    digitalWrite(enablePin, poweron);//turn motor driver on
  } else {//is the track already at the position selected...if so do nothing
    moveToTrackPos = 100;
  }
}

// This function is called whenever a normal DCC Turnout Packet is received and we're in Output Addressing Mode
void notifyDccAccTurnoutOutput( uint16_t Addr, uint8_t Direction, uint8_t OutputPower)
{
  switch (Addr) {
    case 10://entrance point coming onto module
      PointA.attach(PointAPin);
      servoTimer = millis() + servoLiveTime;
      servoDetached = 1;//let system know servos are attached
      delay(30);//shouldn't upset DCC as too fast to press 2nd button
      if (Direction == 0) {
        PointA.write(130); //board entrance
      } else {
        PointA.write(85); //ok
      }
      break;
    case 11://Main line to platform/turntable
      PointB.attach(PointBPin);
      servoTimer = millis() + servoLiveTime;
      servoDetached = 1;//let system know servos are attached
      delay(30);//shouldn't upset DCC as too fast to press 2nd button
      if (Direction == 0) {
        PointB.write(80); //platform
      } else {
        PointB.write(100); //ok
      }
      break;
    case 12://Freight siding
      PointC.attach(PointCPin);
      servoTimer = millis() + servoLiveTime;
      servoDetached = 1;//let system know servos are attached
      delay(30);//shouldn't upset DCC as too fast to press 2nd button
      if (Direction == 0) {
        PointC.write(105); //freight siding
      } else {
        PointC.write(50); //ok
      }
      break;
    case 13://Goods shed
      PointD.attach(PointDPin);
      servoTimer = millis() + servoLiveTime;
      servoDetached = 1;//let system know servos are attached
      delay(30);//shouldn't upset DCC as too fast to press 2nd button
      if (Direction == 0) {
        PointD.write(105); //goods shed shed
      } else {
        PointD.write(75); //ok
      }
      break;

    case 14://Front siding/coal
      PointE.attach(PointEPin);
      servoTimer = millis() + servoLiveTime;
      servoDetached = 1;//let system know servos are attached
      delay(30);//shouldn't upset DCC as too fast to press 2nd button
      if (Direction == 0) {
        PointE.write(110); //engine shed
      } else {
        PointE.write(80); //ok
      }
      break;

    //Turntables have an address for each location, they will activate whatever direction is selected.
    case 15: // Track to station
      if (moveToTrackPos == 100) { //make sure last movement has stopped before starting a new one
        moveToTrackPos = 2;//left
        getTurntableDirection();
      }
      break;
    case 16: // Track to station other end
      if (moveToTrackPos == 100) { //make sure last movement has stopped before starting a new one
        moveToTrackPos = 0;//left
        getTurntableDirection();
      }
      break;
    case 17: //loop at front of station
      if (moveToTrackPos == 100) { //make sure last movement has stopped before starting a new one
        moveToTrackPos = 1;//left
        getTurntableDirection();
      }
      break;
    case 18: //loop at front of station opposite end
      if (moveToTrackPos == 100) { //make sure last movement has stopped before starting a new one
        moveToTrackPos = 3;//left
        getTurntableDirection();
      }
      break;
    default:
      break;
  }
  Serial.print("notifyDccAccTurnoutOutput: ") ;
  Serial.print(Addr, DEC) ;
  Serial.print(',');
  Serial.println(Direction, DEC) ;
}

void adjustTurntablePostionSettings() {
  //can only manually move when not in a movement
  if (digitalRead(capClockPin) > 0 && moveToTrackPos == 100) {
    //Serial.println("capClockPin");
    digitalWrite(enablePin, poweron);
    digitalWrite(dirPin, clockwise); //direction
    delayMicroseconds(stepperStartSpeedDelay);
    digitalWrite(stepPin, HIGH);
    trackPositions[currentTrackPos]++;//adjust the position
    stepCounter++;//update the turntable position
    delayMicroseconds(stepperStartSpeedDelay);
    digitalWrite(stepPin, LOW);
    trackPositions[currentTrackPos]++;
    stepCounter++;//update the turntable position
  }
  if (digitalRead(capantiClockPin) > 0) {
    digitalWrite(enablePin, poweron);
    digitalWrite(dirPin, anticlockwise); //direction
    delayMicroseconds(stepperStartSpeedDelay);
    digitalWrite(stepPin, HIGH);
    trackPositions[currentTrackPos]--;//adjust the position
    stepCounter--;//update the turntable position
    delayMicroseconds(stepperStartSpeedDelay);
    digitalWrite(stepPin, LOW);
    trackPositions[currentTrackPos]--;
    stepCounter--;//update the turntable position
  }
}

void setup() {
  Serial.begin(9600);
  Serial.println("HopeStationV5");
  //stepper set up pins as outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(enablePin, OUTPUT); //turndriver on and off to get rid of the noise
  digitalWrite(enablePin, poweroff);
  digitalWrite(dirPin, clockwise); // LOW for anticlockwise
  turntableDirection = clockwise;

  //cap touch switches for adjustment
  pinMode(capClockPin, INPUT);
  pinMode(capantiClockPin, INPUT);

  //get trackposition data from eeprom
  geteepromdata();

  // Setup which External Interrupt, the Pin it's associated with that we're using and enable the Pull-Up
  Dcc.pin(0, 2, 1);
  // Call the main DCC Init function to enable the DCC Receiver
  Dcc.init( MAN_ID_DIY, 10, CV29_ACCESSORY_DECODER | CV29_OUTPUT_ADDRESS_MODE, 0 );

  //index the unit
  digitalWrite(dirPin, clockwise); // LOW for anticlockwise
  turntableDirection = clockwise;
  moveToTrackPos = 3;//always index clockwise to position 0
  digitalWrite(enablePin, poweron);//turn motor driver on
}



void loop() {
  Dcc.process();//process incoming DCC Commands
  moveTurntable();//deals with turntable control
  adjustTurntablePostionSettings();//touch switches to make adjustments

  //do the eeprom updates
  if (millis() > eepromUpdateTimer && moveToTrackPos == 100) { // not moving so can be updated
    digitalWrite(enablePin, poweroff);//turn the power off as it's not moving
    eepromUpdateTimer = millis() + eepromUpdatePeriod;
    updateeeprom();
  }
  //detach any servos to stop chatter
  if (servoDetached > 0 && millis() >  servoTimer) {
    servoDetached = 0;
    PointA.detach();//detach all the point servos
    PointB.detach();
    PointC.detach();
    PointD.detach();
    PointE.detach();
  }

}