#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

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

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_report = 0;
int width, height;
uint8_t * playtime;
String artist, title;

// 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.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();

  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 = data2;
}

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

void loop(void)
{
  unsigned long time_trigger = millis();
  if (iaqSensor.run()) { // Do IAQ work and proceed unless there's an issue
    if (last_report == 0 || time_trigger-last_report > 60000) {
      if (last_header == 0 || time_trigger-last_header > 600000) {
        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);

      digitalWrite(LED_BUILTIN, HIGH);
      last_report = time_trigger;
      drawGraph(iaqSensor.temperature, iaqSensor.humidity, iaqSensor.co2Equivalent, iaqSensor.co2Accuracy);
    }
  } else {
    checkIaqSensorStatus();
  }

  client.loop();
  sleep(1);
}

void drawGraph(int temp, int humid, int co2e, int co2acc)
{
    myOLED.erase();

    String out = "Temp: "+String(temp)+"'\n"+
      "Hum: "+String(humid)+"%\n";

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

    if(!title.isEmpty() && !artist.isEmpty())
      out += "\n"+title+" - "+artist;

    myOLED.setCursor(0, 0);
    myOLED.print(out);

    //myOLED.line(xS, yS, xE, yE);
    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);
}