// From the Discord channel.

/*********************************************************************
  @author Heath Matthews

  TODO:
  replace polling with interrupts
  implement PWM signal output       --check
  tune debouncing
  reformat display interface
*********************************************************************/
//#include <time.h>                  // debouncing
#include <EncoderStepCounter.h>  // rotary encoder
//#include <SPI.h>                   //?
#include <Wire.h>  // display
//#include <Adafruit_GFX.h>          // display
#include <Adafruit_SH110X.h>  // display

/* encoder */
#define EB_CLK 2
#define EB_DT 3
#define EB_SW 4
EncoderStepCounter eb(EB_CLK, EB_DT);
uint8_t debounce = 10;
int oldPosition = 0;

/* display */
#define i2c_Address 0x3c
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
volatile bool displayValid = true;

/* output */
#define PWM1 5  //PWM pins: 5, 6
#define PWM2 6
uint8_t out_freq = 64;

// stores device settings in a convenient format
struct VertebraConfig {
  static const uint8_t LEN = 12;
  uint8_t names[LEN][7] = { "C1/T1 ", "C2/T2 ",
                            "C3/T3 ", "C4/T4 ",
                            "C5/T5 ", "C6/T6 ",
                            "C7/T7 ", "T8/L1 ",
                            "T9/L2 ", "T10/L3",
                            "T11/L4", "T12/L5"
                          };
  uint8_t freqs[LEN] = { 64, 72, 80, 84,
                         96, 108, 120, 68,
                         76, 88, 100, 116
                       };
  int8_t idx = 0;
} v;
bool fMODE = LOW;
volatile int home = 0;

// Behavior for the encoder rotate.
volatile bool iip = false;  // interrupt in progress
volatile int lastButtonState = 1;
void isr() {
  if (!iip) {
    iip = true;
    analogWrite(PWM1, 0);
    analogWrite(PWM2, 0);
    displayValid = false;
    eb.tick();
    Serial.println("interrupt.");
    Serial.println(PWM1);
    int position = eb.getPosition();

    /* (2) check for rotation */
    if (position != oldPosition) {
      if (fMODE) {
        out_freq = out_freq - home + position;
      } else {
        v.idx = mod(position, v.LEN);
        out_freq = v.freqs[mod(v.idx, v.LEN)];
      }
      oldPosition = position;
    }
    iip = false;
  }
}

//PWM pin #?
void outputFunction(uint8_t freq) {
  uint16_t half_period = 500 / freq;
  uint8_t intensity = 64;
  analogWrite(PWM1, intensity);
  analogWrite(PWM2, 0);
  delay(half_period);
  analogWrite(PWM1, 0);
  analogWrite(PWM2, intensity);
  delay(half_period);
}

void setup() {
  display.begin(i2c_Address, true);  // Address 0x3C default
  eb.begin();

  pinMode(EB_SW, INPUT_PULLUP);
  pinMode(EB_CLK, INPUT_PULLUP);
  // pinMode(EB_DT),INPUT_PULLUP);
  pinMode(PWM1, OUTPUT);
  pinMode(PWM2, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(EB_CLK), isr, CHANGE);
  attachInterrupt(digitalPinToInterrupt(EB_DT), isr, CHANGE);  //?
//  attachInterrupt(digitalPinToInterrupt(EB_SW), isr_sw, CHANGE); // this didn't help with the button press

  Serial.begin(9600);
}

/*
  loop has 4 main components:
  (1) check for button press to know which mode we are in
  (2) otherwise check for rotation
  (3) format and print to display
  (4) output function while display is accurate
*/
void loop() {
  int position = eb.getPosition();
  int buttonState = digitalRead(EB_SW);

  // /* (1) check for button press */
  // this is not working because it is checked only once before the infinite loop is entered
  // putting this into an isr is ideal if I can get it to work
  //
  if (buttonState != lastButtonState) {
    delay(debounce);
    if (buttonState == LOW) {
      fMODE = !fMODE;
      out_freq = v.freqs[v.idx];
      home = position;
    }
    lastButtonState = buttonState;
  }

  // /* (2) check for rotation */
  // this was moved into the isr, it seems clunky and slow though.
  // else if (position != oldPosition) {
  //   if (fMODE){
  //     out_freq = out_freq + home - position;
  //   } else {
  //     v.idx = mod(position, v.LEN);
  //     out_freq = v.freqs[mod(v.idx, v.LEN)];
  //   }
  //   oldPosition = position;
  // }
  // iip = false;


  /* (3) display the menu to LCD */
  printHeader();
  if (fMODE) {
    printFreq();
  } else {
    printVert();
  }
  display.display();
  displayValid = true;

  /* (4) output function loop */
  delay(300);
  while (displayValid) {
    outputFunction(out_freq);
    iip = false;
  }
}

/********************************* utility functions below *******************************/
void printHeader() {
  display.clearDisplay();
  display.setTextColor(SH110X_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("  Vertebra  |   hz");
  display.setCursor(4, 16);
}

void printFreq() {
  char *name = v.names[v.idx];
  display.setTextSize(2);
  display.setTextColor(SH110X_WHITE);
  display.print(name);
  display.print(" ");
  display.setTextColor(SH110X_BLACK, SH110X_WHITE);
  display.println(out_freq);
}

void printVert() {
  char *name = v.names[v.idx];
  display.setTextSize(2);
  display.setTextColor(SH110X_BLACK, SH110X_WHITE);
  display.print(name);
  display.setTextColor(SH110X_WHITE);
  display.print(" ");
  display.println(out_freq);
}

// ensures the array wrapping works
int8_t mod(int8_t a, int8_t b) {
  return (a % b + b) % b;
}