#include "sick_side.hpp"

Sick_side::Sick_side(){
    serial_descriptor = serialOpen("/dev/ttyS0", 9600);
    if(serial_descriptor == -1){
        cout << "Cannot open serial line for communication with side distance sensor" << endl;
    }
}

uint8_t Sick_side::ID(){
    Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
    //while(Data_available() < 12);
    // Readout data which are in buffer from transmittion
    //Receive();
    // Read real received data from sensor
    if(auto responce = Receive()){
        return (*responce)[2];
    } else {
        return 0x00;
    }
}

void Sick_side::Laser(bool state){
    if(state){
        // ON
        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
    } else {
        // OFF
        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
    }

    //Read responce to clear buffer
    Receive();
}

optional<double> Sick_side::Distance(){
    auto distance_data = Distance_data();
    if(distance_data == 0x7fff){
        // Out of measuring range
        return {};
    } else {
        // Convert to 2'complement and then to mm
        return static_cast<int16_t>(distance_data)/100.0;
    }
}

uint16_t Sick_side::Distance_data(){
    Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
    if(auto responce = Receive()){
        return (((*responce)[1]<<8) | (*responce)[2]);
    } else {
        return 0x7fff;
    }
}

void Sick_side::Send_data(array<uint8_t, 3> data){
    array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
    packet[5] = Calculate_BCC(packet);
    Transmit(packet);
}

void Sick_side::Transmit(array<uint8_t, 6> data){
    for(int i = 0; i < 6; i++){
        serialPutchar(serial_descriptor, data[i]);
    }
}

uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
    uint8_t BCC = 0;
    // BCC is calculated only from bytes on positions 1,2,3
    for(int byte_index = 1; byte_index < 4; byte_index++){
        BCC ^= data[byte_index];
    }
    return BCC;
}

optional<array<uint8_t, 3>> Sick_side::Receive(){
    while(Data_available() < 12);
    // Readout data which are in buffer from transmittion
    for(int i = 0; i < 6; i++){
        serialGetchar(serial_descriptor);
    }
    // Receive true data
    array<uint8_t, 6> received_data;
    for(int i = 0; i < 6; i++){
        received_data[i] = serialGetchar(serial_descriptor);
    }
    // Check for communication errors
    if(static_cast<signs>(received_data[1]) == signs::NAK){
        cout << error_codes[received_data[2]] << endl;
        return {};
    }
    if(received_data[5] != Calculate_BCC(received_data)){
        cout << "Received BCC is invalid" << endl;
        return {};
    }

    return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
}

unsigned int Sick_side::Data_available(){
    return serialDataAvail(serial_descriptor);
}
