#include "encoder_drv_kth5763.h"
#include "encoder_port.h"
#include "i2c.h"
#include "soc_errno.h"
#include "sys_config.h"

#define ENABLE_PRINT_INFO 0
#if ENABLE_PRINT_INFO
#define static_print_info(...) sys_enc_log_i(__VA_ARGS__)  // 一般信息打印宏控制
#define static_print_warn(...) sys_enc_log_w(__VA_ARGS__)  // 警告信息打印一般常开
#define static_print_error(...) sys_enc_log_e(__VA_ARGS__) // 错误信息打印一般常开
#else
#define static_print_info(...)
#define static_print_warn(...)
#define static_print_error(...)
#endif

uint32_t kth5763_write_reg(uint8_t reg_addr, uint16_t data)
{
    uint8_t buf[4] = {0};
    buf[0] = WRITE_REG_CMD;
    buf[1] = data >> 8;
    buf[2] = data & 0xFF;
    buf[3] = reg_addr << 2;

    uint8_t recv[4] = {0};
    uint32_t ret = encoder_i2c_data_write_read(buf, 4, recv, 4);
    if (ret != EXT_ERR_SUCCESS) {
        return ret;
    }
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_read_reg(uint8_t reg_addr, uint16_t *data)
{
    uint8_t recv_buf[3] = {0};
    uint8_t cmd_buf[2] = {0};
    cmd_buf[0] = READ_REG_CMD;
    cmd_buf[1] = reg_addr << 2;

    uint32_t ret = encoder_i2c_data_write_read(cmd_buf, 2, recv_buf, 3);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: read reg failed, ret=0x%x", ret);
        return ret;
    }

    *data = (recv_buf[1] << 8) + recv_buf[2];
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_enter_idle(void)
{
    uint8_t status = 0;
    uint32_t ret = encoder_i2c_data_read(EXIT_IDLE_MODE_CMD, 1, &status, 1);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: enter idle failed, ret=0x%x", ret);
        return ret;
    }
    static_print_info("KTH5763: enter idle success status %x", status);
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_exit_idle(void)
{
    uint8_t status = 0;
    uint32_t ret = encoder_i2c_data_read(EXIT_IDLE_MODE_CMD, 1, &status, 1);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: exit idle failed, ret=0x%x", ret);
        return ret;
    }
    static_print_info("KTH5763: exit idle success status %x", status);
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_reset(void)
{
    uint32_t ret = encoder_i2c_cmd_write(RESET_CMD);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: reset failed, ret=0x%x", ret);
        return ret;
    }
    static_print_info("KTH5763: reset success");
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_init_reg(void)
{
    uint32_t ret = 0;

#if (CALIBRATION_MODE == 0)
    /* 线性校准 */
    ret = kth5763_write_reg(0x1B, 0xC718);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: write reg 0x1B failed, ret=0x%x", ret);
        return ret;
    }
#endif

    /**
     * 15 : 14  角度输出平面选择
     * 10 : 9   磁场测量的 ADC 过采样率
     * 2 : 0    数字滤波控制参数
     * 其余保留
     */
#if (CALIBRATION_MODE)
    ret = kth5763_write_reg(0x1C, 0x0637);
#else
    ret = kth5763_write_reg(0x1C, 0x9570);
#endif
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: write reg 0x1C failed, ret=0x%x", ret);
        return ret;
    }

    /**
     * 9 : 6 测量选通信号
     * 5 : 0 周期循环测量模式的待机状态时长
     * 其余保留
     */
#if (CALIBRATION_MODE)
    ret = kth5763_write_reg(0x1D, 0x03C2);
#else
    ret = kth5763_write_reg(0x1D, 0x0002);
#endif
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: write reg 0x1D failed, ret=0x%x", ret);
        return ret;
    }

    /* 14 : 0 设置零点位置 */
#if (CALIBRATION_MODE)
    ret = kth5763_write_reg(0x1E, 0x0000);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: write reg 0x1E failed, ret=0x%x", ret);
        return ret;
    }
#endif

    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_duty_cycle_mode(kth5763_duty_cycle_mode_t mode)
{
    uint8_t status = 0;
    uint32_t ret = encoder_i2c_data_read(DUTY_CYCLE_MODE_CMD | mode, 1, &status, 1);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: set duty cycle failed, ret=0x%x", ret);
        return ret;
    }
    static_print_info("KTH5763: set duty cycle success %x", status);
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_measurement(kth5763_duty_cycle_mode_t mode, uint16_t *ret_angle)
{
    uint8_t buf[5] = {0};
    uint8_t cmd = READ_MEASUREMENT_CMD | 0x0F;
    uint32_t ret = encoder_i2c_data_read(cmd, 1, buf, 4);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: read measurement failed, ret=0x%x", ret);
        return ret;
    }
    *ret_angle = (uint16_t)(buf[3] & 0xFF) * 10 * 360.0 / 256;
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_xyz_measurement(kth5763_duty_cycle_mode_t mode, uint8_t *ret_array)
{
    uint8_t buf[9] = {0};
    if (ret_array == NULL) {
        return EXT_ERR_FAILURE;
    }
    uint8_t cmd = READ_MEASUREMENT_CMD | mode;
    uint32_t ret = encoder_i2c_data_read(cmd, 1, buf, 9);
    if (ret != EXT_ERR_SUCCESS) {
        static_print_error("KTH5763: read measurement failed, ret=0x%x", ret);
        return ret;
    }
    memcpy_s(ret_array, 9, buf, 9);
    return EXT_ERR_SUCCESS;
}

uint32_t kth5763_init(void)
{
    uint16_t chip_id = 0;
    uint32_t ret = 0;
    ret = kth5763_exit_idle();
    if (ret != ENC_SUCC) {
        static_print_error("KTH5763: exit idle, %x", ret);
        return ret;
    }

    ret = kth5763_read_reg(KTH5763_CHIPID_REG, &chip_id);
    if (ret != ENC_SUCC || chip_id != KTH5763_CHIP_ID) {
        static_print_error("KTH5763: get chip id failed, %x", ret);
        return ret;
    }
    static_print_info("KTH5763: chip id = 0x%x", chip_id);

    ret = kth5763_init_reg();
    if (ret != ENC_SUCC) {
        static_print_error("KTH5763: init reg failed, %x", ret);
        return ret;
    }

#if (CALIBRATION_MODE)
    ret = kth5763_duty_cycle_mode(XYZ_MODE);
#else
    ret = kth5763_duty_cycle_mode(XZ_MODE);
#endif
    if (ret != ENC_SUCC) {
        static_print_error("KTH5763: duty cycle mode failed, %x", ret);
        return ret;
    }

    return ENC_SUCC;
}