// ==== CONFIGURATION ====
// BMS
#define BMS_MAX_CELLS 15 // defines size of data types
#define BMS_POLLING_INTERVAL 10*60*1000 // data output interval (shorter = connect more often = more battery consumption from BMS) in ms

// BLE
#define BLE_SCAN_DURATION 1 // duration of scan in seconds
#define BLE_REQUEST_DELAY 500 // package request delay after connecting - make this large enough to have the connection established in ms
#define BLE_TIMEOUT 600*1000 // timeout of scan + gathering packets (too short will fail collecting all packets) in ms

#define BLE_CALLBACK_DEBUG true // send debug messages via MQTT & serial in callbacks (handy for finding your BMS address, name, RSSI, etc)

// ==== MAIN CODE ====
#include "datatypes.h" // for brevity the BMS stuff is in this file
#include <BLEDevice.h> // for BLE

#include <driver/adc.h> // to read ESP battery voltage
#include <rom/rtc.h> // to get reset reason


// Init BMS
static BLEUUID serviceUUID("0000ff00-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms service
static BLEUUID charUUID_rx("0000ff01-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms rx id
static BLEUUID charUUID_tx("0000ff02-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms tx id

const byte cBasicInfo = 3; //datablock 3=basic info
const byte cCellInfo = 4;  //datablock 4=individual cell info
packBasicInfoStruct packBasicInfo;
packCellInfoStruct packCellInfo;
unsigned long bms_last_update_time=0;
bool bms_status;
#define BLE_PACKETSRECEIVED_BEFORE_STANDBY 0b11 // packets to gather before disconnecting

// Other stuff
float battery_voltage=0; // internal battery voltage
String debug_log_string="";
hw_timer_t * wd_timer = NULL;

void debug(String s){
    Serial.println(s);
}

void setup(){
  Serial.begin(115200);
}


// === Main stuff ====
void loop(){
    bleGatherPackets();
}

BLEScan* pBLEScan = nullptr;
BLEClient* pClient = nullptr;
BLEAdvertisedDevice* pRemoteDevice = nullptr;
BLERemoteService* pRemoteService = nullptr;
BLERemoteCharacteristic* pRemoteCharacteristic_rx = nullptr;
BLERemoteCharacteristic* pRemoteCharacteristic_tx = nullptr;

boolean doScan = false; // becomes true when BLE is initialized and scanning is allowed
boolean doConnect = false; // becomes true when correct ID is found during scanning

boolean ble_client_connected = false; // true when fully connected

unsigned int ble_packets_requested = 0b00; // keeps track of requested packets
unsigned int ble_packets_received = 0b00; // keeps track of received packets

void MyEndOfScanCallback(BLEScanResults pBLEScanResult){
    bms_status=false; // BMS not found

    if(BLE_CALLBACK_DEBUG){
      debug("BLE: scan finished");
      Serial.println("Scan finished.");
    }
}

class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks{
  // called for each advertising BLE server

  void onResult(BLEAdvertisedDevice advertisedDevice){
    // found a device


    if(BLE_CALLBACK_DEBUG){
      debug(
          String("BLE: found ") +
          String(advertisedDevice.getName().c_str()) +
          String(" with address ") +
          String(advertisedDevice.getAddress().toString().c_str()) +
          String(" and RSSI ") +
          String(advertisedDevice.getRSSI())
        );

      Serial.print("BLE: found ");
      Serial.println(advertisedDevice.toString().c_str());
    }

    // Check if device is advertising the specific service UUID
    if (!advertisedDevice.isAdvertisingService(serviceUUID)) {
      debug("Device does not advertise the specified service UUID.");
      return;
    }

      if(BLE_CALLBACK_DEBUG){
        debug("BLE: target device found");
      }

      pBLEScan->stop();

        // delete old remote device, create new one
        if(pRemoteDevice != nullptr){
          delete pRemoteDevice;
        }
        pRemoteDevice = new BLEAdvertisedDevice(advertisedDevice);

        doConnect = true;
  }
};

class MyClientCallback : public BLEClientCallbacks{
  // called on connect/disconnect
  void onConnect(BLEClient* pclient){

    if(BLE_CALLBACK_DEBUG){
      debug(String("BLE: connecting to ") + String(pclient->getPeerAddress().toString().c_str()));
    }
  }

  void onDisconnect(BLEClient* pclient){
    ble_client_connected = false;
    doConnect = false;

    if(BLE_CALLBACK_DEBUG){
      debug(String("BLE: disconnected from ") + String(pclient->getPeerAddress().toString().c_str()));
    }

  }
};

static void MyNotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify){
  //this is called when BLE server sents data via notification
  //hexDump((char*)pData, length);
  if(!bleCollectPacket((char *)pData, length)){
    debug("ERROR: packet could not be collected.");
  }
}

void handleBLE(){
  static unsigned long prev_millis_standby = 0;

  prev_millis_standby = millis();

  while(true){ // loop until we hit a timeout or gathered all packets

    if((ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY) || (millis()>prev_millis_standby+BLE_TIMEOUT)){
      if(ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY){
        debug("BLE: all packets received");
        bms_status=true; // BMS was connected, data up-to-date
        printBasicInfo();
        printCellInfo();
      }else{
        debug("BLE: connection timeout");
        bms_status=false; // BMS not (fully) connected
      }

      break; // we're done with BLE, exit while loop
    }
    else if (doConnect){

      // found the desired BLE server, now connect to it
      if (connectToServer()){
        ble_client_connected = true;
        ble_packets_received=0;
        ble_packets_requested=0;

      }else{
        ble_client_connected = false;
        debug("BLE: failed to connect");
      }

      doConnect = false;
    }

    if (ble_client_connected){
        debug("BLE: requesting packet 0b01");
        delay(5000);
        bmsRequestBasicInfo();
        debug("BLE: requesting packet 0b10");
        delay(5000);
        bmsRequestCellInfo();
    }else if ((!doConnect)&&(doScan)){
      // we are not connected, so we can scan for devices
      debug("BLE: not connected, starting scan");
      Serial.print("BLE is not connected, starting scan");

      // Disconnect client
      if((pClient != nullptr)&&(pClient->isConnected())){
        pClient->disconnect();
      }

      // stop scan (if running) and start a new one
      pBLEScan->setActiveScan(true);
      pBLEScan->setInterval(1 << 8); // 160 ms
      pBLEScan->setWindow(1 << 7); // 80 ms
      pBLEScan->start(BLE_SCAN_DURATION, MyEndOfScanCallback, false); // non-blocking, use a callback

      doScan=false;

      debug("BLE: scan started");
    }
  }
}

void bleGatherPackets(){
  bleStart();
  handleBLE();
  blePause();
  BLEDevice::deinit(false);
}

void bleStart(){
  Serial.print("Starting BLE... ");

  BLEDevice::init("");
  //esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT); // release some unused memory

  // Retrieve a BLE client
  pClient = BLEDevice::createClient();
  pClient->setClientCallbacks(new MyClientCallback());

  // Retrieve a BLE scanner
  pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());

  bleContinue();
  Serial.println("done");
}

void blePause(){
  // stop scanning and disconnect from all devices
  doScan=false;

  // Disconnect client
  if((pClient != nullptr)&&(pClient->isConnected())){
    pClient->disconnect();
  }

  delay(50);

  pBLEScan->stop();

  ble_client_connected=false;
  doConnect=false;
  ble_packets_received=0;
  ble_packets_requested=0;

}

void bleContinue(){
  // Prepare for scanning
  ble_client_connected=false;
  doConnect=false;
  ble_packets_received=0;

  doScan=true; // start scanning for new devices
}

bool connectToServer(){
  if(pRemoteDevice==nullptr){
    Serial.println("Invalid remote device, can't connect");
    return false;
  }

  // Disconnect client
  if((pClient != nullptr)&&(pClient->isConnected())){
    pClient->disconnect();
  }

  Serial.print("Forming a connection to ");
  Serial.println(pRemoteDevice->getAddress().toString().c_str());

  delay(100);

  // Connect to the remote BLE Server.
  pClient->connect(pRemoteDevice);
  if(!(pClient->isConnected())){
    debug(String("BLE: failed to connect"));
    Serial.println("Failed to connect to server");
    pClient->disconnect();
    return false;
  }

  Serial.println(" - Connected to server");


  delay(BLE_REQUEST_DELAY); // wait, otherwise writeValue doesn't work for some reason
                            // to do: fix this ugly hack

  debug(String("BLE: connected"));

  return true;
}

bool sendCommand(uint8_t *data, uint32_t dataLen){
  if((pClient!=nullptr)&&(pClient->isConnected())){
    pRemoteCharacteristic_tx->writeValue(data, dataLen, false);
    return true;
  }else{
    return false;
  }
}

bool isPacketValid(byte *packet) //check if packet is valid
{
    if (packet == nullptr){
        return false;
    }

    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
    int checksumPos = pHeader->dataLen + 2; // status + data len + data

    int offset = 2; // header 0xDD and command type are not in data length

    if (packet[0] != 0xDD){
        // start bit missing
        return false;
    }

    if (packet[offset + checksumPos + 2] != 0x77){
        // stop bit missing
        return false;
    }

    byte checksum = 0;
    for (int i = 0; i < checksumPos; i++){
        checksum += packet[offset + i];
    }
    checksum = ((checksum ^ 0xFF) + 1) & 0xFF;

    if (checksum != packet[offset + checksumPos + 1]){
        return false;
    }

    return true;
}

bool processBasicInfo(packBasicInfoStruct *output, byte *data, unsigned int dataLen)
{
    // Expected data len
    if (dataLen != 0x1B)
    {
        //Serial.printf("bad data len %d!\r\n",dataLen);
        //return false;
    }

    output->Volts = ((uint32_t)two_ints_into16(data[0], data[1])) * 10; // Resolution 10 mV -> convert to milivolts   eg 4895 > 48950mV
    output->Amps = ((int32_t)two_ints_into16(data[2], data[3])) * 10;   // Resolution 10 mA -> convert to miliamps

    output->Watts = output->Volts * output->Amps / 1000000; // W

    output->CapacityRemainAh = ((uint16_t)two_ints_into16(data[4], data[5])) * 10;
    output->CapacityRemainPercent = ((uint8_t)data[19]);

    output->Temp1 = (((uint16_t)two_ints_into16(data[23], data[24])) - 2731);
    output->Temp2 = (((uint16_t)two_ints_into16(data[25], data[26])) - 2731);

    output->BalanceCodeLow = (two_ints_into16(data[12], data[13]));
    output->BalanceCodeHigh = (two_ints_into16(data[14], data[15]));
    output->MosfetStatus = ((byte)data[20]);

    printBasicInfo();

    return true;
}

bool processCellInfo(packCellInfoStruct *output, byte *data, unsigned int dataLen)
{
    uint16_t _cellSum;
    uint16_t _cellMin = 5000;
    uint16_t _cellMax = 0;
    uint16_t _cellAvg;
    uint16_t _cellDiff;

    output->NumOfCells = dataLen / 2; // data contains 2 bytes per cell

    //go trough individual cells
    for (byte i = 0; i < dataLen / 2; i++)
    {
        output->CellVolt[i] = ((uint16_t)two_ints_into16(data[i * 2], data[i * 2 + 1])); // Resolution 1 mV
        _cellSum += output->CellVolt[i];
        if (output->CellVolt[i] > _cellMax)
        {
            _cellMax = output->CellVolt[i];
        }
        if (output->CellVolt[i] < _cellMin)
        {
            _cellMin = output->CellVolt[i];
        }
    }

    output->CellMin = _cellMin;
    output->CellMax = _cellMax;
    output->CellDiff = _cellMax - _cellMin;
    output->CellAvg = _cellSum / output->NumOfCells;

    printCellInfo();

    return true;
}

bool bmsProcessPacket(byte *packet)
{
    bool isValid = isPacketValid(packet);

    if (isValid != true)
    {
        Serial.println("Invalid packer received");
        return false;
    }

    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
    byte *data = packet + sizeof(bmsPacketHeaderStruct); // TODO Fix this ugly hack
    unsigned int dataLen = pHeader->dataLen;

    bool result = false;

    // find packet type (basic info or cell info)
    switch (pHeader->type)
    {
    case cBasicInfo:
    {
        // Process basic info
        result = processBasicInfo(&packBasicInfo, data, dataLen);
        if(result==true){
          ble_packets_received |= 0b01;
          bms_last_update_time=millis();
        }

        break;
    }

    case cCellInfo:
    {
        // Process cell info
        result = processCellInfo(&packCellInfo, data, dataLen);
        if(result==true){
          ble_packets_received |= 0b10;
          bms_last_update_time=millis();
        }
        break;
    }

    default:
        result = false;
        Serial.printf("Unsupported packet type detected. Type: %d", pHeader->type);
    }

    return result;
}

bool bleCollectPacket(char *data, uint32_t dataSize) // reconstruct packet, called by notifyCallback function
{
    static uint8_t packetstate = 0; //0 - empty, 1 - first half of packet received, 2- second half of packet received

    // packet sizes:
    //  (packet ID 03) = 4 (header) + 23 + 2*N_NTCs + 2 (checksum) + 1 (stop)
    //  (packet ID 04) = 4 (header) + 2*NUM_CELLS   + 2 (checksum) + 1 (stop)
    static uint8_t packetbuff[4 + 2*25 + 2 + 1] = {0x0}; // buffer size suitable for up to 25 cells

    static uint32_t totalDataSize = 0;
    bool retVal = false;
    hexDump(data,dataSize);

    if(totalDataSize + dataSize > sizeof(packetbuff)){
      Serial.printf("ERROR: datasize is overlength.");

      debug(
        String("ERROR: datasize is overlength. ") +
        String("allocated=") +
        String(sizeof(packetbuff)) +
        String(", size=") +
        String(totalDataSize + dataSize)
        );

      totalDataSize = 0;
      packetstate = 0;

      retVal = false;
    }
    else if (data[0] == 0xdd && packetstate == 0) // probably got 1st half of packet
    {
        Serial.println("PKT1");
        packetstate = 1;
        for (uint8_t i = 0; i < dataSize; i++)
        {
            packetbuff[i] = data[i];
        }
        totalDataSize = dataSize;
        retVal = true;

        if (data[dataSize - 1] == 0x77) {
            //its full packets
            packetstate = 2;
        }
    }
    else if (data[dataSize - 1] == 0x77 && packetstate == 1) //probably got 2nd half of the packet
    {
        Serial.println("PKT2");
        packetstate = 2;
        for (uint8_t i = 0; i < dataSize; i++)
        {
            packetbuff[i + totalDataSize] = data[i];
        }
        totalDataSize += dataSize;
        retVal = true;
    }

    if (packetstate == 2) //got full packet
    {
        Serial.println("PKT3");
        uint8_t packet[totalDataSize];
        memcpy(packet, packetbuff, totalDataSize);

        bmsProcessPacket(packet); //pass pointer to retrieved packet to processing function
        packetstate = 0;
        totalDataSize = 0;
        retVal = true;
    }
    return retVal;
}

bool bmsRequestBasicInfo(){
    // header status command length data checksum footer
    //   DD     A5      03     00    FF     FD      77
    uint8_t data[7] = {0xdd, 0xa5, cBasicInfo, 0x0, 0xff, 0xfd, 0x77};
    return sendCommand(data, sizeof(data));
}

bool bmsRequestCellInfo(){
    // header status command length data checksum footer
    //  DD      A5      04     00    FF     FC      77
    uint8_t data[7] = {0xdd, 0xa5, cCellInfo, 0x0, 0xff, 0xfc, 0x77};
    return sendCommand(data, sizeof(data));
}

void printBasicInfo() //debug all data to uart
{
    Serial.printf("Total voltage: %f\r\n", (float)packBasicInfo.Volts / 1000);
    Serial.printf("Amps: %f\r\n", (float)packBasicInfo.Amps / 1000);
    Serial.printf("CapacityRemainAh: %f\r\n", (float)packBasicInfo.CapacityRemainAh / 1000);
    Serial.printf("CapacityRemainPercent: %d\r\n", packBasicInfo.CapacityRemainPercent);
    Serial.printf("Temp1: %f\r\n", (float)packBasicInfo.Temp1 / 10);
    Serial.printf("Temp2: %f\r\n", (float)packBasicInfo.Temp2 / 10);
    Serial.printf("Balance Code Low: 0x%x\r\n", packBasicInfo.BalanceCodeLow);
    Serial.printf("Balance Code High: 0x%x\r\n", packBasicInfo.BalanceCodeHigh);
    Serial.printf("Mosfet Status: 0x%x\r\n", packBasicInfo.MosfetStatus);
}

void printCellInfo() //debug all data to uart
{
    Serial.printf("Number of cells: %u\r\n", packCellInfo.NumOfCells);
    for (byte i = 1; i <= packCellInfo.NumOfCells; i++)
    {
        Serial.printf("Cell no. %u", i);
        Serial.printf("   %f\r\n", (float)packCellInfo.CellVolt[i - 1] / 1000);
    }
    Serial.printf("Max cell volt: %f\r\n", (float)packCellInfo.CellMax / 1000);
    Serial.printf("Min cell volt: %f\r\n", (float)packCellInfo.CellMin / 1000);
    Serial.printf("Difference cell volt: %f\r\n", (float)packCellInfo.CellDiff / 1000);
    Serial.printf("Average cell volt: %f\r\n", (float)packCellInfo.CellAvg / 1000);
    Serial.println();
}

void hexDump(const char *data, uint32_t dataSize) //debug function
{
    Serial.println("HEX data:");

    for (int i = 0; i < dataSize; i++)
    {
        Serial.printf("0x%x, ", data[i]);
    }
    Serial.println("");
}

int16_t two_ints_into16(int highbyte, int lowbyte) // turns two bytes into a single long integer
{
    int16_t result = (highbyte);
    result <<= 8;                //Left shift 8 bits,
    result = (result | lowbyte); //OR operation, merge the two
    return result;
}