#include <Arduino.h>
#include "DSPWriter.h"
#include <Wire.h>
#include "DataConversion.h"

DSPWriter::DSPWriter() {}
DSPWriter::~DSPWriter() {}

void DSPWriter::downloadProgram()
{
    /*
    DSPWriter::writeRegisterBlock(REG_COREREGISTER_IC_1_ADDR, REG_COREREGISTER_IC_1_BYTE, R0_COREREGISTER_IC_1_Default, CORE_REGISTER_R0_REGSIZE);
    DSPWriter::writeRegisterBlock(PROGRAM_ADDR_IC_1, PROGRAM_SIZE_IC_1, Program_Data_IC_1, PROGRAM_REGSIZE);
    DSPWriter::writeRegisterBlock(PARAM_ADDR_IC_1, PARAM_SIZE_IC_1, Param_Data_IC_1, PARAMETER_REGSIZE);
    DSPWriter::writeRegisterBlock(REG_COREREGISTER_IC_1_ADDR, R3_HWCONFIGURATION_IC_1_SIZE, R3_HWCONFIGURATION_IC_1_Default, HARDWARE_CONF_REGSIZE);
    DSPWriter::writeRegisterBlock(REG_COREREGISTER_IC_1_ADDR, REG_COREREGISTER_IC_1_BYTE, R4_COREREGISTER_IC_1_Default, CORE_REGISTER_R4_REGSIZE);*/
}

bool DSPWriter::writeRegisterBlock(uint16_t subAddress, int dataLength, const uint8_t* pdata, uint8_t registerSize)
{
    uint16_t bytesSent = 0;

    while (bytesSent < dataLength)
    {
        uint8_t MSByte = subAddress >> 8;
        uint8_t LSByte = (uint8_t)(subAddress & 0xFF);

        Wire.beginTransmission(DSP_I2C_ADDRESS);
        Wire.write(MSByte);
        Wire.write(LSByte);

        uint8_t chunk = registerSize;
        if ((bytesSent + chunk) > dataLength) {
            chunk = dataLength - bytesSent;
        }

        for (uint8_t i = 0; i < chunk; i++) {
            Wire.write(pdata[bytesSent++]);
        }

        uint8_t err = Wire.endTransmission();

        if (err != 0) {
            // 1 = data too long, 2 = NACK addr, 3 = NACK data, 4 = other
            Serial.printf("I2C NACK addr=0x%04X err=%u\n", subAddress, err);
            return false;
        }

        subAddress++;
        delay(0);
    }
    return true;
}

void DSPWriter::writeRegister(uint16_t memoryAddress, uint8_t length, const uint8_t* data)
{
    uint8_t LSByte = (uint8_t)memoryAddress & 0xFF;
    uint8_t MSByte = memoryAddress >> 8;

    Wire.beginTransmission(DSP_I2C_ADDRESS); // Begin write

    Wire.write(MSByte); // Send high address
    Wire.write(LSByte); // Send low address

    for (uint8_t i = 0; i < length; i++)
        Wire.write(data[i]); // Send all bytes in passed array

    Wire.endTransmission(); // Write out data to I2C and stop transmitting
}

// ---------------------------------------------------------------
// Safeload counter — static so it survives across DSPWriter
// instances, but exposed via resetSafeload() so the TCP bridge
// can flush and reset cleanly on session start and disconnect.
// ---------------------------------------------------------------
static uint8_t s_safeload_count = 0;

void DSPWriter::resetSafeload()
{
    // If the session ended mid-safeload, the DSP still has its own internal
    // safeload counter pointing at those stale slots. Triggering IST here
    // commits whatever is pending to parameter RAM so the DSP counter resets
    // to zero before the next session writes new safeload data.
    if (s_safeload_count > 0) {
        uint8_t ist[2] = { 0x00, 0x3C };
        DSPWriter::writeRegister(dspRegister::CoreRegister, sizeof(ist), ist);
    }
    s_safeload_count = 0;
}

void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, uint8_t* data, bool finished)
{
    uint8_t addr[2]; // Address array

    addr[0] = (memoryAddress >> 8) & 0xFF;
    addr[1] = memoryAddress & 0xFF;

    // Place the 16-bit memory address into the next safeload address slot
    DSPWriter::writeRegister(dspRegister::SafeloadAddress0 + s_safeload_count, sizeof(addr), addr);

    // Q: Why is the safeload register five bytes long for four-byte parameters?
    // A: Safeload registers also support five-byte slew RAM writes. For normal
    //    parameter RAM writes the first byte is always 0x00.
    DSPWriter::writeRegister(dspRegister::SafeloadData0 + s_safeload_count, 5, data);

    s_safeload_count++;

    if (finished == true || s_safeload_count >= 5) // Max 5 safeload slots
    {
        addr[0] = 0x00;
        addr[1] = 0x3C; // IST bit — initiate safeload transfer
        DSPWriter::writeRegister(dspRegister::CoreRegister, sizeof(addr), addr);
        s_safeload_count = 0;
    }
}

void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, int32_t data, bool finished)
{
    uint8_t dataArray[5];
    DataConversion::intToFixed(data, dataArray);
    safeload_writeRegister(memoryAddress, dataArray, finished);
}

void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, float data, bool finished)
{
    uint8_t dataArray[5];
    DataConversion::floatToFixed(data, dataArray);
    safeload_writeRegister(memoryAddress, dataArray, finished);
}

void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, int16_t data,  bool finished) { safeload_writeRegister(memoryAddress, (int32_t)data,  finished); }
void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, uint32_t data, bool finished) { safeload_writeRegister(memoryAddress, (int32_t)data,  finished); }
void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, uint16_t data, bool finished) { safeload_writeRegister(memoryAddress, (int32_t)data,  finished); }
void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, uint8_t data,  bool finished) { safeload_writeRegister(memoryAddress, (int32_t)data,  finished); }
void DSPWriter::safeload_writeRegister(uint16_t memoryAddress, double data,   bool finished) { safeload_writeRegister(memoryAddress, (float)data,   finished); }