// HO Scale Speedometer

// Pin Definitions:
const byte SensorPinA = 2;  // First Sensor Digital Input
const byte SensorPinB = 4;  // Second Sensor Digital Input
const byte ResetPin = 7;    // Button to reset/restart measurement

// Constant Definitions:
unsigned long ProgMaxWaitTime = 20000;    // Maximum time the program will wait for user input before proceeding with default values (20s)
unsigned long SensorPollingTime = 10;     // Polling time for sensors (10ms, 100Hz)
float SensorDistanceDefault = 12;         // Default sensor distance value (12 in)
const float Scale = 87.1;                 // HO Scale Ratio (1:87)

// Variable Definitions:
bool DefaultSensorState = true;       // Default State for Sensors
float SensorDistance = 0;             // Distance Between Sensors in Inches
unsigned long ProgStartTime = 0;      // Program's start time in ms
unsigned long ProgInputStartTime = 0; // Program's time at which the user input is initially requested
unsigned long ProgCurrentTime = 0;    // Program's current time in ms
unsigned long FirstDetectionTime = 0; // Time at which the first change is detected
unsigned long SecondDetectionTime = 0;  // Time at which the second change is detected
unsigned long TimeElapsed = 0;        // Time elapsed between sensor detections (ms)
bool SensorStateA = true;             // Sensor State Variable for Reading Sensor A
bool SensorStateB = true;             // Sensor State Variable for Reading Sensor B
bool SensorStateA0 = true;            // First Sensor State Variable for Sensor A
bool SensorStateA1 = true;            // Second Sensor State Variable for Sensor A
bool SensorStateB0 = true;            // First Sensor State Variable for Sensor B
bool SensorStateB1 = true;            // Second Sensor State Variable for Sensor B
bool ResetButtonStateLast = true;     // State of reset button
bool ResetButtonState = true;         // State of reset button
byte FirstDetectionState = 0;         // Variable to store the detection state of the first detection (0-4, see compareSensorData2 function for states)
byte SecondDetectionState = 0;        // Variable to store the detection state of the second detection (0-4, see compareSensorData2 function for states)
float SensorDistanceScale = 0;   // Variable to store the distance between the sensors in scale feet (ft)
float ActSpeed = 0;     // Variable to store the actual speed (ft/s)
float ScaleSpeed = 0;   // Variable to store the scale speed (mph)


void setup() {
  // Set Up Defaults
  ProgStartTime = millis();   // Get the time at the start of the program
  Serial.begin(9600);                         // Begin Serial communication
  ResetButtonState = digitalRead(ResetPin);   // Get the initial state of the reset button
  ResetButtonStateLast = ResetButtonState;    // Get the initial state of the reset button
  bool DefaultSensorStateA = digitalRead(SensorPinA);   // Read the default/initial state of sensor A
  bool DefaultSensorStateB = digitalRead(SensorPinB);   // Read the default/initial state of sensor B
  if (DefaultSensorStateA==DefaultSensorStateB) {       // If the sensors agree on the default state, proceed with that default
    DefaultSensorState = DefaultSensorStateA;
  } else {    // If the sensors do not agree on the default state, inform user of error and recommendation to fix error, then stop program
    Serial.println("\nSensors A and B do not agree on a default state.  The detected states are as follows:");
    Serial.print("Sensor A:\t");
    Serial.println(DefaultSensorStateA);
    Serial.print("Sensor B:\t");
    Serial.println(DefaultSensorStateB);
    Serial.println("\nPlease check for anything that may be obstructing the sensors and remove it.  After clearing obstructions, please restart this device to try again.");
    while (true) {delay(SensorPollingTime);} // Stops the program, and requires a restart to try again.
  }

  ProgInputStartTime = millis();  // Get the time at which user input is ititally requested
  Serial.print("Please enter the distance between the 2 sensors in Inches, SensorDistance = ");   // Request User Input for the distance between the sensors
  while (Serial.available()==0 && ((ProgInputStartTime + ProgMaxWaitTime)>=ProgCurrentTime)) {    // Wait for the user to enter the distance between the sensors, or until maximum wait time
    ProgCurrentTime = millis();   // Get the current time to check against the maximum wait time
  }
  if (Serial.available()!=0) {    // If the user entered a value for the distance between the sensors, use that
    float userInput = Serial.parseFloat();
    SensorDistance = userInput;
    Serial.println(SensorDistance);   // Display the entered distance
  } else {                        // If the user did not enter a value for the distance between the sensors, use the default distance value
    SensorDistance = SensorDistanceDefault;
    Serial.print(SensorDistance);   // Display the default distance
    Serial.println(" (Default)");   // Display that the program is using a default value
  }
  Serial.print("Default Sensor State (Nothing Detected) is ");
  Serial.println(DefaultSensorState);         // Inform the user of the default sensor state
  Serial.println("Ready For Measurement");    // Inform the user that the program is ready to begin measurement
}

