#include "TAS5720.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define TAG "TAS5720"

void TAS5720_write(TAS5720_t *tas, uint8_t reg, uint8_t data)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    esp_err_t ret = i2c_master_start(cmd);
    if (ret == ESP_OK)
    {
        ESP_LOGI(TAG, "Writing CMD start");
    }
    else
    {
        ESP_LOGE(TAG, "Writing CMD start");
    }
    ret = i2c_master_write_byte(cmd, (tas->address << 1) | I2C_MASTER_WRITE, true);
    if (ret == ESP_OK)
    {
        ESP_LOGI(TAG, "Writing addr");
    }
    else
    {
        ESP_LOGE(TAG, "Error writing addr");
    }
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_write_byte(cmd, data, true);
    i2c_master_stop(cmd);
    ret = i2c_master_cmd_begin(tas->i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    if (tas->debug)
    {
        if (ret == ESP_OK)
        {
            ESP_LOGI(TAG, "Writing to register 0x%02X with data 0x%02X", reg, data);
        }
        else
        {
            ESP_LOGE(TAG, "Error writing to register 0x%02X with data 0x%02X", reg, data);
        }
    }
}

uint8_t TAS5720_read(TAS5720_t *tas, uint8_t reg)
{
    uint8_t data = 0;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (tas->address << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (tas->address << 1) | I2C_MASTER_READ, true);
    i2c_master_read_byte(cmd, &data, I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(tas->i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    if (tas->debug)
    {
        if (ret == ESP_OK)
        {
            ESP_LOGI(TAG, "Reading from register 0x%02X with data 0x%02X", reg, data);
        }
        else
        {
            ESP_LOGE(TAG, "Error reading from register 0x%02X with data 0x%02X", reg, data);
        }
    }
    return data;
}

void TAS5720_init(TAS5720_t *tas, i2c_port_t i2c_num, uint8_t address, bool debug)
{
    tas->i2c_num = i2c_num;
    tas->address = address;
    tas->debug = debug;

    /*i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = 8,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = 9,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 100000,
    };
    i2c_param_config(tas->i2c_num, &conf);
    i2c_driver_install(tas->i2c_num, conf.mode, 0, 0, 0);*/
}

uint8_t TAS5720_getDeviceIdentification(TAS5720_t *tas)
{
    return TAS5720_read(tas, TAS5720_ADDRESS_DEVICE_IDENTIFICATION);
}

bool TAS5720_getSleepMode(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL) & 0x02) != 0;
}

void TAS5720_setSleepMode(TAS5720_t *tas, bool sleep)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL);
    reg_value = (reg_value & ~0x02) | (sleep ? 0x02 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_POWER_CONTROL, reg_value);
}

bool TAS5720_getShutdown(TAS5720_t *tas)
{
    return !(TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL) & 0x01);
}

void TAS5720_setShutdown(TAS5720_t *tas, bool shutdown)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL);
    reg_value = (reg_value & ~0x01) | (shutdown ? 0x00 : 0x01);
    TAS5720_write(tas, TAS5720_ADDRESS_POWER_CONTROL, reg_value);
}

SAIFormat TAS5720_getSerialAudioInterfaceFormat(TAS5720_t *tas)
{
    return (SAIFormat)(TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL) & 0x07);
}

void TAS5720_setSerialAudioInterfaceFormat(TAS5720_t *tas, SAIFormat format)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL);
    reg_value = (reg_value & ~0x07) | (format & 0x07);
    TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CONTROL, reg_value);
}

bool TAS5720_getSpeed(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL) & 0x08) != 0;
}

void TAS5720_setSpeed(TAS5720_t *tas, bool doubleSpeed)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL);
    reg_value = (reg_value & ~0x08) | (doubleSpeed ? 0x08 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CONTROL, reg_value);
}

DigitalBoost TAS5720_getDigitalBoost(TAS5720_t *tas)
{
    return (DigitalBoost)((TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL) & 0x30) >> 4);
}

void TAS5720_setDigitalBoost(TAS5720_t *tas, DigitalBoost boost)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL);
    reg_value = (reg_value & ~0x30) | ((boost & 0x03) << 4);
    TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CONTROL, reg_value);
}

bool TAS5720_getHighPassFilterMode(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL) & 0x80) != 0;
}

void TAS5720_setHighPassFilterMode(TAS5720_t *tas, bool bypass)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CONTROL);
    reg_value = (reg_value & ~0x80) | (bypass ? 0x80 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CONTROL, reg_value);
}

void TAS5720_muteLeft(TAS5720_t *tas, bool mute)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION);
    reg_value = (reg_value & ~0x01) | (mute ? 0x01 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION, reg_value);
}

void TAS5720_muteRight(TAS5720_t *tas, bool mute)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION);
    reg_value = (reg_value & ~0x02) | (mute ? 0x02 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION, reg_value);
}

