#include "MLX75306.hpp"

MLX75306::MLX75306(SPI_HandleTypeDef SPI_handle, Chip_select_pin SPI_CS) :
    SPI_handle(SPI_handle), SPI_CS(SPI_CS){ }

void MLX75306::Init(){
    Reset();
}

void MLX75306::Reset(){
    Command(Commands::Chip_reset, { 0, 0 });
}

void MLX75306::Wake_up(){
    Command(Commands::Wake_up, { 0, 0 });
}

void MLX75306::Zebra_pattern_1(){
    Command(Commands::Test_zebra_pattern_1, { 0, 0 });
}

array<uint8_t, 159> MLX75306::Read_all_8bit(){
    // Command is set to read all pixels
    return Command<159>(Commands::Read_out_8b, { 0x02, 0x8f });
}

void MLX75306::Integrate(double time_us){
    const unsigned int f_RCO     = 10000000;
    const double min_time_us     = 0.1;
    const double max_time_us     = 100000;
    int64_t integration_register = 0;

    // Cap values
    if (time_us < min_time_us) {
        time_us = min_time_us;
    }
    if (time_us > max_time_us) {
        time_us = max_time_us;
    }

    // Calculate value for short integration
    integration_register = ((time_us / 1000000) * f_RCO) + 4;

    // If integration time is longer then maximal short integration time used long integration command
    if (integration_register > ((1 << 16) - (11 * 16))) {
        integration_register = (((time_us / 1000000) * f_RCO) - 11) / 16;


        // Handle overflow and underflow of integration register
        if (integration_register > (1 << 16)) {
            integration_register = (1 << 16) - 1;
        } else if (integration_register < 0) {
            integration_register = 1;
        }
        // Long integration
        Command(Commands::Start_integration_long,
            {
                static_cast<uint8_t>((integration_register & 0xff00) >> 8),
                static_cast<uint8_t>(integration_register & 0xff)
            }
        );
    } else {
        // Short integration
        Command(Commands::Start_integration,
            {
                static_cast<uint8_t>((integration_register & 0xff00) >> 8),
                static_cast<uint8_t>(integration_register & 0xff)
            }
        );
    }
} // MLX75306::Integrate

MLX75306::Status_byte MLX75306::Status(){
    array<uint8_t, 2> payload = { 0, 0 };
    auto status = Command(Commands::Idle, payload)[0];
    Status_byte status_struct = *((Status_byte *) &(status));
    return status_struct;
}