#include <HardwareSerial.h>
#include "write.h"

Addr::Addr(InfoCommands cmd)
{
    device_address = 0x36;
    class_address = 0x74;
    rw_flag = 0x01;

    switch (cmd)
    {
    case Model:
        subclass_address = 0x02;
        break;
    case FPGA_Ver:
        subclass_address = 0x03;
        break;
    case FPGA_Comp_Time:
        subclass_address = 0x04;
        break;
    case Module_Software_Ver:
        subclass_address = 0x05;
        break;
    case Module_Software_Comp_Time:
        subclass_address = 0x06;
        break;
    case Camera_Calibration_Ver_Time:
        subclass_address = 0x0B;
        break;
    case ISP_Parameter_Ver:
        subclass_address = 0x0C;
        break;
    case Init_State:
        // TODO why is this one so different?
        class_address = 0x7D;
        subclass_address = 0x06;
        rw_flag = 0x00;
    }
}

Addr::Addr(FuncCommands cmd)
{
    device_address = 0x36;
    rw_flag = 0x00;

    switch (cmd)
    {
    case Manual_Shutter_Calibration:
        class_address = 0x7C;
        subclass_address = 0x02;
        break;
    case Manual_Background_Correction:
        class_address = 0x7C;
        subclass_address = 0x03;
        break;
    case Automatic_Shutter_Control:
        class_address = 0x7C;
        subclass_address = 0x04;
        break;
    case Automatic_Shutter_Switching_Interval:
        class_address = 0x7C;
        subclass_address = 0x05;
        break;
    case Vignetting_Correction:
        class_address = 0x7C;
        subclass_address = 0x0C;
        break;
    case Brightness:
        class_address = 0x78;
        subclass_address = 0x02;
        break;
    case Contrast:
        class_address = 0x78;
        subclass_address = 0x03;
        break;
    case Digital_Enhancement:
        class_address = 0x78;
        subclass_address = 0x10;
        break;
    case Static_De_Noising:
        class_address = 0x78;
        subclass_address = 0x15;
        break;
    case Dynamic_De_Noising:
        class_address = 0x78;
        subclass_address = 0x16;
        break;
    case Defective_Pixel_Correction:
        class_address = 0x78;
        subclass_address = 0x1A;
        break;
    case Palette_Setting:
        class_address = 0x78;
        subclass_address = 0x20;
        break;
    case Factory_Reset:
        class_address = 0x74;
        subclass_address = 0x0F;
        break;
    case Save_Settings:
        class_address = 0x74;
        subclass_address = 0x10;
        break;
    case Mirroring:
        class_address = 0x70;
        subclass_address = 0x11;
        break;
    }
}

void command(Addr cmd, uint8_t data_in[], int data_len, uint8_t buffer[])
{
    char begin = 0xF0;
    char size = data_len + 4;
    char end = 0xFF;

    // GET CHECKSUM //////////////////////////////
    // add all addresses + rw_flag + data and take the last 8 bits as the checksum
    int all_data = 0;
    all_data += cmd.device_address;
    all_data += cmd.class_address;
    all_data += cmd.subclass_address;
    all_data += cmd.rw_flag;
    for (int i = 0; i < data_len; i++)
        all_data += data_in[i];
    char checksum = all_data & 0xFF;
    // ///////////////////////////////////////////

    // CREATE OUTPUT BUFFER //////////////////////
    buffer[0] = begin;
    buffer[1] = size;
    buffer[2] = cmd.device_address;
    buffer[3] = cmd.class_address;
    buffer[4] = cmd.subclass_address;
    buffer[5] = cmd.rw_flag;
    for (int i = 0; i < data_len; i++) // copy in the data to this array
        buffer[i + 6] = data_in[i];
    buffer[6 + data_len] = checksum;
    buffer[7 + data_len] = end;
}
