#include <FastLED.h>
#include "ServoTimer2.h"
//#include <Servo.h>
#include <SD.h>
#include <TMRpcm.h>
#define SD_ChipSelectPin 4
#define SPEAKER_PIN 9
TMRpcm audio;

#define BAR_LEDS 5
#define RING_LEDS 8
#define BAR_PIN 8
#define RING_PIN 5

ServoTimer2 doorServo;
//Servo doorServo;

CRGB LED_bar[BAR_LEDS];
CRGB LED_ring[RING_LEDS];
uint16_t brightnessScale = 150;
uint16_t indexScale = 100;

DEFINE_GRADIENT_PALETTE( es_emerald_dragon_16_gp ) {
  0, 255, 255, 255,
  127,  198, 224, 112,
  204,  106, 255,  0,
  255,  25, 52, 77
};

DEFINE_GRADIENT_PALETTE( gr65_hult_gp ) {
  0, 255, 255, 255,
  48, 108, 4, 133,
  89, 143, 24, 172,
  160,   15, 237, 137,
  216,   255, 45, 0,
  255,   0, 0, 0
};

CRGBPalette16 greenPal = es_emerald_dragon_16_gp;
CRGBPalette16 purplePal = gr65_hult_gp;

// #############   GLobal Vars ##############
int trapOpen = 0;
int openAngle = 60;
int closeAngle = 120;
unsigned long lastTick;
unsigned long currentMillis;

// #############   Door Vars ##############


// #############   Light Bar Vars ##############
int barHeight = 0;
int len;
int prevLen;
int lightMode = 0;

// #############   Pedal Vars ##############
int pedalPin = A0;
int reading = 100.0;
int pedalSum;
int pedalAvg;

void setup() {
  Serial.begin(57600);
  if(!SD.begin(4)){
    Serial.println("SD Init failed");
    while(1);
  }
  //audio setup
  audio.speakerPin = SPEAKER_PIN;
  audio.setVolume(5);
  audio.disable();
  audio.quality(1);
  audio.loop(0);

  //Serial.println("resetting");
  FastLED.addLeds<WS2812, BAR_PIN, GRB>(LED_bar, BAR_LEDS);
  FastLED.addLeds<WS2812, RING_PIN, GRB>(LED_ring, RING_LEDS);
  FastLED.setBrightness(84);
  fill_solid(LED_bar, BAR_LEDS, CRGB::Black);
  fill_solid(LED_ring, RING_LEDS, CRGB::Black);
  FastLED.show();
  //doorServo.write(closeAngle);
  doorServo.write(map(closeAngle, 0, 180, 750, 2250));
  doorServo.attach(6);

}

void loop() {
  animateBar(lightMode);
  animateRing(lightMode);
  if (trapOpen == 0) {
    EVERY_N_MILLIS(100) {
      pedalCheck();
    }
  } else {
    if (millis() - currentMillis > 5000) {
      closeTrap();
    }
  }
}

void animateBar(int mode) {
  switch (mode) {

    case 0:
      //idle
      if (!prevLen) prevLen = 0;
      if (!len) len = BAR_LEDS - 1;
      if (len > prevLen) {
        for (int i = prevLen; i <= len; i++) {
          for (int j = 0; j < 256; j += 8) {
            LED_bar[i] = CHSV(40, 255, j);
            FastLED.show();
          }
        }
      } else if (prevLen > len) {
        for (int i = prevLen; i >= len; i--) {
          for (int j = 255; j > 0; j -= 8) {
            LED_bar[i] = CHSV(40, 255, j);
            FastLED.show();
          }
        }
      }
      prevLen = len;
      len = random(0, BAR_LEDS - 1);
      break;

    case 1:
      //alarm
      //Serial.println("alarm");
      EVERY_N_MILLIS(100) {
        for (int k = 0; k < BAR_LEDS; k++) {
          int l = random(1, 3);
          l == 1 ? LED_bar[k] = CHSV(0, 0, 0) : LED_bar[k] = CHSV(0, 255, 255);
        }
        FastLED.show();
      }
      break;
  }
}

void animateRing(int mode) {
  switch (mode) {

    case 0:
      //idle
      EVERY_N_MILLIS(100) {
        for (int i = 0; i < RING_LEDS; i++) {
          LED_ring[i] = ColorFromPalette(greenPal, random(256));
        }
      }
      break;

    case 1:
      //alarm
      EVERY_N_MILLIS(50) {
        for (int i = 0; i < RING_LEDS; i++) {
          LED_ring[i] = ColorFromPalette(purplePal, random(256));
        }
      }
      break;
  }
  FastLED.show();
}


void pedalCheck() {
  if (trapOpen == 0) {
    pedalSum = 0;
    for (int i = 0; i < 16; i++) {
      pedalSum += analogRead(pedalPin);
    }
    pedalAvg = pedalSum / 16;
    reading = round(map(pedalAvg, 15, 104, 0, 100));
    if (reading <= 50) {
      openTrap();
      trapOpen = 1;
    }

  }
}

void openTrap() {
  //##TBD## start smoker
  //##TBD## start internal light show
  //##TBD## start capture sound effects
  
  //resets timer
  currentMillis = millis();
  //opens trap doors
  //doorServo.write(openAngle);
  doorServo.write(map(openAngle, 0, 180, 750, 2250));
  delay(15);
  //changes light bar sequence
  lightMode = 1;
  //sets trap status to open
  trapOpen = 1;
  audio.play("open.wav");
}

void closeTrap() {
  //##TBD## play closing sound
  //##TBD## rumble motor activate
  //##TBD## stop smoker
  //##TBD## flash red LED
  prevLen = 0;
  lightMode = 0;
  trapOpen = 0;
  //doorServo.write(closeAngle);
  doorServo.write(map(closeAngle, 0, 180, 750, 2250));
  audio.play("close.wav");
}