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

// TFT display pins
#define TFT_CS   10
#define TFT_DC   9
#define TFT_RST  8

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

unsigned int tftWidth = 0;
unsigned int tftHeight = 0;
unsigned int tftHalfHeight = 0;


long ampPos = 0;
long freqPos = 0;


// Rotary encoder pins and variables
#define ENCODER1_A 2
#define ENCODER1_B 4
#define ENCODER2_A 3
#define ENCODER2_B 5

Encoder encoder1(ENCODER1_A, ENCODER1_B);
Encoder encoder2(ENCODER2_A, ENCODER2_B);

void setup() {
  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);

  tftWidth = tft.width();
  tftHeight = tft.height();
  tftHalfHeight = tftHeight / 2;

  // Attach interrupts for encoder 1
  pinMode(ENCODER1_A, INPUT_PULLUP);
  pinMode(ENCODER1_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER1_A), encoder1ISR, CHANGE);

  // Attach interrupts for encoder 2
  pinMode(ENCODER2_A, INPUT_PULLUP);
  pinMode(ENCODER2_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER2_A), encoder2ISR, CHANGE);
}

void encoder1ISR() {
  static uint8_t prevA = LOW;
  uint8_t A = digitalRead(ENCODER1_A);
  uint8_t B = digitalRead(ENCODER1_B);

  if (prevA != A) {
    prevA = A;
    (A != B) ? ampPos++ : ampPos--;
  }
}

void encoder2ISR() {
  static uint8_t prevA = LOW;
  uint8_t A = digitalRead(ENCODER2_A);
  uint8_t B = digitalRead(ENCODER2_B);

  if (prevA != A) {
    prevA = A;
    (A != B) ? freqPos++ : freqPos--;
  }
}


void writeDebugValues(int freq, int ampl, int off, int drawTime) {
  // Display encoder values on the screen
  tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setCursor(5, 5);
  tft.print("Freq: ");
  tft.println(freq);
  tft.setCursor(5, 25);
  tft.print("Ampl: ");
  tft.println(ampl);
  tft.setCursor(5, 45);
  tft.print("off: ");
  tft.println(off);

  tft.setCursor(150, 5);
  tft.print("drawTime: ");
  tft.println(drawTime);
}

const float fps = 29.97;
const float frameTime = 1000 / fps;


#define OFFSET_SPEED 1

// Waveform variables
int amplitude = 70;
int frequency = 5;
float offset = 0;

int previousAmplitude = amplitude;
int previousFrequency = frequency;

unsigned long lastFrame = millis();
int frameDisplayTimer = fps;


void loop() {
  // Check if it's time to update the encoder values
  long newPosition1 = ampPos;
  amplitude = constrain(amplitude + newPosition1, 5, 80);
  ampPos = 0;

  long newPosition2 = freqPos;
  frequency = constrain(frequency + newPosition2, 0, 10);
  freqPos = 0;

  //if (currentMillis - lastDrawUpdate >= drawUpdateInterval) {

  offset += OFFSET_SPEED;
  if (offset >= 180) {
    offset = 0;
  }

  const float prevOffset = offset - OFFSET_SPEED;

  // Update user modifiable wave
  updateWave(ILI9341_GREEN, prevOffset, offset, previousFrequency, frequency, previousAmplitude, amplitude);

  previousAmplitude = amplitude;
  previousFrequency = frequency;


  frameDisplayTimer --;
  if (frameDisplayTimer == 0) {
    frameDisplayTimer = fps;
    // print frame time
    tft.fillRect(150, 5, 240 - 150, 15, ILI9341_BLACK);
    tft.setCursor(150, 5);
    tft.print("drawTime: ");
    tft.println(millis() - lastFrame);


    bool aligned = frequency == 2 && (amplitude >= 49 && amplitude <= 51);

    // // Check if waveforms are aligned
    tft.setTextColor(aligned ? ILI9341_GREEN : ILI9341_RED, ILI9341_BLACK);

    tft.setCursor(tftWidth / 2 - 100, tftHeight - 15 );

    (aligned) ? tft.print("  Aligned  ") : tft.print("Mis-Aligned");
  }

  while ((millis() - lastFrame) < fps);
  lastFrame = millis();
}

void updateWave(uint16_t color, float oldPosition, float newPosition, int oldFreq, int newFreq, int oldAmpl, int newAmpl) {
  // const int tftWidth = tft.width();

  int oldY1 = 0;
  int oldY2 = 0;

  int newY1 = 0;
  int newY2 = 0;

  static const int resolution  = 3;

  for (int x = 0; x < tftWidth - 1; x += resolution) {
    float x_ratio = TWO_PI * x / tftWidth;
    float x1_ratio = TWO_PI * (x + resolution) / tftWidth;

    // Calculate the old waveform
    oldY1 = oldAmpl * sin(x_ratio * oldFreq + oldPosition) + tftHalfHeight;
    oldY2 = oldAmpl * sin(x1_ratio * oldFreq + oldPosition) + tftHalfHeight;

    // Erase the old static waveform
    tft.drawLine(x, oldY1, x + resolution, oldY2, ILI9341_BLACK);

    oldY1 = 50 * sin(x_ratio * 2 + oldPosition) + tftHalfHeight;
    oldY2 = 50 * sin(x1_ratio * 2 + oldPosition) + tftHalfHeight;

    // Erase the old user modifeable waveform
    tft.drawLine(x, oldY1, x + resolution, oldY2, ILI9341_BLACK);

    // Calculate and re-draw the new waveform
    newY1 = 50 * sin(x_ratio * 2 + newPosition) + tftHalfHeight;
    newY2 = 50 * sin(x1_ratio * 2 + newPosition) + tftHalfHeight;

    // Draw the static waveform
    tft.drawLine(x, newY1, x + resolution, newY2, ILI9341_WHITE);

    // Calculate the new waveform
    newY1 = newAmpl * sin(x_ratio * newFreq + newPosition) + tftHalfHeight;
    newY2 = newAmpl * sin(x1_ratio * newFreq + newPosition) + tftHalfHeight;

    // Draw the new waveform
    tft.drawLine(x, newY1, x + resolution, newY2, ILI9341_ORANGE);
  }
}