#include <Servo.h>
#include <mcp4xxx.h>

HardwareTimer rd_timer(1);
const uint32_t N_SAMPLES = 50;
const uint32_t TIMER_PERIOD_NANOSECONDS = 5000;
const uint32_t MEASUREMENT_THRESH = 200;
const uint32_t MASSIVE_CHANGE_THRESH = 250;
const uint32_t NUM_CONSECUTIVE_PTS = 5;
const uint32_t MIC_DROP_THRESHOLD = 200;
const uint32_t RADIOMETER_COOLDOWN_MS = 30000;
volatile bool rd_data_reliable = false;
volatile bool rd_spinning_data[NUM_CONSECUTIVE_PTS] = {false};
volatile bool rd_spinning_forward = false;
volatile uint32_t rd_num_reliable_pts = 0;
uint32_t audio_freq = 0;
enum challenge_t {
  RADIOMETER_ONE,
  WAVES,
  RADIOMETER_TWO
};
enum challenge_t cur_challenge = RADIOMETER_ONE;
uint32_t last_ethanol_time = 0;
uint32_t initial_mic_value = 0;
bool audio_on = false;

using namespace icecave::arduino;

MCP4XXX* pot_amplitude;
MCP4XXX* pot_coarse;
MCP4XXX* pot_fine;

Servo servo1;
Servo servo2;

#define PIN_RELAY1 PB4
#define PIN_RELAY2 PB5
#define PIN_RELAY3 PB6
#define PIN_RELAY4 PB7
#define PIN_BUTTON PB15
#define PIN_SERVO1 PA8
#define PIN_SERVO2 PA9
#define PIN_LED1 PA3
#define PIN_LED2 PA2
#define PIN_MIC PA1
#define PIN_RADIOMETER PA0
#define PIN_CS_AMPLITUDE PB0
#define PIN_CS_COARSE PB10
#define PIN_CS_FINE PB1

