+#include <math.h>
+
 #include "Adafruit_SSD1306.h"
 
 #include "FiniteStateMachine.h"
 #include "AlarmManager.h"
+#include "StringStream.h"
+
+SYSTEM_MODE(SEMI_AUTOMATIC);
 
 /*==================== MAIN ====================*/
 
-State Start = State(enterStart, updateStart, NULL);
+State Start = State(NULL, updateStart, exitStart);
 State Clock = State(updateClock);
-State Alarm = State(enterAlarm, updateAlarm, NULL);
+State Alarm = State(enterAlarm, updateAlarm, exitAlarm);
 
 FSM stateMachine = FSM(Start);
 

#define OLED_RESET  D5
Adafruit_SSD1306 Display(OLED_DC, OLED_RESET, OLED_CS);

#define ALARM_LENGTH 10000
AlarmManager alarms = AlarmManager();

#define STARTUP_LENGTH 10000
#define INTERVAL 100

unsigned long stateMillis;
unsigned long previousMillis;


      return;
    }
  }
}

void enterAlarm() {
  stateMillis = millis();
  RGB.color(0, 0, 255);

  iterator = new StringStream(song);

  iterator->find(":"); // Skip name

  if(iterator->find("d")) default_duration = iterator->parseInt();

  if(iterator->find("o")) default_octave = iterator->parseInt();

  if(iterator->find("b")) bpm = iterator->parseInt();

  iterator->find(":");

  wholenote = (60 * 1000L / (float)bpm) * 4;

  Serial.println("Default Duration: " + String(default_duration));
  Serial.println("Default Octave: " + String(default_octave));
  Serial.println("BPM: " + String(bpm));
  Serial.println();
}

void updateAlarm() {
  int noteSize;
  unsigned long currentMillis = millis();

  RGB.brightness(map(abs((currentMillis % 2000) - 1000), 0, 1000, 0, 255));

  render();

  if(currentMillis - previousMillis > duration) {
    if(iterator->available()) {
      uint8_t sharp = 0;

      Serial.print((char)iterator->peek()); Serial.print(' ');
      if(isdigit((char)iterator->peek())) {
        noteSize = iterator->parseInt();
        duration = wholenote / noteSize;
      } else {
        duration = wholenote / default_duration;
      }

      switch((char)iterator->read()) {
        case 'a': note = NOTE_A; sharp = 13; break;
        case 'b': note = NOTE_B; sharp =  0; break;
        case 'c': note = NOTE_C; sharp = 15; break;
        case 'd': note = NOTE_D; sharp = 17; break;
        case 'e': note = NOTE_E; sharp =  0; break;
        case 'f': note = NOTE_F; sharp = 21; break;
        case 'g': note = NOTE_G; sharp = 23; break;
        case 'p': note = NOTE_P; sharp =  0; break;
      }

      if((char)iterator->peek() == '#') {
        note += sharp;
        iterator->read();
      }

      if((char)iterator->peek() == '.') {
        duration += duration / 2;
        iterator->read();
      }

      if(isdigit((char)iterator->peek())) {
        octave = iterator->parseInt();
      } else {
        octave = default_octave;
      }

      if((char)iterator->peek() == ',') iterator->read();

      Serial.print("Note Size: " + String(noteSize) + " ");
      Serial.print("Duration: " + String(duration) + " ");
      Serial.print("Octave: " + String(octave) + " ");
      if(note) {
        Serial.println("Note: " + String(note * pow(2, octave - min_octave)));
        tone(speakerPin, note * pow(2, octave - min_octave), duration);
      } else {
        Serial.println("Rest");
        noTone(speakerPin);
      }
    } else {
      /*Serial.println("Done.");*/
      iterator = new StringStream(song);
      iterator->find(":");
      iterator->find(":");
    }

    previousMillis = currentMillis;
  }

  if(currentMillis - stateMillis >= ALARM_LENGTH)
    stateMachine.transitionTo(Clock);
}

void exitAlarm() {
  RGB.color(0, 0, 0);
  RGB.brightness(0);
  noTone(speakerPin);
}

void render() {
  int hour   = Time.hourFormat12();
  int minute = Time.minute();
  int second = Time.second();

  Display.clearDisplay();
  Display.setTextSize(3);
  Display.setCursor(12, 22);

  if(hour < 10) Display.print(' ');
  Display.print(hour);
  Display.print(second % 2 == 0 ? ' ' : ':');
  if(minute < 10) Display.print('0');
  Display.print(minute);

  Display.setTextSize(1);

  if(Time.isAM()) {
    Display.print("AM");
  } else {
    Display.setCursor(102, 37);
    Display.print("PM");
  }

  Display.display();
}

int addAlarm(String value) {
  return alarms.add(value) ? 1 : -1;
}

int removeAlarm(String value) {
  return alarms.remove(value) ? 1 : -1;
}

int clearAlarms(String value) {
  return alarms.clear() ? 1 : -1;
}
