#include <Wire.h>
#include <SparkFun_WM8960_Arduino_Library.h> 
#include "bsec.h"
#include "EspMQTTClient.h"
#include <ArduinoJson.h>
#include "BluetoothA2DPSink.h"
#include <SparkFun_Qwiic_OLED.h> //http://librarymanager/All#SparkFun_Qwiic_OLED
#include "SparkFun_Qwiic_Keypad_Arduino_Library.h" //Click here to get the library: http://librarymanager/All#SparkFun_keypad

EspMQTTClient client(
  "YourSSID",
  "YourWifiPassword",
  "192.168.10.51",  // MQTT Broker server ip
  "YourMqttUsername",   // Can be omitted if not needed
  "YourMqttPassword",   // Can be omitted if not needed
  "espmqtt"      // Client name that uniquely identify your device
);

#define I2S_SDO 27 //26 // DDAT
#define I2S_WS  14 //25 // DLRC
#define I2S_SCK 13 //33 // BCLK
//#define I2S_SD 17

#define USE_SPEAKER_OUTPUT
#undef USE_3_5MM_OUTPUT

#define LED_BUILTIN 0

WM8960 codec; // http://librarymanager/All#SparkFun_WM8960
BluetoothA2DPSink a2dp_sink; // https://github.com/pschatzmann/ESP32-A2DP
Bsec iaqSensor; // Bsec Bosch sensors
QwiicMicroOLED myOLED; // OLED
KEYPAD keypad1; //Keypad

// via http://en.radzio.dxp.pl/bitmap_converter/
uint8_t volumeIcon[] = {
  0x38, 0x38, 0x7C, 0x00, 0x10, 0x44, 0x38, 0x82, 0x7C,
};

String output;
String header = "Timestamp [ms], IAQ, IAQ accuracy, Static IAQ, CO2 equivalent, breath VOC equivalent, raw temp[°C], pressure [hPa], raw relative humidity [%], gas [Ohm], Stab Status, run in status, comp temp[°C], comp humidity [%], gas percentage";
unsigned long last_header = 0;
unsigned long last_sensor_run = 0;
int width, height;
uint32_t playtime, play_position = 0; // milliseconds
String artist, title;
int speakerVolume = 0;
int speakerAcGain = 0;
int speakerDcGain = 0;
bool useCelsius = false;
int playback_status = esp_avrc_playback_stat_t::ESP_AVRC_PLAYBACK_STOPPED;

// Helper functions declarations
void checkIaqSensorStatus(void);
void errLeds(void);

void setup()
{
  Serial.begin(115200);
  Serial.println("Smart Air Speaker");
  Wire.begin(15,5);
  delay(500);

  if (codec.begin() == false) //Begin communication over I2C
  {
    Serial.println("Sound codec did not respond. Please check wiring.");
  }
  Serial.println("Sound codec is connected properly.");
  codec_setup();
  // Set up I2S
  i2s_install();
  i2s_setpin();

  a2dp_sink.set_avrc_metadata_attribute_mask(ESP_AVRC_MD_ATTR_TITLE | ESP_AVRC_MD_ATTR_ARTIST | ESP_AVRC_MD_ATTR_PLAYING_TIME );
  a2dp_sink.set_avrc_metadata_callback(avrc_metadata_callback);
  a2dp_sink.set_avrc_rn_playstatus_callback(avrc_rn_playstatus_callback);
  a2dp_sink.set_avrc_rn_play_pos_callback(avrc_rn_play_pos_callback);
  a2dp_sink.start("WillTooth Audio"); // Note, you can give your device any name!

  // OLED setup
  if (myOLED.begin() == false)
  {
      Serial.println("OLED setup failed.");
  }
  width = myOLED.getWidth();
  height = myOLED.getHeight();

  if (keypad1.begin() == false)
  {
    Serial.println("Keypad setup failed.");
  }

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  iaqSensor.begin(BME68X_I2C_ADDR_LOW, Wire);
  output = "\nBSEC library version " + String(iaqSensor.version.major) + "." + String(iaqSensor.version.minor) + "." + String(iaqSensor.version.major_bugfix) + "." + String(iaqSensor.version.minor_bugfix);
  Serial.println(output);
  checkIaqSensorStatus();

  bsec_virtual_sensor_t sensorList[13] = {
    BSEC_OUTPUT_IAQ,
    BSEC_OUTPUT_STATIC_IAQ,
    BSEC_OUTPUT_CO2_EQUIVALENT,
    BSEC_OUTPUT_BREATH_VOC_EQUIVALENT,
    BSEC_OUTPUT_RAW_TEMPERATURE,
    BSEC_OUTPUT_RAW_PRESSURE,
    BSEC_OUTPUT_RAW_HUMIDITY,
    BSEC_OUTPUT_RAW_GAS,
    BSEC_OUTPUT_STABILIZATION_STATUS,
    BSEC_OUTPUT_RUN_IN_STATUS,
    BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE,
    BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY,
    BSEC_OUTPUT_GAS_PERCENTAGE
  };

  iaqSensor.updateSubscription(sensorList, 13, BSEC_SAMPLE_RATE_LP);
  checkIaqSensorStatus();
}

