/*  fiddleYardTraverserV4
     24/11/23
     steppers rest after 1 minute of no movement
     baud rate changed
     Adjuster switches added on pins 6 & 7
     18/10/23
     Addresses change to 11-15
    05/10/2023

    This version adds the DCC control

    DCC addresses are Acc numbers 11 - 15 left or right will trigger the move.

    Nema 17 stepper Motor
    Photo interrupter
    pin 2 connected to minabay optoisolator set up to get DCC signal

    Seems to work at about 100 steps per mm using 1/4 steps

    Pins Used:

    A0 Photointerrupter
    2 dcc pin
    6 adjuster pin
    7 adjuster pin
    8 enable pin
    9  Step Pin (stepper)
    10 Direction Pin (stepper)

*/
//EEPROM
//system will use built in eeprom as it should not be written to very much...limited lifecycle.
#include < EEPROM.h >
const byte eepromCheck = 11;//check value
unsigned long eepromDelay = 20000;
unsigned long eepromTimer;
unsigned long currentMillis;
unsigned long lastStepperMoveTimer;
unsigned long stepperRestTime = 60000;//1 minute
byte eepromUpdateFlag;

//stored for emergency
int defaulttrackPositions[5][2] = { //these are the defaults in case eeprom error or Arduino has been swapped.
  {11, 900}, //acc number step offset
  {12, 6300},
  {13, 11700},
  {14, 17100},
  {15, 22500}
};
//5400 step intervals
int trackPositions[5][2] = { //these are the defaults in case eeprom error or Arduino has been swapped.
  {11, 900}, //acc number step offset
  {12, 6300},
  {13, 11700},
  {14, 17100},
  {15, 22500}
};
//adjusters
const int adjustPlusPin = 6;
const int adjustMinusPin = 7;

//Stepper Motor... 800 steps per revolution
const int enablePin = 8; //taken high to disable stepper
const int stepPin = 9;   // change tp 3 for your set up
const int dirPin = 10;   //change t 4 for your set up
const int photoInterrupterPin = 0;
int testArrayPos = 1;
byte lastStep;
int travCounter = 0; //used to count the number of movements to reset auto homing
int travReindexCounter = 20; // Number of movements before reindex, if losing index lower number
const byte travForwards = 1;
const byte travBackwards = 0;
const byte stepperPowerOn = 0;
const byte stepperPowerOff = 1;
//int stepDelay = 100;
int stepDelay = 120;
long stepsTarget;   //Target postion of stepper motor
long stepperPosition = 100;
int stepperArrayPos; //position in the array
byte indexFound = 0; //1 = found...zero searching

//NMRA DCC library
#include < NmraDcc.h >
NmraDcc  Dcc ;
DCC_MSG  Packet ;

//function used for controlling points
// 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)
{
  int q;
  //Print out the DCC command to make sure it's working
  Serial.print("notifyDccAccTurnoutOutput: ") ;
  Serial.print(Addr, DEC) ;
  Serial.print(',');
  Serial.print(Direction, DEC) ;
  Serial.print(',');
  Serial.println(OutputPower, HEX) ;
  switch (Addr) {
    case 11://front track..nearest photo interrupter
      indexFound  = 0;//force system to reindex everytime the nearest track is selected
      stepperArrayPos = 0;
      moveToPosition(trackPositions[0][1]);
      break;
    case 12:
      stepperArrayPos = 1;
      moveToPosition(trackPositions[1][1]);
      break;
    case 13:
      stepperArrayPos = 2;
      moveToPosition(trackPositions[2][1]);
      break;
    case 14:
      stepperArrayPos = 3;
      moveToPosition(trackPositions[3][1]);
      break;
      stepperArrayPos = 4;
    case 15://rear track
      moveToPosition(trackPositions[4][1]);
      break;

    default://not for this device
      break;
  }
}

