#include <SPI.h>
#include <AD9833.h>
#include "VCNL4010.h"
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <Adafruit_ST7789.h>
#include "Queue.h"
#include "LinearRegression.h"

#define FSYNC1 0
#define FSYNC2 1
#define LED_GRN 2
#define MENV 3
#define FRQ_MOSI 4
#define FRQ_SCK 6
#define PROXINT 8
#define LED_REED 9
#define SERVO1 10
#define SERVO2 11
#define TFT_DC 12
#define TFT_RST 13
#define TFT_MOSI 28
#define TFT_MISO 29
#define TFT_SCK 30
#define TFT_CS 31
#define RIN1 17
#define RIN2 18
#define RIN3 19
#define RIN4 20
#define SW_TYP 23
#define SW_SEL 24
#define DBG_RXD 25
#define DBG_TXD 26
#define SW_FUP 14
#define SW_FDN 27
#define TFT_CS 31

#define FREQ_STEP 1.0
#define FREQ_DELAY 10
#define UPDATE_DELAY 100
#define MIN_FREQ 65
#define MAX_FREQ 1500
#define MIN_FREQ_CHALLENGE 300
#define MAX_FREQ_CHALLENGE 1500
#define ETHANOL_DELAY_MS 15000
#define PROCESSING_QUEUE_LENGTH 512
#define THRESHOLD_QUEUE_LENGTH 256
#define REGRESSED_QUEUE_LENGTH 64
#define CORREL_QUEUE_LENGTH 5
#define QUEUE_ITER(mi, _q, ...) { \
    int _idx = _q.front(); \
    int _n = 0; \
    for (;;) { \
      if (_n == _q.count()) { \
        break; \
      } \
      auto item = _q.unsafe_peek(_idx); \
      __VA_ARGS__ \
      _idx++; \
      _n++; \
      if (_idx > mi) { \
        _idx -= (mi + 1); \
      } \
    } \
  }


AD9833 freq1(FSYNC2);
AD9833 freq2(FSYNC1);
VCNL4010 Sensor;
HardwareSerial Debug(DBG_RXD, DBG_TXD);
Servo servo1;
Servo servo2;
SPIClass tft_spi = SPIClass(TFT_MOSI, TFT_MISO, TFT_SCK, TFT_CS);
Adafruit_ST7735 tft = Adafruit_ST7735(&tft_spi, TFT_CS, TFT_DC, TFT_RST);
//Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCK, TFT_RST);