void avrc_metadata_callback(uint8_t data1, const uint8_t *data2) {
  Serial.printf("AVRC metadata rsp: attribute id 0x%x \"%s\"\n", data1, data2);
  if (data1 == 0x1)
    title = String((char*)data2);
  else if (data1 == 0x2)
    artist = String((char*)data2);
  else if (data1 == 0x40) {
    playtime = String((char*)data2).toInt(); // data2 is milliseconds as a text string
    Serial.printf("Playtime \"%s\" stored as %d\n", data2, playtime);
  }
}

void avrc_rn_play_pos_callback(uint32_t play_pos) {
  Serial.printf("Got play_pos %d\n", play_pos);
  play_position = play_pos;
}

void avrc_rn_playstatus_callback(esp_avrc_playback_stat_t playback) {
  Serial.printf("Got playstatus %d\n", playback);
  playback_status = playback;
}

void onConnectionEstablished() {
  client.publish("esp/status", "{\"status\": \"connected\"}");
}

void loop(void)
{
  keypad1.updateFIFO();  // necessary for keypad to pull button from stack to readable register
  char button = keypad1.getButton();
  if (button != 0 && button != -1) {
    if (button == 0x32) { // '2'
      if (speakerVolume < 9) speakerVolume += 1;
      codec.setSpeakerVolumeDB(speakerVolume*2);
      Serial.print("vol: ");
      Serial.println(speakerVolume);
    } else if (button == 0x38) { // '8'
      if (speakerVolume > -10) speakerVolume -= 1;
      codec.setSpeakerVolumeDB(speakerVolume*2);
      Serial.print("vol: ");
      Serial.println(speakerVolume);
    } else if (button == 0x36) { // '6'
      a2dp_sink.next();
      Serial.print("next");
    } else if (button == 0x34) { // '4'
      a2dp_sink.previous();
      Serial.print("prev");
    } else if (button == 0x35) { // '5'
      Serial.println("play/pause");
      if (playback_status == esp_avrc_playback_stat_t::ESP_AVRC_PLAYBACK_PLAYING) a2dp_sink.pause();
      else if (playback_status == esp_avrc_playback_stat_t::ESP_AVRC_PLAYBACK_PAUSED) a2dp_sink.play();
      else if (playback_status == esp_avrc_playback_stat_t::ESP_AVRC_PLAYBACK_STOPPED) a2dp_sink.play();
      else Serial.printf("pause err: %d\n", playback_status);
    } else if (button == 0x23) { // '#'
      useCelsius = !useCelsius;
      Serial.print("celsius: ");
      Serial.println(useCelsius);
    } else {
      Serial.print("Unknown: ");
      Serial.println(button);
    }
    /*if( button == 0x31 ) Serial.println("(1)");
    if( button == 0x32 ) Serial.println("(2)");
    if( button == 0x33 ) Serial.println("(3)");
    if( button == 0x34 ) Serial.println("(4)");
    if( button == 0x35 ) Serial.println("(5)");
    if( button == 0x36 ) Serial.println("(6)");
    if( button == 0x37 ) Serial.println("(7)");
    if( button == 0x38 ) Serial.println("(8)");
    if( button == 0x39 ) Serial.println("(9)");
    if( button == 0x2a ) Serial.println("(*)");
    if( button == 0x30 ) Serial.println("(0)");
    if( button == 0x23 ) Serial.println("(#)");*/
  }

  unsigned long time_trigger = millis();
  if (last_sensor_run == 0 || time_trigger-last_sensor_run > 2000) {
    last_sensor_run = time_trigger;
    if (iaqSensor.run()) { // Do IAQ work and proceed unless there's an issue
      if (last_header == 0 || time_trigger-last_header > 20000) {
        Serial.println(header);
        last_header = time_trigger;
      }
      digitalWrite(LED_BUILTIN, LOW);
      output = String(time_trigger);
      output += ", " + String(iaqSensor.iaq);
      output += ", " + String(iaqSensor.iaqAccuracy);
      output += ", " + String(iaqSensor.staticIaq);
      output += ", " + String(iaqSensor.co2Equivalent);
      output += ", " + String(iaqSensor.breathVocEquivalent);
      output += ", " + String(iaqSensor.rawTemperature);
      output += ", " + String(iaqSensor.pressure);
      output += ", " + String(iaqSensor.rawHumidity);
      output += ", " + String(iaqSensor.gasResistance);
      output += ", " + String(iaqSensor.stabStatus);
      output += ", " + String(iaqSensor.runInStatus);
      output += ", " + String(iaqSensor.temperature);
      output += ", " + String(iaqSensor.humidity);
      output += ", " + String(iaqSensor.gasPercentage);
      Serial.println(output);
      if (iaqSensor.iaqAccuracy > 0){
        client.publish("esp/iaq", String(iaqSensor.iaq));
      } else {
        Serial.println("Not publishing IAQ: low accuracy");
      }
      if (iaqSensor.co2Accuracy > 0){
        client.publish("esp/co2e", String(iaqSensor.co2Equivalent));
      } else {
        Serial.println("Not publishing CO2e: low accuracy");
      }
      if (iaqSensor.breathVocAccuracy > 0) {
        client.publish("esp/bvoce", String(iaqSensor.breathVocEquivalent));
      } else {
        Serial.println("Not publishing bVOCe: low accuracy");
      }
      client.publish("esp/tempc", String(iaqSensor.temperature));
      client.publish("esp/pressure", String(iaqSensor.pressure));
      client.publish("esp/humidity", String(iaqSensor.humidity));

      JsonDocument obj;
      obj["uptime"] = time_trigger;
      obj["stabStatus"] = iaqSensor.stabStatus;
      obj["runInStatus"] = iaqSensor.runInStatus;
      obj["iaqAccuracy"] = iaqSensor.iaqAccuracy;
      obj["co2Accuracy"] = iaqSensor.co2Accuracy;
      obj["breathVocAccuracy"] = iaqSensor.breathVocAccuracy;
      String json;
      serializeJson(obj,json);
      client.publish("esp/status", json);
      // Serial.printf("Took %d ms\n", millis()-last_sensor_run);
      digitalWrite(LED_BUILTIN, HIGH);
    }
  } else {
    checkIaqSensorStatus();
  }

  client.loop();
  drawGraph(iaqSensor.temperature, iaqSensor.humidity, iaqSensor.co2Equivalent, iaqSensor.co2Accuracy);
  delay(10);
}