void loop() {
  Serial.println("SA State\tSB State\tTime (ms)");  // Display legend for user
  bool AttemptState = true;       // Create variable to represent if we should try to check the sensors
  while (AttemptState==true) {    // Attempts to check the sensors
    firstCheck:        // Return point for goto
    byte SensorChangeVal = 0;       // Create variable to represent if the sensor state(s) have changed (0-4, see compareSensorData2 function for states)
    while (SensorChangeVal==0) {   // Wait for at least one sensor to change state
      getSensorData(true);    // Get the current sensor states and the current time
      SensorChangeVal = compareSensorData2(DefaultSensorState, SensorStateA, DefaultSensorState, SensorStateB);   // Check to see if either sensor has changed states
      delay(SensorPollingTime);   // Delay for the Sensor Polling Time
      // Serial.println(SensorChangeVal);  // Debug/Testing
    }
    if (SensorChangeVal==3) {   // If the compareSensorData2 function has returned a 3 (ERROR), then inform the user and prompt them to try again.
      Serial.println("\nAn ERROR Has Occured and both sensors have registered changes at the same time.  The data that the compareSensorData2 function received is as follows:");
      Serial.print("DefaultSensorState = ");
      Serial.println(DefaultSensorState);
      Serial.print("SensorStateA = ");
      Serial.println(SensorStateA);
      Serial.print("SensorStateB = ");
      Serial.println(SensorStateB);
      Serial.println("\nThis ERROR has likely been caused by something unforeseen by the author of this program.  It is also possible (however unlikely) that this has been caused by your train being too fast.  If you would like to try again, please press the reset button.");
      while (true) {    // Stop the loop and wait for either a reset or a restart
        if (digitalRead(ResetPin)!=ResetButtonStateLast) {    // If the reset button is pressed, return to firstCheck point
          Serial.println("SA State\tSB State\tTime (ms)");  // Display legend for user
          goto firstCheck;
        }
        delay(SensorPollingTime);
      }
    } else if (SensorChangeVal==4) {   // If the compareSensorData2 function has returned a 4 (ERROR), then inform the user and stop the program.  This should never occur.
      Serial.println("\nAn ERROR Has Occurred and the data that the compareSensorData2 function has recieved is nonsensical.\nThe nonsensical data is as follows:");
      Serial.print("DefaultSensorState = ");
      Serial.println(DefaultSensorState);
      Serial.print("SensorStateA = ");
      Serial.println(SensorStateA);
      Serial.print("SensorStateB = ");
      Serial.println(SensorStateB);
      Serial.println("\nThere are no reasons known (at the time of writing this program) that should result in the ERROR occurring, and so there are no recommendations to resolve it.  If you wish to try again, please restart this device.");
      while (true) {delay(SensorPollingTime);}   // Stops the program, and requires a restart to try again.
    } else {    // If the sensor state change is valid and sensible, record the values and proceed
      FirstDetectionState = SensorChangeVal;    // Record the values
      FirstDetectionTime = ProgCurrentTime;
      SensorStateA0 = SensorStateA;
      SensorStateB0 = SensorStateB;
      Serial.print(SensorStateA0);
      Serial.print("\t\t");
      Serial.print(SensorStateB0);
      Serial.print("\t\t");
      Serial.println(ProgCurrentTime);
      AttemptState = false;   // Allows the program to exit the first sensor checking while loop
    }
  }
  // Get the second detection:
  AttemptState = true;       // Create variable to represent if we should try to check the sensors
  while (AttemptState==true) {    // Attempts to check the sensors
    secondCheck:        // Return point for goto
    byte SensorChangeVal = 0;       // Create variable to represent if the sensor state(s) have changed (0-4, see compareSensorData2 function for states)
    while (SensorChangeVal==0) {   // Wait for at least one sensor to change state
      getSensorData(true);    // Get the current sensor states and the current time
      SensorChangeVal = compareSensorData2(DefaultSensorState, SensorStateA, DefaultSensorState, SensorStateB);   // Check to see if either sensor has changed states
      delay(SensorPollingTime);   // Delay for the Sensor Polling Time
      // Serial.println(SensorChangeVal);  // Debug/Testing
    }
    if (SensorChangeVal==3) {   // If the compareSensorData2 function has returned a 3 (ERROR), then inform the user and prompt them to try again.
      Serial.println("\nAn ERROR Has Occured and both sensors have registered changes at the same time.  The data that the compareSensorData2 function received is as follows:");
      Serial.print("DefaultSensorState = ");
      Serial.println(DefaultSensorState);
      Serial.print("Previous SensorStateA (SensorStateA0) = ");
      Serial.println(SensorStateA0);
      Serial.print("SensorStateA = ");
      Serial.println(SensorStateA);
      Serial.print("Previous SensorStateB (SensorStateB0) = ");
      Serial.println(SensorStateB0);
      Serial.print("SensorStateB = ");
      Serial.println(SensorStateB);
      Serial.println("\nThis ERROR has likely been caused by something unforeseen by the author of this program.  It is also possible (however unlikely) that this has been caused by your train being too fast.  If you would like to try again, please press the reset button.");
      while (true) {    // Stop the loop and wait for either a reset or a restart
        if (digitalRead(ResetPin)!=ResetButtonStateLast) {    // If the reset button is pressed, return to secondCheck point
          Serial.println("SA State\tSB State\tTime (ms)");  // Display legend for user
          goto secondCheck;
        }
        delay(SensorPollingTime);
      }
    } else if (SensorChangeVal==4) {   // If the compareSensorData2 function has returned a 4 (ERROR), then inform the user and stop the program.  This should never occur.
      Serial.println("\nAn ERROR Has Occurred and the data that the compareSensorData2 function has recieved is nonsensical.\nThe nonsensical data is as follows:");
      Serial.print("DefaultSensorState = ");
      Serial.println(DefaultSensorState);
      Serial.print("Previous SensorStateA (SensorStateA0) = ");
      Serial.println(SensorStateA0);
      Serial.print("SensorStateA = ");
      Serial.println(SensorStateA);
      Serial.print("Previous SensorStateB (SensorStateB0) = ");
      Serial.println(SensorStateB0);
      Serial.print("SensorStateB = ");
      Serial.println(SensorStateB);
      Serial.println("\nThere are no reasons known (at the time of writing this program) that should result in the ERROR occurring, and so there are no recommendations to resolve it.  If you wish to try again, please restart this device.");
      while (true) {delay(SensorPollingTime);}   // Stops the program, and requires a restart to try again.
    } else if (SensorChangeVal==FirstDetectionState) {
      goto secondCheck;
    } else if ((SensorChangeVal!=FirstDetectionState) && (SensorChangeVal!=0)) {  // If the sensor state change is valid and sensible, record the values and proceed
      SecondDetectionState = SensorChangeVal;   // Record the values
      SecondDetectionTime = ProgCurrentTime;
      SensorStateA1 = SensorStateA;
      SensorStateB1 = SensorStateB;
      Serial.print(SensorStateA1);
      Serial.print("\t\t");
      Serial.print(SensorStateB1);
      Serial.print("\t\t");
      Serial.println(ProgCurrentTime);
      AttemptState = false;   // Allows the program to exit the first sensor checking while loop
    }
  }
  Serial.println("Measurement Complete");   // Inform the user of a successful measurement
  TimeElapsed = SecondDetectionTime - FirstDetectionTime;   // Calculate the time elapsed between sensor detections
  Serial.print("\nTime Elapsed (ms):\t");   // Display the time elapsed between sensor detections
  Serial.println(TimeElapsed);
  SensorDistanceScale = (SensorDistance*Scale)/12;    // Calculate the scale distance between the sensors
  Serial.print("The distance in scale feet between the sensors is ");   // Display the scale distance between the sensors
  Serial.print(SensorDistanceScale);
  Serial.println(" ft");
  ActSpeed = (SensorDistance/12)/(float(TimeElapsed)/1000);   // Calculate the actual speed (ft/s)
  Serial.print("The actual speed is ");   // Display the actual speed
  Serial.print(ActSpeed);
  Serial.println(" ft/s");
  // float tempVar1 = (ActSpeed*float(Scale));
  // Serial.println(tempVar1);
  // float tempVar2 = (3600.0/5280.0);
  // Serial.println(tempVar2);
  // ScaleSpeed = tempVar1*tempVar2;
  ScaleSpeed = (ActSpeed*float(Scale))*(3600.0/5280.0);    // Calculate the scale speed (mph)
  Serial.print("The scale speed is ");    // Display the scale speed
  Serial.print(ScaleSpeed);
  Serial.println(" mph");
  while (true) {
    if (digitalRead(ResetPin)!=ResetButtonStateLast) {    // If the reset button is pressed, return to secondCheck point
      Serial.println("Beginning New Measurement:");
      Serial.println("SA State\tSB State\tTime (ms)");  // Display legend for user
      goto firstCheck;
    }
    delay(SensorPollingTime);
  }
}

