#include <Audio.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
#include <play_sd_mp3.h> // https://github.com/FrankBoesing/Arduino-Teensy-Codec-lib
//#include <play_sd_aac.h>
#include <FastLED.h>
#include "easing.h"
#include <NXPMotionSense.h>
#include <EEPROM.h>

//AudioPlaySdWav           playSdWav1;     //xy=154,422
AudioPlaySdMp3           playMp31;
AudioMixer4              mixer1;
AudioOutputAnalog        dac1;
AudioConnection          patchCord1(playMp31, 0, mixer1, 0);
AudioConnection          patchCord2(playMp31, 1, mixer1, 1);
AudioConnection          patchCord3(mixer1, dac1);

NXPMotionSense imu;
NXPSensorFusion filter;

#define LEFT_RING_NEO_PIN 2
#define RIGHT_RING_NEO_PIN 3
#define N_LEDS 16

CRGB ringL[N_LEDS];
CRGB ringR[N_LEDS];

#define PROP_AMP_ENABLE    5
#define FLASH_CHIP_SELECT  6
//#define FLASH_CHIP_SELECT 21  // Arduino 101 built-in SPI Flash

uint8_t max_bright = 128;                                      // Overall brightness definition.

void setup() {
  Serial.begin(115200);
  AudioMemory(8); //4
  delay(2000);
  imu.begin();
  filter.begin(100);

  FastLED.addLeds<NEOPIXEL, LEFT_RING_NEO_PIN>(ringL, N_LEDS);
  FastLED.addLeds<NEOPIXEL, RIGHT_RING_NEO_PIN>(ringR, N_LEDS);
  FastLED.setBrightness(max_bright);

  // Start SerialFlash
  if (!SerialFlash.begin(FLASH_CHIP_SELECT)) {
    while (1) {
      Serial.println ("Cannot access SPI Flash chip");
      delay (1000);
    }
  }

  listFiles();

  //Set Volume
  mixer1.gain(0, 2);
  mixer1.gain(1, 2);

  //Start Amplifier
  pinMode(PROP_AMP_ENABLE, OUTPUT);
  digitalWrite(PROP_AMP_ENABLE, HIGH);    // Enable Amplifier

  fill_solid(ringL, N_LEDS, CRGB::Green);
  fill_solid(ringR, N_LEDS, CRGB::Green);
  FastLED.show();

  //Startup sound
  playFile("WELC.MP3");
}

void listFiles(){
  Serial.println("All Files on SPI Flash chip:");

  if (!SerialFlash.begin(FLASH_CHIP_SELECT)) {
    error("Unable to access SPI Flash chip");
  }

  SerialFlash.opendir();
  while (1) {
    char filename[64];
    uint32_t filesize;

    if (SerialFlash.readdir(filename, sizeof(filename), filesize)) {
      Serial.print("  ");
      Serial.print(filename);
      spaces(20 - strlen(filename));
      Serial.print("  ");
      Serial.print(filesize);
      Serial.print(" bytes");
      Serial.println();
    } else {
      break; // no more files
    }
  }
}

void spaces(int num) {
  for (int i=0; i < num; i++) {
    Serial.print(" ");
  }
}

void error(const char *message) {
  while (1) {
    Serial.println(message);
    delay(2500);
  }
}

void playFile(const char *filename)
{
  SerialFlashFile ff = SerialFlash.open(filename);
  Serial.print("Playing file: ");
  Serial.println(filename);

  uint32_t sz = ff.size();
  uint32_t pos = ff.getFlashAddress();

  // Start playing the file.  This sketch continues to
  // run while the file plays.
  playMp31.play(pos,sz);

  // Simply wait for the file to finish playing.
  //while (playMp31.isPlaying()) {
   //yield();
  //}
}


void loop() {
  if(!playMp31.isPlaying()){
    playFile("R301.MP3");
  }
  readMotionSensor();
}

void readMotionSensor(){
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    // Read the motion sensors
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);

    // Update the SensorFusion filter
    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    //print the data
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
    Serial.print("Accel X: ");
    Serial.println(ax);
    Serial.print("Gyro X: ");
    Serial.println(gx);
    Serial.print("Accel Y: ");
    Serial.println(ay);
    Serial.print("Gyro Y: ");
    Serial.println(gy);
    Serial.print("Accel Z: ");
    Serial.println(az);
    Serial.print("Gyro Z: ");
    Serial.println(gz);
    Serial.println("=============");
  }
}