void drawGraph(int temp, int humid, int co2e, int co2acc)
{
  myOLED.erase();
  int lineHeight = myOLED.getStringHeight("H");

  String out;
  if (useCelsius)
    out += String(temp)+"C ";
  else
    out += String((int)round(temp*1.8+32))+"F ";
  
  out += String(humid)+"% ";
  // out += String(speakerVolume+11); //+String(speakerAcGain)+" "+String(speakerDcGain)+"\n";
  myOLED.setCursor(0, 0);
  myOLED.print(out);

  int offset=0;
  if (speakerVolume < -5) offset=6;
  else if (speakerVolume < 0) offset=4;
  else if (speakerVolume < 5) offset=2;
  // else leave at 0

  myOLED.bitmap(myOLED.getWidth()-9, 0, 9-offset, 8, volumeIcon, 9, 8);

  if(co2acc>0)
    out = "CO2: "+String(co2e);
  else
    out = "CO2: --";

  myOLED.setCursor(0, lineHeight*2);
  myOLED.print(out);

  if(!title.isEmpty() && !artist.isEmpty()) {
    unsigned long time = millis();
    int titlePos, artistPos = 0;
    if (title.length() > 10) {
      titlePos = (time/1000)%(title.length()-9);
    }
    if (artist.length() > 10) {
      artistPos = (time/1000)%(artist.length()-9);
    }
    out = title.substring(titlePos,titlePos+10);

    myOLED.setCursor(0, lineHeight*4-2);
    myOLED.print(out);

    out = artist.substring(artistPos,artistPos+10);
    
    myOLED.setCursor(0, lineHeight*5-2);
    myOLED.print(out);
  }

  if (playtime>0) {
    float playpct = (float)play_position/(float)playtime;
    int height = myOLED.getHeight()-1;
    // Serial.printf("(%d/%d = %.1fpct)\n", play_position, playtime, 100.0*playpct);
    myOLED.line(0, height, round(myOLED.getWidth()*playpct), height);
  }

  myOLED.display();
  delay(10);
}

