#include "uart_communication.h"


LFR_UART::LFR_UART() : fileDescriptor(-1) {
    this->last =std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    this->openSerialPort();
    this->configureSerialPort();
}
LFR_UART::~LFR_UART() {
    this->closeSerialPort();
}

void LFR_UART::openFile(const char *fileName) {
    this->fileDescriptor = open(fileName, O_RDWR | O_NONBLOCK);
}

int LFR_UART::writeDataToFile(int8_t *buff, uint32_t bufferLength) {
    //std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << std::endl;
    std::cout << "Sending Uart: " << std::bitset<8>(buff[0]) << ", " << std::bitset<8>(buff[1]) << ", " << std::bitset<8>(buff[2]) << ", " << std::bitset<8>(buff[3]) << " csum:  "  << std::bitset<8>(buff[4]) << std::endl;
    return write(this->fileDescriptor, buff, bufferLength);
}

int LFR_UART::readFromFile(int8_t *buff, uint32_t bufferLength) {
    return read(this->fileDescriptor, buff, bufferLength);
}

int LFR_UART::closeFile() {
    return close(this->fileDescriptor);
}

int8_t LFR_UART::doubleToByte(double in){
    /*
    * Map the range of -1.0 to 1.0 as double to the range of a byte. (int8 range)
    */
    double minDouble = -1.0;
    double maxDouble = 1.0;
    double rangeDouble = maxDouble - minDouble;

    double minByte = -128.0;
    double maxByte = 127.0;
    double rangeByte = maxByte - minByte;

    double progress = in - minDouble;
    double progressPercent = progress/rangeDouble;

    double inputInByteRange = minByte + progressPercent*rangeByte;
    return int8_t(inputInByteRange);
}

double LFR_UART::byteToDouble(int8_t in){
    double minDouble = -1.0;
    double maxDouble = 1.0;
    double rangeDouble = maxDouble - minDouble;

    double minByte = -128.0;
    double maxByte = 127.0;
    double rangeByte = maxByte - minByte;

    double progress = double(in) - minByte;
    double progressPercent = progress/rangeByte;

    double inputInDoubleRange = minDouble + progressPercent*rangeDouble;
    return inputInDoubleRange;
}

void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){
    std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
    if (deltaMs < 50 && (std::fabs(wheel1)+std::fabs(wheel2)+std::fabs(wheel3)+std::fabs(wheel4)) > 0.0005)
    {
        std::cout << "Too fast" << std::endl;
        return;
    }
    last = now;

    if(wheel1 > 1.0 || wheel2 > 1.0 || wheel3 > 1.0 || wheel4 > 1.0){
        throw CommunicatorException("Wheel value must not be greater than 1.0");
    }
    if(wheel1 < -1.0 || wheel2 < -1.0 || wheel3 < -1.0 || wheel4 < -1.0){
        throw CommunicatorException("Wheel value must not be smaller than -1.0");
    }
    // Discrepancy between the numbering of the wheels in App/Pi and the µc
    // Our - their
    // 1 - 4
    // 2 - 2
    // 3 - 3
    // 4 - 1
    int8_t wheel1B = this->doubleToByte(wheel4);
    //int8_t wheel1B = this->doubleToByte(wheel1);
    int8_t wheel2B = this->doubleToByte(wheel2);
    int8_t wheel3B = this->doubleToByte(wheel3);
    int8_t wheel4B = this->doubleToByte(wheel1);
    //int8_t wheel4B = this->doubleToByte(wheel4);

    int8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;

    int8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; 
    uint32_t telegram_length = 5;
    this->writeDataToFile(telegram_buffer, telegram_length);
}

bool LFR_UART::readTelegram(double* buffer){
    int8_t tmp_buffer[5] = {0, 0, 0, 0, 0};
    uint32_t telegram_length = 5;
    this->readFromFile(tmp_buffer, telegram_length);

    //std::cout << "Read from file: " << std::bitset<8>(tmp_buffer[0]) << ", " << std::bitset<8>(tmp_buffer[1]) << ", " << std::bitset<8>(tmp_buffer[2]) << ", " << std::bitset<8>(tmp_buffer[3]) << ", "  << std::bitset<8>(tmp_buffer[4]) << std::endl;

    if (std::bitset<8>(tmp_buffer[0]^tmp_buffer[1]^tmp_buffer[2]^tmp_buffer[3]) != std::bitset<8>(tmp_buffer[4])){
        return false;
    }
    else {
        buffer[0] = this->byteToDouble(tmp_buffer[0]);    
        buffer[1] = this->byteToDouble(tmp_buffer[1]);    
        buffer[2] = this->byteToDouble(tmp_buffer[2]);    
        buffer[3] = this->byteToDouble(tmp_buffer[3]);    
    }
    return true;
}

void LFR_UART::openSerialPort() {
    openFile(this->serialPortPath);
    if(this->fileDescriptor < 0) {
        throw CommunicatorException("LFR_UART couldn't open the serial port.");
        exit(EXIT_FAILURE);
    }
}

void LFR_UART::configureSerialPort() {
    if(tcgetattr(this->fileDescriptor, &this->tty)) {
        throw CommunicatorException("LFR_UART couldn't get the port attributes.");
        exit(EXIT_FAILURE);
    }

    cfsetispeed(&this->tty,B115200);
    cfsetospeed(&this->tty,B115200);

    cfmakeraw(&this->tty);

    if(tcsetattr(this->fileDescriptor,TCSANOW,&this->tty)) {
        throw CommunicatorException("LFR_UART couldn't set the port attributes.");
        exit(EXIT_FAILURE);
    }
}

void LFR_UART::closeSerialPort() {
    this->closeFile();
}