#define TPS25751_REG_CMD1   0x08
#define TPS25751_REG_DATA1  0x09

static int tps25751_write_bq25798_iindpm(const msdk_device_t *dev)
{
    int ret = MSDK_STATUS__ERROR;
    uint8_t data1_setup[] = { 0x05, 0x6B, 0x03, 0x0A, 0x00, 0x58 };

    ret = write_reg(dev, TPS25751_REG_DATA1, data1_setup, sizeof(data1_setup));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to write TPS25751 DATA1");
        return ret;
    }

    // Optional: Verify write to DATA1
    uint8_t data1_readback[6] = {0};
    ret = read_reg(dev, TPS25751_REG_DATA1, data1_readback, sizeof(data1_readback));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to read back TPS25750 DATA1");
        return ret;
    }
    MSDK_LOG_DBG("DATA1 Readback: %02X %02X %02X %02X %02X %02X",
        data1_readback[0], data1_readback[1], data1_readback[2],
        data1_readback[3], data1_readback[4], data1_readback[5]);

    // Step 2: Write 'I2Cw' to CMD1 register (0x08)
    uint8_t i2cw_cmd[] = { 0x04, 'I', '2', 'C', 'w' };  
    ret = write_reg(dev, TPS25751_REG_CMD1, i2cw_cmd, sizeof(i2cw_cmd));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to send I2Cw command to CMD1");
        return ret;
    }

    // Step 3: Read back CMD1 status
    uint8_t cmd_status[5] = {0};
    ret = read_reg(dev, TPS25751_REG_CMD1, cmd_status, sizeof(cmd_status));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to read CMD1 status\n");
        return ret;
    }

    MSDK_LOG_DBG("CMD1 Status: %02X %02X %02X %02X %02X",
        cmd_status[0], cmd_status[1], cmd_status[2], cmd_status[3],cmd_status[4]);


    return MSDK_STATUS__OK;
}

static int tps25751_read_bq25798_iindpm(const msdk_device_t *dev, uint8_t *data_out)
{
    int ret = MSDK_STATUS__ERROR;
 
    // Step 1: Write DATA1 (0x09), with data to set up TPS25751 to read from BQ25798
    uint8_t setup_data[4];
    setup_data[0] = 0x03;       // length = 3 bytes
    setup_data[1] = 0x6B;       // BQ25798 I2C slave address
    setup_data[2] = 0x0A;       // target register in BQ25798
    setup_data[3] = 0x02;       // read 2 bytes
    ret = write_reg(dev, TPS25751_REG_DATA1, setup_data, sizeof(setup_data));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to write TPS25751 DATA1");
        return ret;
    }
 
    // Step 2: Write CMD1 (0x08) with 'I2Cr' command
    uint8_t i2cr_cmd[5] = { 0x04, 'I', '2', 'C', 'r' };
    ret = write_reg(dev, TPS25751_REG_CMD1 , i2cr_cmd, sizeof(i2cr_cmd));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to write TPS25751 CMD1 for I2Cr");
        return ret;
    }
 
    // Step 3: Read CMD1 back to confirm command executed (should return 0x00 0x00 0x00 0x00)
    uint8_t cmd_status[4] = {0};
    ret = read_reg(dev, TPS25751_REG_CMD1, cmd_status, sizeof(cmd_status));
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to read TPS25751 CMD1 status");
        return ret;
    }
	MSDK_LOG_DBG("CMD1 Status: %02X %02X %02X %02X", 
              cmd_status[0], cmd_status[1], cmd_status[2], cmd_status[3]);
    /*if (cmd_status[0] != 0x00 || cmd_status[1] != 0x00 || cmd_status[2] != 0x00 || cmd_status[3] != 0x00) {
        MSDK_LOG_ERR("TPS25751 I2Cr command failed to execute properly");
        return MSDK_STATUS__ERROR;
    }*/
 
    // Step 4: Read back the 2 bytes result from DATA1 (0x09)
    ret = read_reg(dev, TPS25751_REG_DATA1, data_out, 8);
    if (ret != MSDK_STATUS__OK) {
        MSDK_LOG_ERR("Failed to read response from TPS25750 DATA1");
        return ret;
    }
	MSDK_LOG_DBG("Read Data from BQ25798: %02X %02X  %02X %02X %02X %02X  %02X %02X", data_out[0], data_out[1],data_out[2], data_out[3], data_out[4], data_out[5],data_out[6], data_out[7]);
    return MSDK_STATUS__OK;   
}  

void tps25751_download_patch(void)
{
	uint8_t buf[8] = {};
    tps25751_download_patch_bundle(&nina_tps25751_device, tps25751_patch_data, tps25751_patch_size);
    msdk_time_delay_ms(500);
    tps25751_read_bq25798_iindpm(&nina_tps25751_device, buf);
    msdk_time_delay_ms(500);
	
    MSDK_LOG_DBG("Start to write BQ25798 register!");
    tps25751_write_bq25798_iindpm(&nina_tps25751_device);
    msdk_time_delay_ms(500);
    tps25751_read_bq25798_iindpm(&nina_tps25751_device, buf);
    msdk_time_delay_ms(500);
    
}