/********************************************************************
*********************************************************************
** 
** Golf Tee Controller Software
** 
** Designed by: Sarah E. Smith
**         for: Eastern Washington University
**              2023-2024 Spring Capstone Project
**
Revision History
---------------------------------------------------------------------
   Created                                           11 May 2023

*********************************************************************
*********************************************************************/

// Local Includes
#include "Adafruit_VL53L0X.h"

// Local Definitions
#define MOTOR1_PIN1 2
#define MOTOR1_PIN2 3
#define MOTOR2_PIN1 4
#define MOTOR2_PIN2 5
#define BUTTON_PIN  7
#define SENSOR_PIN  0

#define SENSOR_HIGH 0
#define SENSOR_LOW  1023

#define BALL_PRESENT_THRESHOLD 300

// Local Constants

// change this to fit the number of steps per revolution
// for your motor
#define SECONDS_PER_REVOLUTION 10

// change this based on the measurement of the circumference of
// the gear driving the chain
#define SECONDS_PER_HALF_INCH 0.5

// Local declaration of global variables
int currentStep;
bool ballIsPresent;
bool teeIsLow;
bool userButtonPressed;
bool ballStateReported;
bool raiseTee;
uint16_t rangeToBall;
uint8_t rangeStatus;

// Create the Adafruit Sensor (communication via the I2C bus)
Adafruit_VL53L0X myLox = Adafruit_VL53L0X();

/********************************************************************
// Function Name: setup()
// Input Variables: 
//   none
// Returns: void
// Description:
//   The following function is a run once operation. It is called by
//   the bootstrap code once the board boot up is complete. There is
//   no need for the software to make this call.
*********************************************************************/
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  // wait until serial port opens for native USB devices
  while (!Serial) {
    delay(1);
  }

  // Check the L0X sensor to make sure it is up and operating
  Serial.println("Adafruit VL53L0X test.");
  if (!myLox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    // while (1);
  }

  // power
  Serial.println(F("VL53L0X API Set for Simple Ranging\n\n"));

  // Initialize the user button
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // Set up the motor connections
  pinMode(MOTOR1_PIN1, OUTPUT);
  pinMode(MOTOR1_PIN2, OUTPUT);
  pinMode(MOTOR2_PIN2, OUTPUT);
  pinMode(MOTOR2_PIN2, OUTPUT);

  lowerTee();

  ballIsPresent = false;
  userButtonPressed = false;
  ballStateReported = false;
  currentStep = 0;
}

/********************************************************************
// Function Name: readUserButton()
// Input Variables: 
//   none
// Returns: bool
// Description:
//   This function interprets the user interaction with the button.
//   The operation of the button is dual mode. If the user presses
//   the button and releases it, this indicates the user wants to 
//   raise the tee level.
*********************************************************************/
bool readUserButton() {
  if (userButtonPressed) {
    if (digitalRead(BUTTON_PIN) == HIGH) {
      // The button had been pressed but has been released. So, return true.
      userButtonPressed = false;
      return true;
    }
  } else {
    // Handle the case where the button had not previously been pressed.

    // Is the button currently being pressed?
    if (digitalRead(BUTTON_PIN) == LOW) {
      userButtonPressed = true;
    }

    return false;
  }

  delay(0);
}

/********************************************************************
// Function Name: rotateMotor()
// Input Variables: 
//   none
// Returns: void
// Description:
//   This function steps the motor from one tee level to another
//   at 1/2 inch increments. When the tee is at its highest and 
//   is commanded to raise again, the tee will lower to the start
//   height of 1/2 inch.
*********************************************************************/
void rotateMotor() {
  if (currentStep == 8) {
    Serial.print("Rotating motor counter clockwise: ");
    Serial.print(SECONDS_PER_HALF_INCH * (currentStep-1));
    Serial.println(" seconds.");

    digitalWrite(MOTOR1_PIN1, LOW);
    digitalWrite(MOTOR1_PIN2, HIGH);

    digitalWrite(MOTOR2_PIN1, HIGH);
    digitalWrite(MOTOR2_PIN2, LOW);

    delay(SECONDS_PER_HALF_INCH * (currentStep-1)*1000);
   
    digitalWrite(MOTOR1_PIN1, LOW);
    digitalWrite(MOTOR1_PIN2, LOW);

    digitalWrite(MOTOR2_PIN1, LOW);
    digitalWrite(MOTOR2_PIN2, LOW);
    currentStep = 1;
    teeIsLow = false;
  } else {
    Serial.print("Rotating motor clockwise: ");
    Serial.print(SECONDS_PER_HALF_INCH);
    Serial.println(" seconds.");
    
    digitalWrite(MOTOR1_PIN1, HIGH);
    digitalWrite(MOTOR1_PIN2, LOW);

    digitalWrite(MOTOR2_PIN1, LOW);
    digitalWrite(MOTOR2_PIN2, HIGH);

    delay(SECONDS_PER_HALF_INCH * 1000);
   
    digitalWrite(MOTOR1_PIN1, LOW);
    digitalWrite(MOTOR1_PIN2, LOW);

    digitalWrite(MOTOR2_PIN1, LOW);
    digitalWrite(MOTOR2_PIN2, LOW);
    currentStep++;
    teeIsLow = false;
  }

  Serial.print("tee height: ");
  Serial.print(currentStep * 0.5);
  Serial.println(" inches.");

  delay(1);
}

