#include "VCNL4010.h"
#include "Queue.h"
#include "LinearRegression.h"
#include <stdint.h>
#include <assert.h>

#define PROCESSING_QUEUE_LENGTH 512
#define THRESHOLD_QUEUE_LENGTH 256
#define REGRESSED_QUEUE_LENGTH 64
#define CORREL_QUEUE_LENGTH 5
#define PIN_LED_RED 7
#define PIN_LED_GREEN 6
#define PIN_SENSOR_INT 18
#define CORREL_THRESHOLD 0.3
#define CORREL_MIN_POINTS 15
#define CORREL_MAX_DURATION_MS 250
#define QUEUE_ITER(_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 > _q._maxitems) { \
        _idx -= (_q._maxitems + 1); \
      } \
    } \
  }

VCNL4010 Sensor;
HardwareSerial 
void setup() {
  // put your setup code here, to run once:
  pinMode(PB1, OUTPUT);
  Serial.begin(500000);
  digitalWrite(PB1, LOW);
  pinMode(PIN_LED_RED, OUTPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  digitalWrite(PIN_LED_RED, HIGH);
  digitalWrite(PIN_LED_GREEN, LOW);
  while (!Serial) {}
  Serial.println("Initializing");
//  attachInterrupt(PIN_SENSOR_INT, new_data_isr, FALLING);
  while (!Sensor.begin(0x13, I2C_HIGH_SPEED_MODE)) {
    digitalWrite(PIN_LED_RED, HIGH);
    digitalWrite(PIN_LED_GREEN, HIGH);
    Serial.println("Couldn't initialize sensor!");
    delay(1000);
  }
  Serial.println("Sensor initialized");
  digitalWrite(PB1, HIGH);
  digitalWrite(PIN_LED_RED, LOW);
 // Sensor.setInterrupt(1, true, false, false, false, 0, 65536);
  Sensor.setProximityHz(250);
  Sensor.setAmbientLight(2, 32);
  Sensor.setLEDmA(200);
  Sensor.setProximityContinuous(true);
}
Queue<uint32_t> toproc = Queue<uint32_t>(PROCESSING_QUEUE_LENGTH);
Queue<uint32_t> readings = Queue<uint32_t>(THRESHOLD_QUEUE_LENGTH);
Queue<uint32_t> regreadings = Queue<uint32_t>(REGRESSED_QUEUE_LENGTH);
Queue<double> correls = Queue<double>(CORREL_QUEUE_LENGTH);
uint32_t correl_start_t = 0;
uint32_t threshold = 0;
uint32_t mean = 0;
uint32_t median = 0;
uint32_t stddev = 0;
double regression_values[3] = {0};
double correl = 0;
void cleanup_regression() {
  regreadings.clear();
  correl_start_t = 0;
}
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(regreadings, {
    reg.learn((double) x, (double) item);
    x++;
  });
  reg.getValues(regression_values);
  correl = reg.correlation();
  if (correl > CORREL_THRESHOLD) {
    digitalWrite(PIN_LED_RED, HIGH);
  }
  else if (correl < -CORREL_THRESHOLD) {
    digitalWrite(PIN_LED_RED, LOW);
  }
  Serial.print("Completed regression: m=");
  Serial.print(regression_values[0]);
  Serial.print(" n=");
  Serial.print(regreadings.count());
  Serial.print(" correl=");
  Serial.print(correl);
  Serial.print(" correl_t=");
  Serial.print(correl_t);
  Serial.print(" threshold=");
  Serial.println(threshold);
  cleanup_regression();
}
void update_threshold() {
  int n = readings.count();
  uint32_t sum = 0;
  uint32_t sum_squares = 0;
  QUEUE_ITER(readings, {
    if ((n / 2) == _n) {
      median = 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 / 3);
}
void new_data_isr() {
  toproc.push(Sensor.getProximity());
  Sensor.clearInterrupt();
}
void loop() {
  for (;;) {
  //while (toproc.count() > 0) {
    uint32_t start = millis();
    if (readings.count() >= readings._maxitems) {
      readings.pop();
    }
    uint32_t prox = Sensor.getProximity();
    //uint32_t prox = toproc.pop();
    readings.push(prox);
    update_threshold();
    if (prox < threshold) {
      if (correl_start_t == 0) {
        correl_start_t = millis();
      }
      digitalWrite(PIN_LED_GREEN, HIGH);
      regreadings.push(prox);
    }
    else {
      if (regreadings.count() > CORREL_MIN_POINTS) {
        digitalWrite(PIN_LED_GREEN, LOW);
        process_regression();
      }
    }
    uint32_t endt = millis() - start;

    Serial.print(prox);
    Serial.print(",");
    Serial.print(mean);
    Serial.print(",");
    Serial.print(median);
    Serial.print(",");
    Serial.print(stddev);
    Serial.print(",");
    Serial.print(threshold);
    Serial.print(",");
    Serial.print(endt);
    Serial.println("");
  }
}