#define TRACE
#include "BLEDevice.h"
#include "mydatatypes.h"
#include <WiFi.h>

HardwareSerial commSerial(0);
HardwareSerial bmsSerial(1);
const char* ssid     = "xxx"; // nazev wifi  
const char* password = "xxx";  // heslo wifi 
unsigned long  export_zmena = 0;      // export dat na SQL
unsigned long  export_interval = 30*1000;        //interval exportu 30 sec
unsigned long  cas_export=0; //cas_exportu
float bms_soc=0;
float bms_u=0;
float bms_i=0;
char* host = "test.test.cz";
const int httpPort = 80;
char* apiKey ="SOC_1"; 

void initWiFi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi ..");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print('.');
    delay(1000);
  }
  Serial.println(WiFi.localIP());
}

std::string value[5];

// The remote service we wish to connect to. Needs check/change when other BLE module used.
static BLEUUID serviceUUID("0000ff00-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms original module
static BLEUUID charUUID_tx("0000ff02-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms original module
static BLEUUID charUUID_rx("0000ff01-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms original module


static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
packBasicInfoStruct packBasicInfo; //here shall be the latest data got from BMS
//packEepromStruct packEeprom;     //here shall be the latest data got from BMS
//packCellInfoStruct packCellInfo;   //here shall be the latest data got from BMS

const byte cBasicInfo3 = 3; //type of packet 3= basic info
const byte cCellInfo4 = 4;  //type of packet 4= individual cell info

unsigned long previousMillis = 0;
const long interval = 2000;

bool toggle = false;
bool newPacketReceived = false;

static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLERemoteCharacteristic* pRemoteCharacteristicTX;
static BLERemoteCharacteristic* pRemoteCharacteristicRX;
static BLEAdvertisedDevice* myDevice;

void sendCommand(uint8_t *data, uint32_t dataLen)
{
    if (pRemoteCharacteristicTX)
    {
        pRemoteCharacteristicTX->writeValue(data, dataLen);
    }
    else
    {
        commSerial.println("Remote TX characteristic not found");
    }
}

static void notifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify) //this is called when BLE server sents data via notofication
{
//    hexDump((char*)pData, length);
    bleCollectPacket((char *)pData, length);
}
//**************************
bool isPacketValid(byte *packet) //check if packet is valid
{
    if (packet == nullptr)
    {
//        Serial.println("IsPacketValid FALSE...nullptr...");
        return false;
    }

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

    if (pHeader->start != 0xDD)
    {
//        Serial.println("IsPacketValid FALSE...header-start...");
        return false;
    }

    int offset = 2; // header 0xDD and command type are skipped

    byte checksum = 0;
    for (int i = 0; i < checksumLen; i++)
    {
        checksum += packet[offset + i];
    }

    //printf("checksum: %x\n", checksum);

    checksum = ((checksum ^ 0xFF) + 1) & 0xFF;
    //printf("checksum v2: %x\n", checksum);

    byte rxChecksum = packet[offset + checksumLen + 1];

    if (checksum == rxChecksum)
    {
        //printf("Packet is valid\n");
        return true;
    }
    else
    {
//        Serial.println("IsPacketValid FALSE...checksum...");
        //printf("Packet is not valid\n");
        //printf("Expected value: %x\n", rxChecksum);
        return false;
    }
}

bool bleCollectPacket(char *data, uint32_t dataSize) // reconstruct packet from BLE incomming data, called by notifyCallback function
{
    
    static uint8_t packetstate = 0; //0 - empty, 1 - first half of packet received, 2- second half of packet received
    static uint8_t packetbuff[40] = {0x0};
    static uint32_t previousDataSize = 0;
    bool retVal = false;
    //hexDump(data,dataSize);
//    Serial.println("CollectPacket...");

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

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

    if (packetstate == 2) //got full packet
    {
//        Serial.println("Full packet...");
        uint8_t packet[dataSize + previousDataSize];
        memcpy(packet, packetbuff, dataSize + previousDataSize);

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

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

    if (isValid != true)
    {
//        Serial.println("Invalid packet 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;

    // |Decision based on pac ket type (info3 or info4)
    switch (pHeader->type)
    {
    case cBasicInfo3:
    {
        // Process basic info
//        Serial.print("Process basic info...");

        result = processBasicInfo(&packBasicInfo, data, dataLen);
        newPacketReceived = true;
//        Serial.println(newPacketReceived);
        break;
    }

    case cCellInfo4:
    {
//        Serial.println("Process cell info...");
//        result = processCellInfo(&packCellInfo, data, dataLen);
        newPacketReceived = true;
        break;
    }

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

    return result;
}

//bool processCellInfo(packCellInfoStruct *output, byte *data, unsigned int dataLen)
//{
//
//    return true;
//};

bool processBasicInfo(packBasicInfoStruct *output, byte *data, unsigned int dataLen)
{
//    Serial.print("Process basic info dataLen:");
//    Serial.println(dataLen);
//    hexDump((char*)data, dataLen);
    // Expected data len
    if (dataLen != 0x1D)
    {
//        Serial.println("Process basic info ERROR.");
        return false;
    }

    output->Volts = ((uint32_t)two_ints_into16(data[0], data[1])) * 10; // Resolution 10 mV -> convert to milivolts   eg 4895 > 48950mV
    Serial.print("output->Volts:");
    Serial.println(output->Volts);
    bms_u=(float)output->Volts/1000;
    output->Amps = ((int32_t)two_ints_into16(data[2], data[3])) * 10;   // Resolution 10 mA -> convert to miliamps
    Serial.print("output->Amps:");
    Serial.println(output->Amps);
    bms_i=(float)output->Amps/1000;
    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]);
    Serial.print("output->CapacityRemainPercent:");
    Serial.println(output->CapacityRemainPercent);
    bms_soc=(float)output->CapacityRemainPercent;
//    Serial.print("output->CapacityRemainAh:");
//    Serial.println(output->CapacityRemainAh);

//    output->CapacityRemainWh = (output->CapacityRemainAh * c_cellNominalVoltage) / 1000000 * packCellInfo.NumOfCells;

    output->Temp1 = (((uint16_t)two_ints_into16(data[23], data[24])) - 2731);
//    Serial.print("output->Temp1:");
//    Serial.println(output->Temp1);
    
    output->Temp2 = (((uint16_t)two_ints_into16(data[25], data[26])) - 2731);
//    Serial.print("output->Temp2:");
//    Serial.println(output->Temp2);
    output->BalanceCodeLow = (two_ints_into16(data[12], data[13]));
    output->BalanceCodeHigh = (two_ints_into16(data[14], data[15]));
    output->MosfetStatus = ((byte)data[20]);

    return true;
};

//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;
}

//void constructBigString() //debug all data to uart
//{
//    
//    stringBuffer[0] = '\0'; //clear old data
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Total voltage: %f\n", (float)packBasicInfo.Volts / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Amps: %f\n", (float)packBasicInfo.Amps / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "CapacityRemainAh: %f\n", (float)packBasicInfo.CapacityRemainAh / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "CapacityRemainPercent: %d\n", packBasicInfo.CapacityRemainPercent);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Temp1: %f\n", (float)packBasicInfo.Temp1 / 10);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Temp2: %f\n", (float)packBasicInfo.Temp2 / 10);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Balance Code Low: 0x%x\n", packBasicInfo.BalanceCodeLow);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Balance Code High: 0x%x\n", packBasicInfo.BalanceCodeHigh);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Mosfet Status: 0x%x\n", packBasicInfo.MosfetStatus);
//
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Number of cells: %u\n", packCellInfo.NumOfCells);
//    for (byte i = 1; i <= packCellInfo.NumOfCells; i++)
//    {
//        snprintf(stringBuffer, STRINGBUFFERSIZE, "Cell no. %u", i);
//        snprintf(stringBuffer, STRINGBUFFERSIZE, "   %f\n", (float)packCellInfo.CellVolt[i - 1] / 1000);
//    }
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Max cell volt: %f\n", (float)packCellInfo.CellMax / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Min cell volt: %f\n", (float)packCellInfo.CellMin / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Difference cell volt: %f\n", (float)packCellInfo.CellDiff / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Average cell volt: %f\n", (float)packCellInfo.CellAvg / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "Median cell volt: %f\n", (float)packCellInfo.CellMedian / 1000);
//    snprintf(stringBuffer, STRINGBUFFERSIZE, "\n");
//}

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

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