// Speedometer Functions:
void getSensorData(bool timeStamp) {        // Function to read sensor states
  if (timeStamp==true) {                    // If the current time is requested/required, record the current time
    ProgCurrentTime = millis();             // Get the current program time
  }
  SensorStateA = digitalRead(SensorPinA);   // Read sensor 0
  SensorStateB = digitalRead(SensorPinB);   // Read sensor 1
}

byte compareSensorData(bool SensorA, bool SensorB) {    // Function to compare sensor states
  if ((SensorA!=DefaultSensorState) && (SensorB==DefaultSensorState)) {   // If sensor A is detecting something and sensor B is not, return 1
    return 1;
  } else if ((SensorA==DefaultSensorState) && (SensorB!=DefaultSensorState)) {  // If sensor B is detecting something and sensor A is not, return 2
    return 2;
  } else if ((SensorA!=DefaultSensorState) && (SensorB!=DefaultSensorState)) {  // If both sensors are detecting something, return 3
    return 3;
  } else {    // If both sensors are at their default state, return 0
    return 0;
  }
}

byte compareSensorData2(bool SensorA0, bool SensorA1, bool SensorB0, bool SensorB1) {   // Function to determine which sensor has changed from is previous state
  // SensorA0: First Sensor First State; Sensor A1: First Sensor Second State; SensorB0: Second Sensor First State; SensorB1: Second Sensor Second State
  if ((SensorA0==SensorA1) && (SensorB0==SensorB1)) {   // If neither sensor has changed, return 0
    return 0;
  } else if ((SensorA0!=SensorA1) && (SensorB0==SensorB1)) {    // If only sensor A has changed, return 1
    return 1;
  } else if ((SensorA0==SensorA1) && (SensorB0!=SensorB1)) {    // If only sensor B has changed, return 2
    return 2;
  } else if ((SensorA0!=SensorA1) && (SensorB0!=SensorB1)) {    // If both sensors A and B have changed, return 3 (ERROR)
    return 3;
  } else {    // If something has gone horribly wrong and none of the above are true, return 4 (ERROR)
    return 4;
  }
}