#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <RotaryEncoder.h>
#include "settings.h"
#include "structures.h"

// ***************************************************
//                  !!! DISCLAIMER !!!
//
// as per:
// - https://docs.wokwi.com/parts/wokwi-arduino-mega
// - https://github.com/wokwi/avr8js/issues/125
//
// Input Capture is not implemented in Wokwi.
// The sketch is not going to work in simulation mode.
// ***************************************************

// *** 128x32 SCREEN
Adafruit_SSD1306 Display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET);
// ***

// *** ROTARY ENCODER
RotaryEncoder Encoder(ENCODER_CLK_PIN, ENCODER_DT_PIN);
// ***

// *** MENU
byte selectedMenu = 0;
// ***

// *** DATA
bool isMenuOpen = false;

volatile uint8_t measurementsCount = 0;

#ifdef USE_ICP1
volatile uint16_t initialValue = 0;
volatile uint16_t finalValue = 0;
volatile uint16_t timer1Counts = 0;
#else
// volatile bool initializationCompleted = false;
volatile uint32_t initialValue = 0;
volatile uint32_t finalValue = 0;
#endif

ChronoData chronoData;
// ***

void setup() {
  // initialize serial
  Serial.begin(115200);

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!Display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1) {}
  }

  // set pin mode for both probes
  pinMode(PROBES_PIN, INPUT);

  // set pull-up resistor on rotary encoder input pin
  pinMode(ENCODER_SW_PIN, INPUT_PULLUP);

  // attach two interrupts to the encoder in order to make it high priority
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK_PIN), ISR0, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_DT_PIN), ISR1, CHANGE);

  // set initial encoder position
  Encoder.setPosition((int)(chronoData.bbWeight_g * 100));

#ifdef USE_ICP1
  // reset Timer1 register to its default value
  TCCR1A = 0;

  // set 8x prescaler
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B &= ~(1 << CS10);

  // reset Timer1 value
  TCNT1 = 0;

  // reset the value of ICR1 that stores the data in which the pulse occurred
  ICR1 = 0;

  // set the interruption via Input Capture on Timer1
  TIMSK1 = (1 << ICIE1);

  // enable global interrupts
  sei();
#else
  pinMode(BUTTON_PIN, INPUT);
  // attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), buttonInterruptHandler, RISING);
  // initializationCompleted = true;
#endif
}

void loop() {
#ifndef USE_ICP1
  buttonHandler();
#endif

  measureData();
  handleScreen();
}

void measureData() {
  if (isMenuOpen || measurementsCount < 2) {
    return;
  }

#ifdef USE_ICP1
  // calculate timing using clock cycles

  if (finalValue < initialValue) {
    timer1Counts = 65535 - ((65535 - initialValue) + finalValue);
  }

  if (finalValue >= initialValue) {
    timer1Counts = finalValue - initialValue;
  }

  // (Periodo clock * Prescaler * timer1Counts) / 1E6
  float time_ms = ((1.0 / F_CPU * 1E9) * 8.0 * (float)timer1Counts) / 1E6;
#else
  // calculate timing using micros() function, less accurate but functioning in simulator

  uint32_t usec = finalValue - initialValue;
  float time_ms = usec * 0.001;
#endif

  float time_s = time_ms / 1000.0;

  if ((PROBES_DISTANCE_M / time_s) <= 3047.00) {
    chronoData.bbSpeed_m_s = PROBES_DISTANCE_M / time_s;
    chronoData.bbSpeed_fps = chronoData.bbSpeed_m_s * 3.280839;
  }

  if ((PROBES_DISTANCE_M / time_s) > 3047.00) {
    chronoData.bbSpeed_m_s = 0;
    chronoData.bbSpeed_fps = 0;
  }

  chronoData.bbEnergy_J = ((chronoData.bbWeight_g / 1000.0) * (chronoData.bbSpeed_m_s * chronoData.bbSpeed_m_s)) / 2;

  measurementsCount = 0;
}

