#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Encoder.h>

// Pin definition for ILI9341
#define TFT_CS     10
#define TFT_RST    9
#define TFT_DC     8

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Encoder pins
#define ENCODER_CLK 2
#define ENCODER_DT  3
#define ENCODER_SW  4  // Optional, for reset
Encoder myEncoder(ENCODER_CLK, ENCODER_DT);

long oldPosition  = -999;
long newPosition;
float dutyCycleA = 0;
float dutyCycleB = 0;
float phaseShift = 0;

// Setup the encoder button (optional for reset functionality)
void setup() {
  pinMode(ENCODER_SW, INPUT_PULLUP); // Set the button with internal pull-up

  tft.begin();
  tft.setRotation(3);  // Adjust rotation based on how your display is connected
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);

  Serial.begin(9600);
}

void loop() {
  // Read the position from the encoder
  newPosition = myEncoder.read();

  // If the position has changed, update the display
  if (newPosition != oldPosition) {
    oldPosition = newPosition;

    // Clear the display before drawing new values
    tft.fillScreen(ILI9341_BLACK);
    tft.setCursor(0, 0);

    // Display encoder position
    tft.println("Encoder Position: ");
    tft.println(newPosition);

    // Duty cycle and phase shift (simulated for now)
    dutyCycleA = measureDutyCycle(ENCODER_CLK);  // Measure duty cycle for CLK
    dutyCycleB = measureDutyCycle(ENCODER_DT);   // Measure duty cycle for DT
    phaseShift = calculatePhaseShift();          // Simulate phase shift

    // Display duty cycle and phase shift
    tft.println("Duty Cycle A: ");
    tft.println(dutyCycleA);
    tft.println("Duty Cycle B: ");
    tft.println(dutyCycleB);
    tft.println("Phase Shift: ");
    tft.println(phaseShift);

    // Print to serial for debugging
    Serial.print("Position: ");
    Serial.print(newPosition);
    Serial.print(" | DutyA: ");
    Serial.print(dutyCycleA);
    Serial.print(" | DutyB: ");
    Serial.print(dutyCycleB);
    Serial.print(" | PhaseShift: ");
    Serial.println(phaseShift);
  }

  // Check if the reset button is pressed (optional)
  if (digitalRead(ENCODER_SW) == LOW) {
    myEncoder.write(0);  // Reset the encoder position to zero
    delay(500);  // Debounce delay
  }
}

// Function to simulate duty cycle measurement (can be refined with interrupts)
float measureDutyCycle(int pin) {
  unsigned long highTime = pulseIn(pin, HIGH);
  unsigned long lowTime = pulseIn(pin, LOW);
  float totalTime = highTime + lowTime;
  return (highTime / totalTime) * 100.0;  // Return duty cycle in percentage
}

// Function to simulate phase shift between A and B signals
float calculatePhaseShift() {
  unsigned long timeA = pulseIn(ENCODER_CLK, HIGH);  // Measure pulse width for CLK
  unsigned long timeB = pulseIn(ENCODER_DT, HIGH);   // Measure pulse width for DT
  float phaseShift = (360.0 * abs(timeA - timeB)) / (timeA + timeB);  // Example phase calculation
  return phaseShift;  // Return phase shift in degrees
}