#include <math.h>
#include <vector>

#include "custom_cast.h"
#include "knight.h"
#include "timestamp.h"

Knight::Knight (int board_id, struct BrainFlowInputParams params) : KnightBase (board_id, params)
{
}

void Knight::read_thread ()
{
    /*
    [0] 1 Byte: Start byte
    [1] 2 Byte: Sample Number
    [2-3] 3-4 Bytes: EXG channel 1
    [4-5] 5-6 Bytes: Data value for EXG channel 2
    [6-7] 7-8 Bytes: Data value for EXG channel 3
    [8-9] 9-10 Bytes: Data value for EXG channel 4
    [10-11] 11-12 Bytes: Data value for EXG channel 5
    [12-13] 13-14 Bytes: Data value for EXG channel 6
    [14-15] 15-16 Bytes: Data value for EXG channel 7
    [16-17] 17-18 Bytes: Data value for EXG channel 8
    [18] 19 Byte: Data LOFF STATP
    [19] 20 Byte: Data LOFF STATN
    [20] 21 Byte: End byte
    */

    int res;
    unsigned char b[20] = {0};
    float eeg_scale = 4 / float ((pow (2, 15) - 1)) / 12 * 1000000.;
    int num_rows = board_descr["default"]["num_rows"];
    double *package = new double[num_rows];
    for (int i = 0; i < num_rows; i++)
    {
        package[i] = 0.0;
    }
    bool first_package_received = false;

    std::vector<int> eeg_channels = board_descr["default"]["eeg_channels"];
    std::vector<int> other_channels = board_descr["default"]["other_channels"];

    while (keep_alive)
    {
        // checking the start byte
        res = serial->read_from_serial_port (b, 1);
        if (res != 1)
        {
            safe_logger (spdlog::level::debug, "unable to read 1 byte, {}");
            continue;
        }
        if (b[0] != KnightBase::start_byte)
        {
            continue;
        }

        int remaining_bytes = 20;
        int pos = 0;
        while ((remaining_bytes > 0) && (keep_alive))
        {
            res = serial->read_from_serial_port (b + pos, remaining_bytes);
            remaining_bytes -= res;
            pos += res;
        }

        if (!keep_alive)
        {
            break;
        }

        if (b[19] != KnightBase::end_byte)
        {
            safe_logger (spdlog::level::warn, "Wrong end byte {}", b[19]);
            continue;
        }

        // package number CHANGE TO 1 if not working
        package[board_descr["default"]["package_num_channel"].get<int> ()] = (double)b[0];

        // exg data retrieval
        for (unsigned int i = 0; i < eeg_channels.size (); i++)
        {
            package[eeg_channels[i]] =
                eeg_scale * cast_16bit_to_int32 (b + 1 + 2 * i); // CHANGE TO 2+2*i if not working
        }

        // other channel data retrieval
        package[other_channels[0]] = (double)b[17]; // LOFF STATP
        package[other_channels[1]] = (double)b[18]; // LOFF STATN

        // time stamp channel
        package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();

        push_package (package);
    }
    delete[] package;
}