// Helper function definitions
void checkIaqSensorStatus(void)
{
  if (iaqSensor.bsecStatus != BSEC_OK) {
    if (iaqSensor.bsecStatus < BSEC_OK) {
      output = "BSEC error code : " + String(iaqSensor.bsecStatus);
      Serial.println(output);
      for (;;)
        errLeds(); /* Halt in case of failure */
    } else {
      output = "BSEC warning code : " + String(iaqSensor.bsecStatus);
      Serial.println(output);
    }
  }

  if (iaqSensor.bme68xStatus != BME68X_OK) {
    if (iaqSensor.bme68xStatus < BME68X_OK) {
      output = "BME68X error code : " + String(iaqSensor.bme68xStatus);
      Serial.println(output);
      for (;;)
        errLeds(); /* Halt in case of failure */
    } else {
      output = "BME68X warning code : " + String(iaqSensor.bme68xStatus);
      Serial.println(output);
    }
  }
}

void errLeds(void)
{
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(100);
  digitalWrite(LED_BUILTIN, LOW);
  delay(100);
}

void codec_setup()
{
  // General setup needed
  codec.enableVREF();
  codec.enableVMID();

  // Connect from DAC outputs to output mixer
  codec.enableLD2LO();
  codec.enableRD2RO();

  // Set gainstage between booster mixer and output mixer
#ifdef USE_3_5MM_OUTPUT
  // For this loopback example, we are going to keep these as low as they go
  codec.setLB2LOVOL(WM8960_OUTPUT_MIXER_GAIN_NEG_21DB); 
  codec.setRB2ROVOL(WM8960_OUTPUT_MIXER_GAIN_NEG_21DB);
#endif
#ifdef USE_SPEAKER_OUTPUT
  codec.setLB2LOVOL(WM8960_OUTPUT_MIXER_GAIN_0DB); 
  codec.setRB2ROVOL(WM8960_OUTPUT_MIXER_GAIN_0DB); 
#endif

  // Enable output mixers
  codec.enableLOMIX();
  codec.enableROMIX();

  // CLOCK STUFF, These settings will get you 44.1KHz sample rate, and class-d 
  // freq at 705.6kHz
  codec.enablePLL(); // Needed for class-d amp clock
  codec.setPLLPRESCALE(WM8960_PLLPRESCALE_DIV_2);
  codec.setSMD(WM8960_PLL_MODE_FRACTIONAL);
  codec.setCLKSEL(WM8960_CLKSEL_PLL);
  codec.setSYSCLKDIV(WM8960_SYSCLK_DIV_BY_2);
  codec.setBCLKDIV(4);
  codec.setDCLKDIV(WM8960_DCLKDIV_16);
  codec.setPLLN(7);
  codec.setPLLK(0x86, 0xC2, 0x26); // PLLK=86C226h
  //codec.setADCDIV(0); // Default is 000 (what we need for 44.1KHz)
  //codec.setDACDIV(0); // Default is 000 (what we need for 44.1KHz)
  codec.setWL(WM8960_WL_16BIT);

  codec.enablePeripheralMode();
  //codec.enableMasterMode();
  //codec.setALRCGPIO(); // Note, should not be changed while ADC is enabled.

  // Enable DACs
  codec.enableDacLeft();
  codec.enableDacRight();

  //codec.enableLoopBack(); // Loopback sends ADC data directly into DAC
  codec.disableLoopBack();

  // Default is "soft mute" on, so we must disable mute to make channels active
  codec.disableDacMute(); 

#ifdef USE_3_5MM_OUTPUT
  codec.enableHeadphones();
  codec.enableOUT3MIX(); // Provides VMID as buffer for headphone ground
  codec.setHeadphoneVolumeDB(0.00);
  Serial.println("Headphone volume set to +0dB");
#endif
#ifdef USE_SPEAKER_OUTPUT
  codec.enableSpeakers();
  codec.setSpeakerVolumeDB(0.00);
  Serial.println("Speaker volume set to +0dB");
#endif

  Serial.println("Codec Setup complete. Connect via Bluetooth, play music, and listen on Headphone outputs.");
}

void i2s_install() {
  // Set up I2S Processor configuration
  static i2s_config_t i2s_config = {
    .mode = (i2s_mode_t) (I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = 44100, // Updated automatically by A2DP
    .bits_per_sample = (i2s_bits_per_sample_t)16,
    .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
    .communication_format = (i2s_comm_format_t) (I2S_COMM_FORMAT_STAND_I2S),
    .intr_alloc_flags = 0, // Default interrupt priority
    .dma_buf_count = 8,
    .dma_buf_len = 64,
    .use_apll = true,
    .tx_desc_auto_clear = true // Avoiding noise in case of data unavailability
  };

  a2dp_sink.set_i2s_config(i2s_config);
}

void i2s_setpin() {
  // Set I2S pin configuration
    i2s_pin_config_t my_pin_config = {
    .bck_io_num = I2S_SCK,
    .ws_io_num = I2S_WS,
    .data_out_num = I2S_SDO,
    .data_in_num = I2S_PIN_NO_CHANGE
  };

  a2dp_sink.set_pin_config(my_pin_config);
}