#include "uart_communication.h"


LFR_UART::LFR_UART() : fileDescriptor(-1) {
    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(uint8_t *buff, uint32_t bufferLength) {
    std::cout << "Sending uart telegram" << std::endl;
    return write(this->fileDescriptor, buff, bufferLength);
}

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

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

uint8_t LFR_UART::doubleToByte(double in){
    /*
    * Map the range of -1.0 to 1.0 as double to the range of a byte.
    * 
    * -1 -> 0
    * 0.0 -> 127
    * 1.0 -> 254
    * Not using the full range upto 254 to hit the 0.0
    */
    double minDouble = -1.0;
    double maxDouble = 1.0;
    double rangeDouble = maxDouble - minDouble;

    double minByte = 0.0;
    double maxByte = 255.0;
    double rangeByte = maxByte - minByte;

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

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

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

    double minByte = 0.0;
    double maxByte = 255.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){
    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");
    }

    uint8_t wheel1B = this->doubleToByte(wheel1);
    uint8_t wheel2B = this->doubleToByte(wheel2);
    uint8_t wheel3B = this->doubleToByte(wheel3);
    uint8_t wheel4B = this->doubleToByte(wheel4);

    uint8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;

    uint8_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){
    uint8_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();
}