/********************************************************************
// Function Name: isBallPresent()
// Input Variables: 
//   bool &isPresent
// Returns: uint8_t
// Description:
//   This function sets a flag indicating if there is a ball
//   on top of the tee. Additionally it will return an error value
//   created by the Adafruit sensor. If a ball is present on the 
//   tee, then it's range will be measured and stored.
*********************************************************************/
uint8_t isBallPresent(bool &isPresent, uint16_t &range) {
  uint8_t status;

  if (analogRead(SENSOR_PIN) > BALL_PRESENT_THRESHOLD) {
    if (!ballIsPresent)
      ballStateReported = false;

    isPresent = true;
    if (!ballStateReported) {
      Serial.println("ball present");
      ballStateReported = true;
    }

    status = measureRangeToBall(range);

  } else {
    if (ballIsPresent)
      ballStateReported = false;

    isPresent = false;

    if (!ballStateReported) {
      Serial.println("ball gone");
      ballStateReported = true;
    }

    status = 0;
  }

  delay(1);

  return status;
}

/********************************************************************
// Function Name: measureRangeToBall()
// Input Variables: 
//   uint16_t &measurement
// Returns: uint8_t
// Description:
//   This function is designed use the Adafruit range sensor to 
//   measure the range to the ball on top of the tee. The
//   measurement value (passed by address) will be in millimeters
//   and a status flag for the measurement will be returned for
//   evaluation.
*********************************************************************/
uint8_t measureRangeToBall(uint16_t &measurement) {
  VL53L0X_RangingMeasurementData_t measure;

  // Serial.print("Reading a measurement... ");
  myLox.rangingTest(&measure, false);  // pass in 'true' to get debug data printout!

  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    // Serial.print("Distance (mm): ");
    // Serial.println(measure.RangeMilliMeter);
    measurement = measure.RangeMilliMeter;
  } else {
    Serial.println(" out of range ");
  }

  delay(1);

  return measure.RangeStatus;
}

/********************************************************************
// Function Name: lowerTee()
// Input Variables: 
//   none
// Returns: void
// Description:
//   This function is designed to lower the tee to its lowest
//   position so that the user can load a ball on top of the
//   teeing position. The position of the lowered tee cannot
//   be verified since no ball is present, so the current step
//   is used to spin down the tee.
*********************************************************************/
void lowerTee() {
  if (!teeIsLow) {
    Serial.print("Rotating motor counter clockwise: ");
    Serial.print(currentStep * SECONDS_PER_HALF_INCH);
    Serial.println(" seconds.");

    digitalWrite(MOTOR1_PIN1, LOW);
    digitalWrite(MOTOR1_PIN2, HIGH);

    digitalWrite(MOTOR2_PIN1, HIGH);
    digitalWrite(MOTOR2_PIN2, LOW);

    delay(currentStep * SECONDS_PER_HALF_INCH * 1000);
   
    digitalWrite(MOTOR1_PIN1, LOW);
    digitalWrite(MOTOR1_PIN2, LOW);

    digitalWrite(MOTOR2_PIN1, LOW);
    digitalWrite(MOTOR2_PIN2, LOW);

    currentStep = 0;
    teeIsLow = true;
    Serial.println("Tee Lowered");
    Serial.print("tee height: ");
    Serial.print(currentStep * 0.5);
    Serial.println(" inches.");
  }
}

/********************************************************************
// Function Name: loop()
// Input Variables: 
//   none
// Returns: void
// Description:
//   The forever system loop begins here. This call is invoked at
//   the OS loop so there is no need to make a call to it. The 
//   specifics of system operation can be found in the design 
//   document held by the Mechanical Engineering Department at
//   Eastern Washington University. Those versed in code can
//   examine the operation below.
*********************************************************************/
void loop() {
  // Is the ball present?
  rangeStatus = isBallPresent(ballIsPresent, rangeToBall);
  if (rangeStatus == 4) {
    Serial.println("Error trying to measure range");
  }

  if (!ballIsPresent) {
    // If the ball is not present, make sure to lower the tee to
    // it's lowest setting.
    lowerTee();
  }

  // put your main code here, to run repeatedly:
  raiseTee = readUserButton();
  if (raiseTee && ballIsPresent)
    rotateMotor();

  delay(50);
}