//Moves the stepper to the selected position.
//will auto index if not already indexed.
void moveToPosition(int trackPos) {
  Serial.print("Target: ");
  Serial.println(trackPos);
  int q;
  int travelDirection;
  int stepsToMove;
  travCounter++;
  if (travCounter >= travReindexCounter) {
    travCounter = 0;
    indexFound  = 0;
    Serial.print("Re Index");
  }
  if (indexFound < 1) {
    findIndex();//finds the index point
  }
  digitalWrite(enablePin, stepperPowerOn); //Enable stepper driver
  if (stepperPosition > trackPos) {
    digitalWrite(dirPin, travForwards);//move towards photointerrupter
    travelDirection = travForwards;
    stepsToMove = stepperPosition - trackPos;
  } else {
    digitalWrite(dirPin, travBackwards);//move away from photointerrupter
    travelDirection = travBackwards;
    stepsToMove = trackPos - stepperPosition;
  }
  Serial.print("Current Steps: ");
  Serial.println(stepperPosition);
  Serial.print("Target Steps: ");
  Serial.println(trackPos);
  Serial.print("stepper to move: ");
  Serial.println(stepsToMove);
  for (int q = 0; q < stepsToMove; q++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(stepDelay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(stepDelay);
  }
  stepperPosition = trackPos;
  //digitalWrite(enablePin, stepperPowerOff); //disable stepper driver
  lastStepperMoveTimer = currentMillis;
  Serial.println(stepperPosition);
  Serial.println("stepperArrayPos: " + String(stepperArrayPos));

}

void checkInterrupter() {
  int photointerrupterState = 0;
  photointerrupterState = analogRead(photoInterrupterPin);//Read photointerrupter
  //Serial.println(photointerrupterState);
  if (photointerrupterState < 100) {
    indexFound = 1;
  } else {
    indexFound = 0;
  }
}


//finds the index point
void findIndex() {
  digitalWrite(enablePin, stepperPowerOn); //Enable stepper driver
  checkInterrupter();
  if (indexFound > 0) { //if the system starts with the interrupter blocked
    indexFound = 0;//restest as traverser will move away 1st
    Serial.println("initial move away from photointerrupter");
    digitalWrite(dirPin, travBackwards);//move backwards away from the photointerrupter
    //for (int q = 0; q < 21700; q++) {//move 3 rotations
    for (int q = 0; q < 4000; q++) {//move away 4000 steps
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(stepDelay);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(stepDelay);
    }
  }
  Serial.println("moving towards photointerrupter");//towards photointerrupter is forwards?
  do {
    digitalWrite(dirPin, travForwards);//move towards photointerrupter
    digitalWrite(stepPin, HIGH);
    checkInterrupter();
    delayMicroseconds(stepDelay);
    if (indexFound < 1) {
      digitalWrite(stepPin, LOW);
      checkInterrupter();
      delayMicroseconds(stepDelay);
    }
  }
  while (indexFound < 1); //keep moving forward until index is set

  stepperPosition = 1000;//a1lows stepper to pass beyond photointerupter
  Serial.println("Indexed :-)");
  digitalWrite(enablePin, stepperPowerOff); //Enable stepper driver
  Serial.println(stepperPosition);
  delay(500);

}

//deals with adjuster switches
void adjusterSwitches(){
  if (indexFound > 0) {//only runs when system has been indexed
    if (digitalRead(adjustMinusPin) > 0) {
      digitalWrite(enablePin, stepperPowerOn);
      digitalWrite(dirPin, travForwards);//move towards photointerrupter
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(stepDelay * 20);//slow speed
      digitalWrite(stepPin, LOW);
      delayMicroseconds(stepDelay * 20);
      stepperPosition--;//update stepper position
      trackPositions[stepperArrayPos][1]--;//update trackPositionArray
      eepromUpdateFlag = 1;//set flag to update eeprom
      eepromTimer = currentMillis;
      lastStepperMoveTimer = currentMillis;
      Serial.println(trackPositions[stepperArrayPos][1]);
    }
    if (digitalRead(adjustPlusPin) > 0) {
      digitalWrite(enablePin, stepperPowerOn);
      digitalWrite(dirPin, travBackwards);//move towards photointerrupter
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(stepDelay * 20);//slow speed
      digitalWrite(stepPin, LOW);
      delayMicroseconds(stepDelay * 20);
      stepperPosition++;//update stepper position
      trackPositions[stepperArrayPos][1]++;//update trackPositionArray
      eepromUpdateFlag = 1;//set flag to update eeprom
      eepromTimer = currentMillis;
      lastStepperMoveTimer = currentMillis;
      Serial.println(trackPositions[stepperArrayPos][1]);
    }
  }  
}

//gets the data from EEPROM
void getEEPROMData() {
  int q;
  byte testEEPROM = EEPROM.read(0); //this should have a value of 16
  Serial.println("Read EEPROM");
  Serial.println("EEEPROM TEST: " + String(testEEPROM));
  if (testEEPROM != eepromCheck) {//check valid eeprom in case Arduino has been changed
    Serial.println("EEPROM Fail...writing new EEPROM data");
    updateEEPROM();//will write default addresses from original positionArray[q]
  } else { //good set so read away
    Serial.println("Good EEPROM data");
    for (q = 0; q < 5; q++) {
      trackPositions[q][1] = EEPROMReadint((q + 1) * 10);
      
    }
    for (q = 0; q < 5; q++) {
      Serial.println(trackPositions[q][1]);
    }
  }
}

//writes the corrected data set to EEPROM
void updateEEPROM() {
  int q;
  EEPROM.write(0, eepromCheck); //set up marker
  for (q = 0; q < 5; q++) {
    EEPROMWritelong((q + 1) * 10, trackPositions[q][1]);
  }
  Serial.println("EE");
}

int EEPROMReadint(long address){
  int two = EEPROM.read(address);
  int one = EEPROM.read(address + 1); 
  return two + (one * 256); 
}
void EEPROMWritelong(int address, int value) {
  byte two = (value & 255);
  byte one = ((value >> 8) & 255); 
  EEPROM.update(address, two);
  EEPROM.update(address + 1, one);
}

void eepromCheckForUpdate(){
  if(eepromUpdateFlag > 0){
    if(currentMillis - eepromTimer >= eepromDelay){
      updateEEPROM();
      eepromUpdateFlag = 0; 
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("fiddleYardTraverserV4");
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(adjustMinusPin, INPUT);
  pinMode(adjustPlusPin, INPUT);

  //DCC
  // 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 );
  Serial.println("DCC Init Done");

  getEEPROMData();//read the info from EEPROM
}

void loop() {
  Dcc.process();//checks for DCC connads being received
  adjusterSwitches();
  currentMillis = millis();
  eepromCheckForUpdate();
  restStepper();
}

//turns steppers off after an interval....talking at exhibition
void restStepper(){
 if(currentMillis - lastStepperMoveTimer >= stepperRestTime){
  digitalWrite(enablePin, stepperPowerOff);
  Serial.println("Step rest");   
 }
}