#include <windows.h>
#include <iostream>
#include <string>
#include <regex>
#include <mutex>
#include <thread>
#include "serialcomm.hpp"

HANDLE initSerialPort (const char* portName, DWORD baudRate) {

    // open serial port
    HANDLE hSerial = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);

    if (hSerial == INVALID_HANDLE_VALUE) {
        std::cerr << "Error opening serial port: " << GetLastError() << std::endl;
        return INVALID_HANDLE_VALUE;
    }

     // configure the serial port
     DCB dcbSerialParams = {0};
     dcbSerialParams.DCBlength = sizeof(dcbSerialParams);

     if (!GetCommState(hSerial, &dcbSerialParams)) {
        std::cerr << "Error getting comm state: " << GetLastError() << std::endl;
        CloseHandle(hSerial);
        return INVALID_HANDLE_VALUE;
     }

     dcbSerialParams.BaudRate = baudRate;
     dcbSerialParams.ByteSize = 8;
     dcbSerialParams.StopBits = ONESTOPBIT;
     dcbSerialParams.Parity = NOPARITY;

     if (!SetCommState(hSerial, &dcbSerialParams)) {
        std::cerr << "Error getting comm state: " << GetLastError() << std::endl;
        CloseHandle(hSerial);
        return INVALID_HANDLE_VALUE;
     }

     // set timeouts
        COMMTIMEOUTS timeouts = {0};
        timeouts.ReadIntervalTimeout = 50;
        timeouts.ReadTotalTimeoutConstant = 0;
        timeouts.ReadTotalTimeoutMultiplier = 0;
     if (!SetCommTimeouts(hSerial, &timeouts)) {
        std::cerr << "Error setting timeout: " << GetLastError() << std::endl;
        CloseHandle(hSerial);
        return INVALID_HANDLE_VALUE;
     }

     return hSerial;

}

bool readSerialData (HANDLE hSerial, char* buffer, DWORD bufferSize, DWORD& bytesRead) {
    if (!ReadFile(hSerial, buffer, bufferSize - 1, &bytesRead, NULL)){
        std::cerr << "Error reading from serial port: " << GetLastError() << std::endl;
        return false;
    }

    buffer[bytesRead] = '\0';
    return true;
}

bool parseQuaternion(const std::string& data, Quaternion& quat) {
   // regex pattern to match the floats in the uart data stream from stm32
   std::regex pattern("qw: ([+-]?\\d*\\.?\\d+) qx: ([+-]?\\d*\\.?\\d+) qy: ([+-]?\\d*\\.?\\d+) qz: ([+-]?\\d*\\.?\\d+)");
   std::smatch matches;

   // match every occurance to the desired quaternion value
   if (std::regex_search(data, matches, pattern) && matches.size() == 5) {
      // access the array like object std::smatch with [0] being the entire matched string
      quat.w = std::stof(matches[1]);
      quat.x = std::stof(matches[2]);
      quat.y = std::stof(matches[3]);
      quat.z = std::stof(matches[4]);
      return true;
   } else {return false;}
}