#include <avr/io.h>
#include <avr/interrupt.h>
#include <EEPROM.h>

volatile long encoderPosition = 0;
volatile bool directionUp = false;
volatile unsigned long pulseWidth = 0;
volatile bool measureHeight = false;
volatile unsigned long lastPulseTime = 0;
volatile float speed = 0.0;
volatile float positionMM = 0.0;
volatile float startPositionMM = 1000.0;

const int proximitySensor = 7;
const int signAPin = 2;
const int signBPin = 3;
bool proximitySensorLogic = HIGH; // Change to LOW if the sensor is normally low
unsigned long proximitySensorWidth = 10; // Width in milliseconds
const int startPositionAddress = 201; // EEPROM address for startPositionMM
const unsigned long showPeriod = 500; // Time interval to show speed and position
unsigned long lastShowTime = 0; // Last time speed and position were shown
const unsigned long speedSmoothTime = 10; // Time interval to smooth speed
unsigned long lastSpeedSmoothTime = 0; // Last time speed was smoothed
float displayedSpeed = 0.0; // Smoothed speed to display

// Variables for distance calculation
const int drivePulleyTeeth = 26; // Number of teeth on the drive pulley
const float profilePitch = 5.0; // Pitch of the profile in mm
const int pulsesPerRevolution = 32; // Pulses per revolution of the encoder

// Calculate the distance per revolution
const float distancePerRevolution = drivePulleyTeeth * profilePitch; // Distance per revolution in mm
const float distancePerPulse = distancePerRevolution / pulsesPerRevolution; // Distance per encoder pulse in mm

bool startPositionSet = false; // Flag to indicate if the start position has been set

// Variables for pulse counting
volatile long lastEncoderPosition = 0;
volatile unsigned long lastPulseCountTime = 0;

void setup() {
  // Encoder settings
  pinMode(signAPin, INPUT); // Signal A
  pinMode(signBPin, INPUT); // Signal B
  attachInterrupt(digitalPinToInterrupt(signAPin), encoderA, RISING);
  attachInterrupt(digitalPinToInterrupt(signBPin), encoderB, RISING);

  // Timer1 settings
  cli(); // Disable interrupts
  TCCR1A = 0; // Set TCCR1A register to 0
  TCCR1B = 0; // Set TCCR1B register to 0
  TCNT1  = 0; // Set TCNT1 counter to 0
  OCR1A = 15999; // Set compare value for OCR1A register to 15999
  TCCR1B |= (1 << WGM12); // Set Timer1 to CTC mode
  TCCR1B |= (1 << CS10); // Set Timer1 prescaler to 1
  TIMSK1 |= (1 << OCIE1A); // Enable Timer1 compare interrupt
  sei(); // Enable interrupts

  // Proximity sensor settings
  pinMode(proximitySensor, INPUT);

  // Read start position from EEPROM
  EEPROM.get(startPositionAddress, startPositionMM);
  if (isnan(startPositionMM) || startPositionMM == 0) {
    startPositionMM = 1000.0; // Default value if EEPROM is empty or uninitialized
  }
  Serial.begin(115200);
  Serial.print("Start Position: ");
  Serial.print(startPositionMM);
  Serial.println(" mm");
}

void loop() {
  unsigned long currentTime = millis();

  // Check if the start position should be set
  if (!startPositionSet && directionUp && (digitalRead(proximitySensor) == proximitySensorLogic)) {
    static unsigned long sensorHighTime = 0;
    if (currentTime - sensorHighTime >= proximitySensorWidth) {
      startPositionSet = true;
      encoderPosition = startPositionMM / distancePerPulse;
      positionMM = startPositionMM;
      Serial.println("Start position set.");
    }
  } else if (!directionUp && (digitalRead(proximitySensor) == proximitySensorLogic)) {
    static unsigned long sensorHighTime = 0;
    if (currentTime - sensorHighTime >= proximitySensorWidth) {
      encoderPosition = 0;
      positionMM = 0.0;
      Serial.println("Position reset due to downward movement and proximity sensor trigger.");
    }
  }

  // Update positionMM based on encoderPosition
  positionMM = encoderPosition * distancePerPulse;

  // Smooth the speed
  if (currentTime - lastSpeedSmoothTime >= speedSmoothTime) {
    lastSpeedSmoothTime = currentTime;
    displayedSpeed += (speed - displayedSpeed) * 0.1; // Adjust the smoothing factor as needed
  }

  // Display speed and position every showPeriod milliseconds
  if (currentTime - lastShowTime >= showPeriod) {
    lastShowTime = currentTime;

    // Calculate speed based on pulse count
    long pulseCount = encoderPosition - lastEncoderPosition;
    unsigned long timeDifference = currentTime - lastPulseCountTime;
    if (timeDifference > 0) {
      speed = (abs(pulseCount) * distancePerPulse / timeDifference) * 1000.0; // mm/s
    }
    lastEncoderPosition = encoderPosition;
    lastPulseCountTime = currentTime;

    Serial.print("Speed: ");
    if (displayedSpeed == 0.0) {
      Serial.print("-");
    } else {
      Serial.print(abs(displayedSpeed)); // Ensure positive speed
      Serial.print(" mm/s, ");
      if (directionUp) {
        Serial.print("Going up");
      } else {
        Serial.print("Going down");
      }
    }
    Serial.print(", Position: ");
    if (positionMM == 0.0) {
      Serial.println("-");
    } else {
      Serial.print(positionMM);
      Serial.println(" mm");
    }
  }

  // Check if the encoder has stopped
  if (currentTime - lastPulseTime > 100) { // 100 ms threshold for stopping
    speed = 0.0;
  }
}

//Detecting a signA rising, compare if signAPin is oposite of signBPin
void encoderA() {
  if (digitalRead(signAPin) == !digitalRead(signBPin)) {
    encoderPosition++;
    directionUp = true;
  }
//  Serial.println(encoderPosition);
  lastPulseTime = millis();
}

//Detecting a signB rising, compare if signBPin is oposite of signAPin
void encoderB() {
  if (digitalRead(signBPin) == !digitalRead(signAPin)) {
    encoderPosition--;
    directionUp = false;
  }
//  Serial.println(encoderPosition);
  lastPulseTime = millis();
}

ISR(TIMER1_COMPA_vect) {
  // Code for debounce and timing
  unsigned long currentTime = millis();
  
  if (directionUp && (currentTime - lastPulseTime >= pulseWidth)) {
    measureHeight = true;
    lastPulseTime = currentTime;
  }
}

void saveStartPosition(float newStartPositionMM) {
  if (newStartPositionMM != startPositionMM) {
    startPositionMM = newStartPositionMM;
    EEPROM.put(startPositionAddress, startPositionMM);
    Serial.print("New Start Position Saved: ");
    Serial.print(startPositionMM);
    Serial.println(" mm");
  }
}