void TAS5720_mute(TAS5720_t *tas, bool mute)
{
    TAS5720_muteLeft(tas, mute);
    TAS5720_muteRight(tas, mute);
}

void TAS5720_setFade(TAS5720_t *tas, bool fade)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION);
    reg_value = (reg_value & ~0x80) | (fade ? 0x80 : 0x00);
    TAS5720_write(tas, TAS5720_ADDRESS_VOLUME_CONTROL_CONFIGURATION, reg_value);
}

void TAS5720_getVolume(TAS5720_t *tas)
{
    tas->volumeLeft = TAS5720_read(tas, TAS5720_ADDRESS_VOLUME_CONTROL_LEFT);
    tas->volumeRight = TAS5720_read(tas, TAS5720_ADDRESS_VOLUME_CONTROL_RIGHT);
}

void TAS5720_setVolume(TAS5720_t *tas, uint8_t left, uint8_t right)
{
    TAS5720_write(tas, TAS5720_ADDRESS_VOLUME_CONTROL_LEFT, left);
    //TAS5720_write(tas, TAS5720_ADDRESS_VOLUME_CONTROL_RIGHT, right);
}

void TAS5720_setVolumeBoth(TAS5720_t *tas, uint8_t volume)
{
    TAS5720_setVolume(tas, volume, volume);
}

ChannelSelection TAS5720_getChannelSelection(TAS5720_t *tas)
{
    return (ChannelSelection)((TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL) & 0x02) >> 1);
}

void TAS5720_setChannelSelection(TAS5720_t *tas, ChannelSelection channel)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL);
    reg_value = (reg_value & ~0x02) | ((channel & 0x01) << 1);
    TAS5720_write(tas, TAS5720_ADDRESS_ANALOG_CONTROL, reg_value);
}

AnalogGain TAS5720_getAnalogGain(TAS5720_t *tas)
{
    return (AnalogGain)((TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL) & 0x0C) >> 2);
}

void TAS5720_setAnalogGain(TAS5720_t *tas, AnalogGain gain)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL);
    reg_value = (reg_value & ~0x0C) | ((gain & 0x03) << 2);
    TAS5720_write(tas, TAS5720_ADDRESS_ANALOG_CONTROL, reg_value);
}

PWMRate TAS5720_getPWMRate(TAS5720_t *tas)
{
    return (PWMRate)((TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL) & 0x70) >> 4);
}

void TAS5720_setPWMRate(TAS5720_t *tas, PWMRate rate)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_ANALOG_CONTROL);
    reg_value = (reg_value & ~0x70) | ((rate & 0x07) << 4);
    TAS5720_write(tas, TAS5720_ADDRESS_ANALOG_CONTROL, reg_value);
}

bool TAS5720_getOverTemperatureErrorStatus(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x01) != 0;
}

bool TAS5720_getOutputDCErrorStatus(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x02) != 0;
}

bool TAS5720_getOverCurrentErrorStatus(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x04) != 0;
}

bool TAS5720_getClockErrorStatus(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x08) != 0;
}

bool TAS5720_getErrorStatus(TAS5720_t *tas)
{
    return (TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x0F) != 0;
}

OCEThreshold TAS5720_getOCEThreshold(TAS5720_t *tas)
{
    return (OCEThreshold)((TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS) & 0x30) >> 4);
}

void TAS5720_setOCEThreshold(TAS5720_t *tas, OCEThreshold threshold)
{
    uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS);
    reg_value = (reg_value & ~0x30) | ((threshold & 0x03) << 4);
    TAS5720_write(tas, TAS5720_ADDRESS_FAULT_CONFIGURATION_ERROR_STATUS, reg_value);
}

uint32_t TAS5720_getDigitalClipper(TAS5720_t *tas)
{
    uint32_t clip = 0;
    clip |= ((TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CLIPPER_1) >> 2) & 0x3F);
    clip |= (TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CLIPPER_2) << 6);
    clip |= (((TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL) >> 2) & 0x3F) << 14);
    return clip;
}

void TAS5720_setDigitalClipper(TAS5720_t *tas, uint32_t clip)
{
    if (clip < 0x100000)
    {
        uint8_t reg_value = TAS5720_read(tas, TAS5720_ADDRESS_DIGITAL_CLIPPER_1);
        reg_value = (reg_value & ~0xFC) | ((clip & 0x3F) << 2);
        TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CLIPPER_1, reg_value);
        TAS5720_write(tas, TAS5720_ADDRESS_DIGITAL_CLIPPER_2, (clip >> 6) & 0xFF);
        reg_value = TAS5720_read(tas, TAS5720_ADDRESS_POWER_CONTROL);
        reg_value = (reg_value & ~0xFC) | ((clip >> 14) & 0x3F);
        TAS5720_write(tas, TAS5720_ADDRESS_POWER_CONTROL, reg_value);
    }
}