//********************************************************
//static void notifyCallback(
//  BLERemoteCharacteristic* pBLERemoteCharacteristic,
//  uint8_t* pData,
//  size_t length,
//  bool isNotify) {
//    Serial.print("Notify callback for characteristic ");
//    Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
//    Serial.print(" of data length ");
//    Serial.println(length);
//    Serial.print("data: ");
//    Serial.println((char*)pData);
//}

class MyClientCallback : public BLEClientCallbacks {
  void onConnect(BLEClient* pclient) {
  }

  void onDisconnect(BLEClient* pclient) {
    connected = false;
    Serial.println("onDisconnect");
  }
};

bool connectToServer() {
    Serial.print("Forming a connection to ");
    Serial.println(myDevice->getAddress().toString().c_str());
    
    BLEClient*  pClient  = BLEDevice::createClient();
    Serial.println(" - Created client");

    pClient->setClientCallbacks(new MyClientCallback());

    // Connect to the remove BLE Server.
    pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
    Serial.println(" - Connected to server");

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");


    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristicRX = pRemoteService->getCharacteristic(charUUID_rx);
    if (pRemoteCharacteristicRX == nullptr) {
      Serial.print("Failed to find our characteristicRX UUID: ");
      Serial.println(charUUID_rx.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our characteristicRX");
    // Read the value of the characteristic.
    if (pRemoteCharacteristicRX->canRead())
    {
        std::string value = pRemoteCharacteristicRX->readValue();
        Serial.print("The characteristic value was: ");
        Serial.println(value.c_str());
    }

    if (pRemoteCharacteristicRX->canNotify())
        pRemoteCharacteristicRX->registerForNotify(notifyCallback);


    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristicTX = pRemoteService->getCharacteristic(charUUID_tx);
    if (pRemoteCharacteristicTX == nullptr) {
      Serial.print("Failed to find our characteristic TX UUID: ");
      Serial.println(charUUID_tx.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our characteristicTX");

    if (pRemoteCharacteristicTX->canRead())
    {
        std::string value = pRemoteCharacteristicTX->readValue();
        Serial.print("The characteristic value was: ");
        Serial.println(value.c_str());
    }
    
      
      
    connected = true;
    return true;
        
 
}
/**
 * Scan for BLE servers and find the first one that advertises the service we are looking for.
 */
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
 /**
   * Called for each advertising BLE server.
   */
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect = true;
      doScan = true;

    } // Found our server
  } // onResult
}; // MyAdvertisedDeviceCallbacks


void setup() {
  Serial.begin(115200);
  initWiFi();
  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);
}
// This is the Arduino main loop function.
void loop() {
  unsigned long currentMillis = millis();
  // If the flag "doConnect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
  // connected we set the connected flag to be true.
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
    } else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }

  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {

//      Serial.println("Connected...");
        unsigned long currentMillis = millis();
        if ((currentMillis - previousMillis >= interval || newPacketReceived)) //every time period or when packet is received
        {
            previousMillis = currentMillis;
//            Serial.println("Ctu data...");
//ask       showInfoLcd();

            if (toggle) //alternate info3 and info4
            {
//                bmsGetInfo3();
//                header status command length data checksum footer
//                DD     A5      03     00    FF     FD      77
                  uint8_t data[7] = {0xdd, 0xa5, 0x3, 0x0, 0xff, 0xfd, 0x77};
//                bmsSerial.write(data, 7);
                  sendCommand(data, sizeof(data));
//                  Serial.println("INFO3 sent...");
//                commSerial.println("Request info3 sent");
                  newPacketReceived = false;
            }
            else
            {
//                bmsGetInfo4();
//                DD  A5 04 00  FF  FC  77
//ask                  uint8_t data[7] = {0xdd, 0xa5, 0x4, 0x0, 0xff, 0xfc, 0x77};
//                bmsSerial.write(data, 7);
//ask                  sendCommand(data, sizeof(data));
//                commSerial.println("Request info4 sent");
//                  Serial.println("INFO4 sent...");
                  //showCellInfo();
                  newPacketReceived = false;
            }
            toggle = !toggle;
        }
//  if (newPacketReceived == true)
//  {
//  Serial.println("Print BMS info..");
//    printBasicInfo();
    //printCellInfo();
//  }  else {    //printBasicInfo();
//}     
 
  }else if(doScan){
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }
  delay(1000); // Delay a second between loops.
//odeslani dat na SQL
  if(currentMillis - export_zmena > export_interval) {
      cas_export=currentMillis-export_zmena;
      export_zmena = currentMillis;

//String serverName = "http://skopec.sks.cz/d34/arduino/bms.php";
//Check WiFi connection status
    if(WiFi.status()== WL_CONNECTED){
      WiFiClient client;
      if(client.connect(host, httpPort)){
        delay(100);
        String url = "/arduino/bms.php?api_key=";
        url += apiKey;
        url += "&bms_soc=";
        url += (bms_soc);
        url += "&bms_u=";
        url += (bms_u);
        url += "&bms_i=";
        url += (bms_i);
        url += "&cas_export=";
        url += (cas_export);
        Serial.println(url);
        client.print(String("GET ") + url + " HTTP/1.1\r\n" +
               "Host: " + host + "\r\n" + 
               "Connection: close\r\n\r\n");
        delay(10);
        while(client.available()){
          String line = client.readStringUntil('\r');
        }  
       }    
      } 
    }



} // End of loop