#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

#define TFT_GREY 0x7BEF
#define GREY TFT_GREY
#define BLACK ILI9341_BLACK
#define WHITE ILI9341_WHITE
#define GREEN ILI9341_GREEN

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;

// try
short prevStaticY1[320];
short prevStaticY2[320];
short prevUserY1[320];
short prevUserY2[320];

// Rotary encoder pins and variables
#define ENCODER_AMP_A 2
#define ENCODER_AMP_B 4
#define ENCODER_FREQ_A 3
#define ENCODER_FREQ_B 5

// Encoder ampEncoder(ENCODER_AMP_A, ENCODER_AMP_B);
// Encoder freqEncoder(ENCODER_FREQ_A, ENCODER_FREQ_B);

const PROGMEM int staticWaveRes = 4;
const PROGMEM int resolution = 2;

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

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

  //  prevStaticY1 = new int[tftWidth/staticWaveRes];
  //  prevStaticY2 = new int[tftWidth/staticWaveRes];


  //  prevUserY1 = new int[tftWidth/resolution];
  // prevUserY2 = new int[tftWidth/resolution];


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

  // Attach interrupts for encoder 2
  pinMode(ENCODER_FREQ_A, INPUT_PULLUP);
  pinMode(ENCODER_FREQ_B, INPUT_PULLUP);
  //attachInterrupt(digitalPinToInterrupt(ENCODER_FREQ_A), encoder2ISR, CHANGE);


}

void encoder1ISR() {
  static uint8_t prevA = LOW;
  uint8_t A = digitalRead(ENCODER_AMP_A);
  uint8_t B = digitalRead(ENCODER_AMP_B);

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

void encoder2ISR() {
  static uint8_t prevA = LOW;
  uint8_t A = digitalRead(ENCODER_FREQ_A);
  uint8_t B = digitalRead(ENCODER_FREQ_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 2

// 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( offset, frequency, 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) < frameTime);
  // lastFrame = millis();
}

void updateWave(const float& newPosition, const int& newFreq, const int& newAmpl) {

  int gapWidth = tftWidth / (8 * newFreq);

  for (int x = 10; x < tftWidth - 10; x += staticWaveRes) {
    float x_ratio = TWO_PI * x / tftWidth;
    float xNext_ratio = TWO_PI * (x + staticWaveRes) / tftWidth;

    int newY1 = 50 * sin(x_ratio * 2 + newPosition) + tftHalfHeight;
    int newY2 = 50 * sin(xNext_ratio * 2 + newPosition) + tftHalfHeight;

    bool drawWave = (x / gapWidth) % 2 == 0;

    if (prevStaticY1[x] != newY1 || prevStaticY2[x] != newY2) {
      tft.drawLine(x, prevStaticY1[x], x + staticWaveRes, prevStaticY2[x], BLACK);

      if (drawWave) {
        tft.drawLine(x, newY1, x + staticWaveRes, newY2, GREY);
      }
    }

    prevStaticY1[x] = newY1;
    prevStaticY2[x] = newY2;
  }




  for (int x = 10; x < tftWidth - 10; x += resolution) {
    float x_ratio = TWO_PI * x / tftWidth;
    float xNext_ratio = TWO_PI * (x + resolution) / tftWidth;

    int newY1 = 0;// newAmpl * sin(x_ratio * newFreq + newPosition) + tftHalfHeight;
    int newY2 = 0; //newAmpl * sin(xNext_ratio * newFreq + newPosition) + tftHalfHeight;

    if (prevUserY1[x] != newY1 || prevUserY2[x] != newY2) {
      tft.drawLine(x, prevUserY1[x], x + resolution, prevUserY2[x], BLACK);
      tft.drawLine(x, newY1, x + resolution, newY2, GREEN);
    }


    prevUserY1[x] = newY1;
    prevUserY2[x] = newY2;
  }
}