#pragma once

#include <iostream>
#include <array>
#include <optional>
#include <map>
#include <string>

#include <wiringSerial.h>

using namespace std;

/**
Type: OD1-B100C50A15
**/
class Sick_side{

    int serial_descriptor = 0;

    enum class signs: uint8_t{
        STX       = 0x02,
        ETX       = 0x03,
        ACK       = 0x06,
        NAK       = 0x15,
    };

    map<uint8_t, string> error_codes{
        {0x02, "Address is invalid"},
        {0x04, "BCC values is invalid"},
        {0x05, "Invalid command"},
        {0x06, "Invalid setting value (out of spec)"},
        {0x07, "Invalid setting value (out of range)"}
    };

    enum class commands: uint8_t{
        Command     = 0x43, // Also used for reading out measured values
        Write       = 0x57,
        Read        = 0x52,
    };

public:
    /**
     * @brief   Construct a new Sick_side object
     *          Opens serial line and save file descriptor
     */
    Sick_side();

    /**
     * @brief           Read sensor ID from memory of sensor
     *                  Used for connection check
     *
     * @return uint8_t  Value 0x64, 0x00 if communication fails
     */
    uint8_t ID();

    /**
     * @brief           Turn ON or OFF laser of sensor
     *
     * @param state     New state of laser
     */
    void Laser(bool state);

    /**
     * @brief   Read and calculate distance from distance sensor
     *          Distance is relative to sensors zero point
     *
     * @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
     */
    optional<double> Distance();

private:

    /**
     * @brief Reads distance data from sensor register
     *
     * @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
     */
    uint16_t Distance_data();

    /**
     * @brief       Transmit given data over serial to RS485 transmitter
     *
     * @param data  Data to send via serial line, header and footer are added
     */
    void Send_data(array<uint8_t, 3> data);

    /**
     * @brief           Calculates BCC for given packet of data
     *                  BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
     *
     * @param data      Packet used for calculation
     * @return uint8_t  Values of BCC
     */
    uint8_t Calculate_BCC(array<uint8_t, 6> data);

    /**
     * @brief       Transmits data over UART to RS485 transmitter
     *              Data are also copied to input buffer due to half-duplex connection
     *
     * @param data  Data to transmit
     */
    void Transmit(array<uint8_t, 6> data);

    /**
     * @brief       Data received from sensor
     *              Only usefull data are returned, without STX, ACK/NACK, BCC
     *
     * @return optional<array<uint8_t, 3>>  True and value if communication is valid, False otherwise
     */
    optional<array<uint8_t, 3>> Receive();

    /**
     * @brief Reads amopunt of data in input buffer
     *
     * @return unsigned int Number of bytes in input buffer
     */
    unsigned int Data_available();

};