void handleScreen() {
  // handle encoder click for entering menu
  if (!digitalRead(ENCODER_SW_PIN)) {
    isMenuOpen = true;
  }

  // handle encoder click for exiting menu
  if (isMenuOpen && digitalRead(ENCODER_SW_PIN)) {
    selectedMenu++;
    isMenuOpen = false;
  }

  if (selectedMenu == 0) {
    showMeasures();
  }

  if (selectedMenu == 1) {
    setBBWeight();
  }

  if (selectedMenu >= 2) {
    selectedMenu = 0;
  }
}

void showMeasures() {
  Display.clearDisplay();
  Display.setTextColor(SSD1306_WHITE);

  // // TODO: remove (debug only)
  // chronoData.bbEnergy_J = 9.78;
  // chronoData.bbSpeed_fps = 399;
  // chronoData.bbSpeed_m_s = 999.2;
  // // ***

  // DRAW FIRST LINE (J - g - m/s)
  Display.setTextSize(1);

  // [x.xx J]
  Display.setCursor(0, 3);
  Display.print(chronoData.bbEnergy_J, 2);
  Display.setCursor(28, 3);
  Display.print("J");

  // [y.yy g]
  Display.setCursor(45, 3);
  Display.print(chronoData.bbWeight_g, 2);
  Display.setCursor(73, 3);
  Display.print("g");

  // [zzz fps]
  Display.setCursor(90, 3);
  Display.print(chronoData.bbSpeed_fps, 0);
  Display.setCursor(110, 3);
  Display.print("fps");

  // DRAW SECOND LINE (FPS value)
  Display.setTextSize(4);

  if (chronoData.bbSpeed_m_s <= 9.9) {
    Display.setCursor(30, 18);
  }

  if (chronoData.bbSpeed_m_s > 9.9 && chronoData.bbSpeed_m_s <= 99.9) {
    Display.setCursor(18, 18);
  }

  if (chronoData.bbSpeed_m_s > 99.9 && chronoData.bbSpeed_m_s <= 999.9) {
    Display.setCursor(4, 18);
  }

  Display.print(chronoData.bbSpeed_m_s, 1);

  // DRAW THIRD LINE (FPS text)
  Display.setTextSize(2);
  Display.setCursor(50, 50);
  Display.print("m/s");

  Display.display();
}

void setBBWeight() {
  Display.clearDisplay();
  Display.setTextColor(SSD1306_WHITE);

  Display.setTextSize(1);
  Display.setCursor(26, 0);
  Display.print("Set BBs weight");

  Display.setTextSize(3);
  Display.setCursor(20, 22);
  Display.print(chronoData.bbWeight_g, 2);
  Display.setCursor(100, 22);
  Display.print("g");

  int16_t encoderPosition = Encoder.getPosition();

  if (Encoder.getPosition() < 0) {
    Encoder.setPosition(0);
    encoderPosition = 0;
  }

  if (Encoder.getPosition() > 999) {
    Encoder.setPosition(999);
    encoderPosition = 999;
  }

  chronoData.bbWeight_g = encoderPosition / 100.00;

  Display.display();
}

// encoder interrupt 0
void ISR0() {
  if (selectedMenu == 1) {
    Encoder.tick();
  }
}

// encoder interrupt 1
void ISR1() {
  if (selectedMenu == 1) {
    Encoder.tick();
  }
}

#ifdef USE_ICP1
// interrupt callback to get probe times
ISR(TIMER1_CAPT_vect) {
  if (measurementsCount == 0) {
    initialValue = ICR1;
  }

  if (measurementsCount == 1) {
    finalValue = ICR1;
  }

  measurementsCount++;
}
#else
void buttonHandler() {
  // if (!initializationCompleted) {
  //   return;
  // }

  if (digitalRead(BUTTON_PIN) == HIGH) {
    if (measurementsCount == 0) {
      initialValue = micros();
      finalValue = initialValue + random(950, 1050);

      measurementsCount = 2;
    }
  }
}
#endif