const uint32_t FANCY_CHANGE_THRESH = 10;
const uint32_t MIN_CORREL_READINGS = 5;
const uint32_t CORREL_MAX_DURATION_MS = 300;
const uint32_t CORREL_MAX_GAP_MS = 1500;
const uint32_t NUM_CONSECUTIVE_PTS = 5;
const uint32_t CORREL_MIN_RANGE = 400;
const double CORREL_LOW_RVALUE = 0.3;
uint32_t rd_completion_t = 0;
volatile bool rd_spinning_data[NUM_CONSECUTIVE_PTS] = {false};
volatile bool rd_spinning_forward = false;
uint32_t last_correlation_t = 0;
uint32_t num_reliable_points = 0;
bool serial_debug_rd = false;
void init_servo() {
  Debug.println("INIT: servos");
  // NOTE servos swapped to make cabling easier
  servo1.attach(SERVO2);
  servo2.attach(SERVO1);
  servos_closed();
}
const uint8_t SERVO1_CLOSED = 175;
const uint8_t SERVO2_CLOSED = 18;
const uint8_t SERVO1_OPEN = 0;
const uint8_t SERVO2_OPEN = 180;
void servos_closed() {
  servo1.write(SERVO1_CLOSED);
  servo2.write(SERVO2_CLOSED);
}
void init_tft() {
  Debug.println("INIT: tft");
  tft_spi.begin();
  tft.initR(INITR_144GREENTAB);
  tft.fillScreen(ST77XX_BLACK);
  tft.println("Weizmann!");
}
void init_ctl() {
  Debug.println("INIT: ctl");
  pinMode(LED_REED, OUTPUT);
  pinMode(LED_GRN, OUTPUT);
  digitalWrite(LED_REED, HIGH);
  digitalWrite(LED_GRN, HIGH);
  delay(1000);
  digitalWrite(LED_REED, LOW);
  digitalWrite(LED_GRN, LOW);
  pinMode(SW_TYP, INPUT);
  pinMode(SW_FUP, INPUT);
  pinMode(SW_SEL, INPUT);
  pinMode(SW_FDN, INPUT);
}
void init_radiometer() {
  Debug.println("INIT: radiometer");
  while (!Sensor.begin(0x13, I2C_HIGH_SPEED_MODE)) {
    Debug.println("ERR: Couldn't initialize sensor!");
    delay(1000);
  }
  Sensor.setProximityHz(250);
  Sensor.setAmbientLight(2, 32);
  Sensor.setLEDmA(200);
  Sensor.setProximityContinuous(true);
  delay(500);
  uint32_t cur = Sensor.getProximity();
  randomSeed(cur);
}
void init_pins() {
  pinMode(PB1, OUTPUT);
  pinMode(MENV, INPUT);
}
void init_relay() {
  pinMode(RIN1, OUTPUT);
  pinMode(RIN2, OUTPUT);
  pinMode(RIN3, OUTPUT);
  pinMode(RIN4, OUTPUT);
  digitalWrite(RIN1, HIGH);
  digitalWrite(RIN2, HIGH);
  digitalWrite(RIN3, HIGH);
  digitalWrite(RIN4, HIGH);
}
void init_freq() {
  Debug.println("INIT: freq");
  freq1.Begin();
  freq2.Begin();
  freq1.SleepMode(false);
  freq2.SleepMode(false);
}
void test_ctl() {
  Debug.println("TEST: ctl");
  digitalWrite(LED_REED, HIGH);
  digitalWrite(LED_GRN, HIGH);
  Debug.println("Press SW_FUP");
  while (digitalRead(SW_FUP) == LOW) {}
  Debug.println("Press SW_FDN");
  while (digitalRead(SW_FDN) == LOW) {}
  Debug.println("Press SW_SEL");
  while (digitalRead(SW_SEL) == LOW) {}
  Debug.println("Press SW_TYP");
  while (digitalRead(SW_TYP) == LOW) {}
  Debug.println("Test complete!");
  digitalWrite(LED_REED, LOW);
  digitalWrite(LED_GRN, LOW);
}
void setup() {
  Debug.begin(115200);
  Debug.println("The Physics Penitentiary");
  Debug.println("Control system code - theta.eu.org\n");
  pinMode(PROXINT, INPUT);
  randomSeed(analogRead(PROXINT));
  init_pins();
  init_ctl();
  //init_tft();
  init_freq();
  init_radiometer();
  init_servo();
  init_relay();
  delay(1000);
  digitalWrite(PB1, HIGH);
  if (digitalRead(SW_SEL) == HIGH) {
    digitalWrite(LED_REED, HIGH);
    digitalWrite(LED_GRN, HIGH);
    Debug.println("*** DEBUG MODE ACTIVATED");
    Debug.println("SW_FUP for servos\nSW_FDN for ethanol\nSW_SEL for debug start\nSW_TYP for debug end");
    while (digitalRead(SW_SEL) == HIGH) {}
    delay(1000);
    serial_debug_rd = true;
    digitalWrite(LED_REED, LOW);
    digitalWrite(LED_GRN, LOW);
    for (;;) {
      if (digitalRead(SW_FUP) == HIGH) {
        Debug.println("Servo debug");
        for (;;) {
          servo_debug();
        }
      }
      if (digitalRead(SW_FDN) == HIGH) {
        Debug.println("Ethanol debug");
        ethanol_debug();
      }
      if (digitalRead(SW_SEL) == HIGH) {
        Debug.println("Debug radiometer start");
        for (;;) {
          rd_challenge_start();
        }
      }
      if (digitalRead(SW_TYP) == HIGH) {
        Debug.println("Debug radiometer end");
        for (;;) {
          rd_even_fancier();
        }
      }
    }
  }
  //test_relay();
  //test_marbles();
  drop_marble(false);
  test_freq();
  //test_ctl();
  //freq_challenge();
  digitalWrite(LED_REED, HIGH);
}
void test_marbles() {
  Debug.println("TEST: marbles");
  drop_marble(true);
  delay(1000);
  drop_marble(false);
}
void ethanol_debug() {
  for (;;) {
    if (digitalRead(SW_SEL) == HIGH) {
      digitalWrite(RIN4, LOW);
    }
    else {
      digitalWrite(RIN4, HIGH);
    }
  }
}
void drop_marble(bool top) {
  Debug.print("Dropping marble: ");
  Debug.println(top);
  if (top) {
    servo1.write(SERVO1_OPEN);
    servo2.write(SERVO2_CLOSED);
  }
  else {
    servo1.write(SERVO1_CLOSED);
    servo2.write(SERVO2_OPEN);
  }
  delay(750);
  servos_closed();
}
void test_relay() {
  Debug.println("TEST: relay");
  digitalWrite(RIN1, LOW);
  delay(500);
  digitalWrite(RIN2, LOW);
  delay(500);
  digitalWrite(RIN3, LOW);
  delay(500);
  digitalWrite(RIN4, LOW);
  delay(2000);
  digitalWrite(RIN1, HIGH);
  digitalWrite(RIN2, HIGH);
  digitalWrite(RIN3, HIGH);
  digitalWrite(RIN4, HIGH);
  delay(1000);
}
void release_ethanol() {
  digitalWrite(RIN4, LOW);
  delay(1260);
  digitalWrite(RIN4, HIGH);
}
void test_freq() {
  Debug.println("TEST: freq1");
  freq1.ApplySignal(SINE_WAVE, REG0, 587.33);
  freq1.EnableOutput(true);
  delay(250);
  Debug.println("TEST: freq2");
  freq2.ApplySignal(SINE_WAVE, REG0, 466.16);
  freq2.EnableOutput(true);
  delay(250);
  freq1.EnableOutput(false);
  freq2.EnableOutput(false);
}
double cur_freq = 440.0;
uint8_t cur_wave_i = 0;
WaveformType int_to_wavetype(uint8_t i) {
  switch (i % 3) {
    case 0:
      return SINE_WAVE;
      break;
    case 1:
      return TRIANGLE_WAVE;
      break;
    case 2:
      return SQUARE_WAVE;
      break;
    default:
      return SINE_WAVE;
      break;
  }
}
uint8_t challenge_wavetype = 0;
double challenge_freq_d = 0.0;
void freq_init_challenge() {
  Debug.print("Initializing frequency challenge: type=");
  challenge_wavetype = random(0, 3);
  Debug.print(challenge_wavetype);
  Debug.print(" freq=");
  uint32_t challenge_freq = random(MIN_FREQ_CHALLENGE, MAX_FREQ_CHALLENGE);
  challenge_freq_d = (double) challenge_freq;
  Debug.println(challenge_freq);
  freq1.ApplySignal(int_to_wavetype(challenge_wavetype), REG0, (double) challenge_freq);
  cur_freq = 440.0;
  cur_wave_i = 0;
  freq_update();
  freq1.EnableOutput(true);
  freq2.EnableOutput(true);
}
uint32_t last_update_t = 0;
uint32_t freq_score = 0;
void freq_update() {
  Debug.print("Updating user wave: freq=");
  Debug.print(cur_freq);
  Debug.print(" type=");
  Debug.println(cur_wave_i);
  freq2.ApplySignal(int_to_wavetype(cur_wave_i), REG0, cur_freq);
}
void freq_stop() {
  freq1.EnableOutput(false);
  freq2.EnableOutput(false);
}
void rd_challenge_start() {
  Debug.println("*** CHALLENGE 1 INIT");
  for (;;) {
    uint32_t now = millis();
    rd_even_fancier();
    if (rd_completion_t > now) {
      // We just got a new set of readings, just now.
      Debug.print("*** CHALLENGE 1 - Got reading=");
      Debug.println(rd_spinning_forward);
      if (rd_spinning_forward == 0) {
        break;
      }
    }
  }
  Debug.println("*** CHALLENGE 1 COMPLETED");
  drop_marble(true);
  success();
}
void rd_challenge_end() {
  static uint32_t last_ethanol_time = 0;

  Debug.println("*** CHALLENGE 3 INIT");
  for (;;) {
    uint32_t now = millis();
    rd_even_fancier();
    if ((now - last_ethanol_time) >= ETHANOL_DELAY_MS) {
      digitalWrite(LED_GRN, HIGH);
      if (digitalRead(SW_SEL) == HIGH) {
        release_ethanol();
        last_ethanol_time = now;
      }
    }
    else {
      digitalWrite(LED_GRN, LOW);
    }
    if (rd_completion_t > now) {
      // We just got a new set of readings, just now.
      Debug.print("*** CHALLENGE 3 - Got reading=");
      Debug.println(rd_spinning_forward);
      if (rd_spinning_forward == 1) {
        break;
      }
    }
  }
  Debug.println("*** CHALLENGE 3 COMPLETED");
  drop_marble(false);
  success();
}
void success() {
  for (int i = 0; i < 10; i++) {
    digitalWrite(LED_GRN, HIGH);
    delay(100);
    digitalWrite(LED_GRN, LOW);
    delay(100);
  }
}
void ethanol_challenge() {

}
void freq_challenge() {
  Debug.println("*** CHALLENGE 2 INIT");
  freq_init_challenge();
  for (;;) {
    bool changed = false;
    if (digitalRead(SW_FUP) == HIGH && cur_freq < MAX_FREQ) {
      cur_freq += FREQ_STEP;
      changed = true;
      delay(FREQ_DELAY);
    }
    if (digitalRead(SW_FDN) == HIGH && cur_freq > MIN_FREQ) {
      cur_freq -= FREQ_STEP;
      changed = true;
      delay(FREQ_DELAY);
    }
    if (digitalRead(SW_TYP) == HIGH) {
      cur_wave_i = (cur_wave_i + 1) % 3;
      Debug.print("Wave type changed, type=");
      Debug.println(cur_wave_i);
      changed = true;
      digitalWrite(LED_GRN, HIGH);
      while (digitalRead(SW_TYP) == HIGH) {}
      delay(100);
      digitalWrite(LED_GRN, LOW);
    }
    if (changed) {
      freq_update();
    }
    if ((fabs(cur_freq - challenge_freq_d) < 10.0) && cur_wave_i == challenge_wavetype) {
      freq_score += 1;
      delay(1);
      if (freq_score > 3000) {
        Debug.println("Matching!");
        break;
      }
    }
    else {
      freq_score = 0;
    }
  }
  Debug.println("*** CHALLENGE 2 COMPLETED");
  freq_stop();
  success();
}
// EVEN NEWER FANCY RADIOMETER CODE
Queue<uint32_t> readings = Queue<uint32_t>(THRESHOLD_QUEUE_LENGTH);
Queue<uint32_t> regreadings = Queue<uint32_t>(REGRESSED_QUEUE_LENGTH);
uint32_t threshold = 0;
uint32_t mean = 0;
uint32_t median = 0;
uint32_t stddev = 0;
uint32_t correl_start_t = 0;
uint32_t range = 0;
double regression_values[3] = {0};
double correl = 0;
void cleanup_regression() {
  regreadings.clear();
  correl_start_t = 0;
}
void update_threshold() {
  int n = readings.count();
  uint32_t sum = 0;
  uint32_t sum_squares = 0;
  uint32_t _maxval = 0;
  uint32_t _minval = UINT32_MAX;
  QUEUE_ITER(THRESHOLD_QUEUE_LENGTH, readings, {
    if ((n / 2) == _n) {
      median = item;
    }
    if (item > _maxval) {
      _maxval = item;
    }
    if (item < _minval) {
      _minval = item;
    }
    sum += item;
    sum_squares += pow(item, 2);
  });
  if (n == 0) {
    threshold = UINT32_MAX;
    return;
  }
  mean = sum / n;
  stddev = sqrt((sum_squares / n) - pow(mean, 2));
  threshold = mean - (stddev / 4);
  range = _maxval - _minval;
}
void process_regression() {
  uint32_t x = 0;
  uint32_t correl_t = millis() - correl_start_t;
  if (correl_t > CORREL_MAX_DURATION_MS) {
    cleanup_regression();
    return;
  }
  LinearRegression reg = LinearRegression();
  QUEUE_ITER(REGRESSED_QUEUE_LENGTH, regreadings, {
    reg.learn((double) x, (double) item);
    x++;
  });
  reg.getValues(regression_values);
  correl = reg.correlation();
  Debug.print("Completed regression: m=");
  Debug.print(regression_values[0]);
  Debug.print(" n=");
  Debug.print(regreadings.count());
  Debug.print(" correl=");
  Debug.print(correl);
  Debug.print(" threshold=");
  Debug.print(threshold);
  Debug.print(" stddev=");
  Debug.print(stddev);
  Debug.print(" range=");
  Debug.println(range);
  if (range < CORREL_MIN_RANGE) {
    Debug.println("Rejecting low range reading.");
    cleanup_regression();
    return;
  }
  if (fabs(correl) < CORREL_LOW_RVALUE) {
    Debug.println("Rejecting low R-value.");
    cleanup_regression();
    return;
  }
  if ((millis() - last_correlation_t) > CORREL_MAX_GAP_MS) {
    Debug.println("Starting new regression series (timeout).");
    num_reliable_points = 0;
  }
  Debug.print("Regression datapoint part of series, n=");
  Debug.println(num_reliable_points);
  rd_spinning_data[num_reliable_points] = (regression_values[0] > 0);
  if (num_reliable_points + 1 == NUM_CONSECUTIVE_PTS) {
    int votes = 0;
    for (int i = 0; i < NUM_CONSECUTIVE_PTS; i++) {
      if (rd_spinning_data[i]) {
        votes++;
      }
    }
    rd_spinning_forward = (votes > (NUM_CONSECUTIVE_PTS / 2));
    Debug.print("*** SERIES COMPLETE - value=");
    Debug.println(rd_spinning_forward);
    rd_completion_t = millis();
    num_reliable_points = 0;
  }
  else {
    num_reliable_points++;
  }
  last_correlation_t = millis();
  cleanup_regression();
}
void rd_even_fancier() {
  uint32_t cur = Sensor.getProximity();
  if (serial_debug_rd) {
    Debug.println(cur);
  }
  static uint32_t last = 0;
  static bool was_above = false;
  int32_t delta = 0;
  delta = cur - last;
  last = cur;
  if (readings.count() >= THRESHOLD_QUEUE_LENGTH) {
    readings.pop();
  }
  readings.push(cur);
  update_threshold();
  if (cur < threshold) {
    if (abs(delta) < FANCY_CHANGE_THRESH) {
      if (correl_start_t == 0) {
        correl_start_t = millis();
      }
      regreadings.push(cur);
    }
  }
  else {
    if (regreadings.count() > MIN_CORREL_READINGS) {
      process_regression();
    }
  }
}
void servo_debug() {
  //Debug.println("*** Servo manual control");
  static uint32_t servo1_v = SERVO1_OPEN;
  static uint32_t servo2_v = SERVO2_OPEN;
  static bool is_servo1 = false;
  servo1.write(servo1_v);
  servo2.write(servo2_v);
  if (is_servo1) {
    digitalWrite(LED_GRN, HIGH);
    while (digitalRead(SW_FUP) == HIGH) {
      servo1_v++;
      servo1.write(servo1_v);
      Debug.print("servo1: ");
      Debug.println(servo1_v);
      delay(100);
    }
    while (digitalRead(SW_FDN) == HIGH) {
      if (servo1_v == 0) {
        delay(100);
        continue;
      }
      servo1_v--;
      servo1.write(servo1_v);
      Debug.print("servo1: ");
      Debug.println(servo1_v);
      delay(100);
    }
    while (digitalRead(SW_SEL) == HIGH) {
      is_servo1 = false;
      delay(1000);
    }
  }
  else {
    digitalWrite(LED_GRN, LOW);
    while (digitalRead(SW_FUP) == HIGH) {
      servo2_v++;
      servo2.write(servo2_v);
      Debug.print("servo2: ");
      Debug.println(servo2_v);
      delay(100);
    }
    while (digitalRead(SW_FDN) == HIGH) {
      if (servo2_v == 0) {
        delay(100);
        continue;
      }
      servo2_v--;
      servo2.write(servo2_v);
      Debug.print("servo2: ");
      Debug.println(servo2_v);
      delay(100);
    }
    while (digitalRead(SW_SEL) == HIGH) {
      is_servo1 = true;
      delay(1000);
    }
    Debug.println(analogRead(PROXINT));
  }

}
void loop() {
  rd_challenge_start();
  freq_challenge();
  rd_challenge_end();
  success();
  delay(10000);
}