void setup() {
  // put your setup code here, to run once:
  pinMode(PIN_RADIOMETER, INPUT);
  pinMode(PIN_BUTTON, INPUT_PULLUP);
  pinMode(PIN_MIC, INPUT);
  pinMode(PIN_LED1, OUTPUT);
  pinMode(PIN_LED2, OUTPUT);
  pinMode(PIN_RELAY1, OUTPUT);
  pinMode(PIN_RELAY2, OUTPUT);
  pinMode(PIN_RELAY3, OUTPUT);
  pinMode(PIN_RELAY4, OUTPUT);
  pinMode(PC13, OUTPUT);
  digitalWrite(PIN_RELAY1, HIGH);
  digitalWrite(PIN_RELAY2, HIGH);
  digitalWrite(PIN_RELAY3, HIGH);
  digitalWrite(PIN_RELAY4, HIGH);
  pot_amplitude = new MCP4XXX(PIN_CS_AMPLITUDE);
  pot_coarse = new MCP4XXX(PIN_CS_COARSE);
  pot_fine = new MCP4XXX(PIN_CS_FINE);
  Serial.begin(9600);
  Serial.println("init servos");
  drop_marble(false);
  //delay(2000);
  digitalWrite(PIN_LED2, HIGH);
  digitalWrite(PIN_LED1, HIGH);
  Serial.println("audio test");
  for (;;) {
    test_audio(0, true);
    delay(5000);
    test_audio(255, true);
    delay(5000);
  }
  for (int i = 0; i < 255; i++) {
    test_audio(i, true);
    delay(5);
  }
  for (int i = 0; i < 255; i++) {
    test_audio(i, false);
    delay(5);
  }
  stop_audio();
  digitalWrite(PIN_LED2, HIGH);
  digitalWrite(PIN_LED1, LOW);
  Serial.println("init radiometer");
  start_timer();
  /*
    for (;;) {
    //digitalWrite(PIN_LED2, rd_data_reliable);
    if (rd_spinning_forward) {
      digitalWrite(PIN_LED1, HIGH);
    }
    else {
      digitalWrite(PIN_LED1, LOW);
    }
    }*/
  stop_timer();
}
void start_timer() {
  rd_timer.pause();
  rd_timer.setPeriod(TIMER_PERIOD_NANOSECONDS);
  rd_timer.setChannel1Mode(TIMER_OUTPUT_COMPARE);
  rd_timer.setCompare(TIMER_CH2, 1);
  rd_timer.attachCompare1Interrupt(rd_update_handler);
  rd_timer.refresh();
  rd_timer.resume();
}
void stop_timer() {
  rd_timer.pause();
}
void test_audio(uint32_t val, bool sine) {
  pot_amplitude->set(pot_amplitude->max_value());
  pot_coarse->set(val);
  pot_fine->set(0);
  if (sine) {
    digitalWrite(PIN_RELAY3, LOW);
  }
  else {
    digitalWrite(PIN_RELAY3, HIGH);
  }
  digitalWrite(PIN_RELAY4, LOW);
}
void start_audio() {
  pot_amplitude->set(pot_amplitude->max_value());
  pot_coarse->set(random(0, pot_coarse->max_value()));
  pot_fine->set(0);
  // pot_fine->set(random(0, pot_fine->max_value()));
  if (false) {
    digitalWrite(PIN_RELAY3, LOW);
  }
  else {
    digitalWrite(PIN_RELAY3, HIGH);
  }
  digitalWrite(PIN_RELAY4, LOW);
}
void stop_audio() {
  digitalWrite(PIN_RELAY4, HIGH);
  digitalWrite(PIN_RELAY3, HIGH);
}
void release_ethanol() {
  digitalWrite(PIN_RELAY2, LOW);
  delay(420);
  digitalWrite(PIN_RELAY2, HIGH);
}
void drop_marble(bool top) {
  servo1.attach(PIN_SERVO1);
  servo2.attach(PIN_SERVO2);
  if (!top) {
    servo1.write(90);
    servo2.write(170);

  }
  else {
    servo1.write(0);
    servo2.write(90);
  }
  delay(1000);
  servo1.write(0);
  servo2.write(170);
}
void challenge_waves() {
  Serial.println("starting audio");
  start_audio();
  delay(1000);
  initial_mic_value = analogRead(PIN_MIC);
  Serial.print("value ");
  Serial.println(initial_mic_value);
  int score = 0;
  delay(3000);
  for (;;) {
    Serial.println("looping");
    Serial.println(analogRead(PIN_MIC));
    if ((analogRead(PIN_MIC) + MIC_DROP_THRESHOLD) < initial_mic_value) {
      Serial.println("done");
      score++;
      delay(10);
    }
    if (score >= 10) {
      break;
    }
  }
  stop_audio();
}
void success() {
  for (int i = 0; i < 10; i++) {
    digitalWrite(PIN_LED1, HIGH);
    delay(100);
    digitalWrite(PIN_LED1, LOW);
    delay(100);
  }
}
void loop() {
  challenge_radiometer_one();
  success();
  delay(1000);
  challenge_waves();
  success();
  delay(5000);
  challenge_radiometer_two();
  success();
  success();
  success();
  delay(5000);
}
void challenge_radiometer_two() {
  start_timer();
  for (;;) {
    if (rd_data_reliable) {
      if (!rd_spinning_forward) {
        break;
      }
    }
    if (millis() <= (last_ethanol_time + RADIOMETER_COOLDOWN_MS)) {
      digitalWrite(PIN_LED1, LOW);
    }
    else {
      digitalWrite(PIN_LED1, HIGH);
      if (digitalRead(PIN_BUTTON) == LOW) {
        while (digitalRead(PIN_BUTTON) == LOW) {}
        for (int i = 0; i < 3; i++) {
          release_ethanol();
        }
        last_ethanol_time = millis();
        digitalWrite(PIN_LED1, LOW);
      }
    }
  }
  stop_timer();
  drop_marble(false);
}

void challenge_radiometer_one() {
  start_timer();
  for (;;) {
    if (rd_data_reliable) {
      if (rd_spinning_forward) {
        break;
      }
    }
  }
  stop_timer();
  drop_marble(true);
}
void rd_update_handler() {
  // put your main code here, to run repeatedly
  static volatile int smpl = 0;
  static volatile int last = 0;
  static volatile int delta = 0;
  int cur = analogRead(PIN_RADIOMETER);
  int maybe_delta = cur - last;
  if (abs(maybe_delta) <= MASSIVE_CHANGE_THRESH) {
    delta += maybe_delta;
  }
  //digitalWrite(PC13, LOW);
  last = cur;
  smpl++;
  if (smpl > N_SAMPLES) {
    if (abs(delta) < MEASUREMENT_THRESH) {
      rd_num_reliable_pts = 0;
      rd_data_reliable = false;
    }
    else {
      rd_num_reliable_pts++;
      memmove((void *) &rd_spinning_data[1], (void *) &rd_spinning_data[0], NUM_CONSECUTIVE_PTS - 1);
      rd_spinning_data[0] = (delta > 0);
      int votes = 0;
      for (int i = 0; i < NUM_CONSECUTIVE_PTS; i++) {
        if (rd_spinning_data[i]) {
          votes++;
        }
      }

      if (rd_num_reliable_pts > NUM_CONSECUTIVE_PTS) {
        rd_spinning_forward = (votes > (NUM_CONSECUTIVE_PTS / 2));
        rd_data_reliable = true;
      }
      //digitalWrite(PIN_LED2, HIGH);
    }
    delta = 0;
    smpl = 0;
  }
}