#include "bsp_wm8904.h"
#include <stdio.h>
#include <string.h>
#include "FreeRTOS.h"
#include "task.h"
#include "elog.h"

static const char *TAG = "bsp_wm8904";

#define BSP_WM8904_I2C_ADDR 0x34

/*******************************************************************************
 * Definitions
 ******************************************************************************/

#define WM8904_RESET (0x00)
#define WM8904_ANALOG_ADC_0 (0x0A)
#define WM8904_POWER_MGMT_0 (0x0C)
#define WM8904_POWER_MGMT_2 (0x0E)
#define WM8904_POWER_MGMT_6 (0x12)
#define WM8904_CLK_RATES_0 (0x14)
#define WM8904_CLK_RATES_1 (0x15)
#define WM8904_CLK_RATES_2 (0x16)
#define WM8904_AUDIO_IF_0 (0x18)
#define WM8904_AUDIO_IF_1 (0x19)
#define WM8904_AUDIO_IF_2 (0x1A)
#define WM8904_AUDIO_IF_3 (0x1B)
#define WM8904_DAC_DIG_1 (0x21)
#define WM8904_ANALOG_LEFT_IN_0 (0x2C)
#define WM8904_ANALOG_RIGHT_IN_0 (0x2D)
#define WM8904_ANALOG_LEFT_IN_1 (0x2E)
#define WM8904_ANALOG_RIGHT_IN_1 (0x2F)
#define WM8904_ANALOG_OUT1_LEFT (0x39)
#define WM8904_ANALOG_OUT1_RIGHT (0x3A)
#define WM8904_ANALOG_OUT2_LEFT (0x3B)
#define WM8904_ANALOG_OUT2_RIGHT (0x3C)
#define WM8904_ANALOG_OUT12_ZC (0x3D)
#define WM8904_DC_SERVO_0 (0x43)
#define WM8904_DC_SERVO_1 (0x44)
#define WM8904_ANALOG_HP_0 (0x5A)
#define WM8904_CHRG_PUMP_0 (0x62)
#define WM8904_CLS_W_0 (0x68)
#define WM8904_WRT_SEQUENCER_0 (0x6C)
#define WM8904_WRT_SEQUENCER_3 (0x6F)
#define WM8904_WRT_SEQUENCER_4 (0x70)

/*******************************************************************************
 * Code
 ******************************************************************************/

HAL_StatusTypeDef WM8904_WriteRegister(I2C_HandleTypeDef *hi2c, uint8_t reg_addr, uint16_t reg_value)
{
  HAL_StatusTypeDef status;
  uint8_t reg_bytes[2] = { reg_value >> 8, reg_value & 0xFF };

  uint8_t retries = 0;
  while (1) {
    status = HAL_I2C_Mem_Write(hi2c, BSP_WM8904_I2C_ADDR, reg_addr, 1, reg_bytes, sizeof(reg_bytes), 100);

    if (status == HAL_OK) {
      elog_debug(TAG, "write register@0x%02X success, value=0x%04X", reg_addr, reg_value);
      break;
    } else {
      elog_error(TAG, "write register@0x%02X failed", reg_addr);
      I2C_RESET_CR2(hi2c);
      retries ++;
      if (retries > 10) {
        break;
      }
      vTaskDelay(pdMS_TO_TICKS(10));
    }
  }

  return status;
}

HAL_StatusTypeDef WM8904_ReadRegister(I2C_HandleTypeDef *hi2c, uint8_t reg_addr, uint16_t *reg_value_out)
{
  HAL_StatusTypeDef status;
  uint8_t reg_bytes[2] = { 0 };

  uint8_t retries = 0;
  while (1) {
    status = HAL_I2C_Mem_Read(hi2c, BSP_WM8904_I2C_ADDR, reg_addr, 1, reg_bytes, sizeof(reg_bytes), 100);
    *reg_value_out = (reg_bytes[0] << 8) | reg_bytes[1];

    if (status == HAL_OK) {
      elog_debug(TAG, "read register@0x%02X success, value=0x%04X", reg_addr, *reg_value_out);
      break;
    } else {
      elog_error(TAG, "read register@0x%02X failed", reg_addr);
      I2C_RESET_CR2(hi2c);
      retries ++;
      if (retries > 10) {
        break;
      }
      vTaskDelay(pdMS_TO_TICKS(10));
    }
  }

  return status;
}

HAL_StatusTypeDef WM8904_ModifyRegister(I2C_HandleTypeDef *hi2c, uint8_t reg, uint16_t mask, uint16_t value)
{
  uint16_t reg_value;
  HAL_StatusTypeDef ret;

  ret = WM8904_ReadRegister(hi2c, reg, &reg_value);
  if(ret != HAL_OK) {
    return ret;
  }

  reg_value &= (uint16_t)~mask;
  reg_value |= value;

  return WM8904_WriteRegister(hi2c, reg, reg_value);
}

HAL_StatusTypeDef WM8904_WaitOnWriteSequencer(I2C_HandleTypeDef *hi2c)
{
  HAL_StatusTypeDef result;
  uint16_t value;

  do {
    vTaskDelay(pdMS_TO_TICKS(20));
    result = WM8904_ReadRegister(hi2c, WM8904_WRT_SEQUENCER_4, &value);
  } while ((result == HAL_OK) && (value & 0x0001));

  return result;
}

HAL_StatusTypeDef WM8904_Init(I2C_HandleTypeDef *hi2c)
{
  HAL_StatusTypeDef result;

  elog_info(TAG, "configuring codec");

  /* reset */
  result = WM8904_WriteRegister(hi2c, WM8904_RESET, 0x0000);
  if (result != HAL_OK) {
    return result;
  }

  /* MCLK_INV=0, SYSCLK_SRC=0, TOCLK_RATE=1, OPCLK_ENA=0,
   * CLK_SYS_ENA=1, CLK_DSP_ENA=1, TOCLK_ENA=1 */
  result = WM8904_WriteRegister(hi2c, WM8904_CLK_RATES_2, 0x1007);
  if (result != HAL_OK) {
    return result;
  }

  /* Set Clock ratio and sample rate */
  result = WM8904_WriteRegister(hi2c, WM8904_CLK_RATES_1, 0x0C05); //sysclk/fs=256, sample_rate=44-48khz
  if (result != HAL_OK) {
    return result;
  }

  /* WSEQ_ENA=1, WSEQ_WRITE_INDEX=0_0000 */
  result = WM8904_WriteRegister(hi2c, WM8904_WRT_SEQUENCER_0, 0x0100);
  if (result != HAL_OK) {
    return result;
  }

  /* WSEQ_ABORT=0, WSEQ_START=1, WSEQ_START_INDEX=00_0000 */
  result = WM8904_WriteRegister(hi2c, WM8904_WRT_SEQUENCER_3, 0x0100);
  if (result != HAL_OK) {
    return result;
  }

  result = WM8904_WaitOnWriteSequencer(hi2c);
  if (result != HAL_OK) {
    return result;
  }

  /* TOCLK_RATE_DIV16=0, TOCLK_RATE_x4=0, SR_MODE=0, MCLK_DIV=0
    * (Required for MMCs: SGY, KRT see erratum CE000546) */
  result = WM8904_WriteRegister(hi2c, WM8904_CLK_RATES_0, 0x845E);
  if (result != HAL_OK) {
    return result;
  }

  /* HPL_PGA_ENA=1, HPR_PGA_ENA=1 */
  // result = WM8904_WriteRegister(hi2c, WM8904_POWER_MGMT_2, 0x0003);
  // if (result != HAL_OK) {
  //   return result;
  // }

  /* DACL_ENA=1, DACR_ENA=1 */
  // result = WM8904_WriteRegister(hi2c, WM8904_POWER_MGMT_6, 0x000C);
  // if (result != HAL_OK) {
  //   return result;
  // }

  /* BCLK_DIR=1 (output), AIF_WL=00 (16-bits) */
  result = WM8904_WriteRegister(hi2c, WM8904_AUDIO_IF_1, 0x0002 | (1ul << 6)); //16bits, I2S
  if (result != HAL_OK) {
    return result;
  }

  /* BCLK_DIV=(sysclk/8) */
  result = WM8904_WriteRegister(hi2c, WM8904_AUDIO_IF_2, 0x0008);
  if (result != HAL_OK) {
    return result;
  }

  /* LRCLK_DIR=1 (output), LRCLK_RATE=(BCLK/32) */
  result = WM8904_WriteRegister(hi2c, WM8904_AUDIO_IF_3, 0x0020 | (1ul << 11));
  if (result != HAL_OK) {
    return result;
  }




  /* FLL_FRACN_EN=1 */
  result = WM8904_WriteRegister(hi2c, 0x74, (1ul << 2));
  if (result != HAL_OK) {
    return result;
  }

  /* FLL_OUTDIV=000111 (divice by 8), FLL_CTRL_RATE=000 (FVCO/1), FLL_FRATIO=000 (divide by 1) */
  result = WM8904_WriteRegister(hi2c, 0x75, (7ul << 8));
  if (result != HAL_OK) {
    return result;
  }

  /* FLL_K=0.35 */
  result = WM8904_WriteRegister(hi2c, 0x76, 0x599A);
  if (result != HAL_OK) {
    return result;
  }

  /* FLL_N=7 */
  result = WM8904_WriteRegister(hi2c, 0x77, 7ul << 5);
  if (result != HAL_OK) {
    return result;
  }

  /* MCLK_INV=0, SYSCLK_SRC=1, TOCLK_RATE=1, OPCLK_ENA=0,
   * CLK_SYS_ENA=1, CLK_DSP_ENA=1, TOCLK_ENA=1 */
  result = WM8904_WriteRegister(hi2c, WM8904_CLK_RATES_2, 0x1007 | (1ul << 14)); //sysclk connects to fll output
  if (result != HAL_OK) {
    return result;
  }

  /* FLL_FRACN_EN=1, FLL_ENA=1 */
  result = WM8904_WriteRegister(hi2c, 0x74, (1ul << 2) | (1ul << 0));
  if (result != HAL_OK) {
    return result;
  }

  vTaskDelay(pdMS_TO_TICKS(100));





  /* DAC_MONO=0, DAC_SB_FILT-0, DAC_MUTERATE=0, DAC_UNMUTE RAMP=0,
    * DAC_OSR128=1, DAC_MUTE=0, DEEMPH=0 (none) */
  result = WM8904_WriteRegister(hi2c, WM8904_DAC_DIG_1, 0x0040); //unmute DAC
  if (result != HAL_OK) {
    return result;
  }

  /* HPL_RMV_SHORT=1, HPL_ENA_OUTP=1, HPL_ENA_DLY=1, HPL_ENA=1,
    * HPR_RMV_SHORT=1, HPR_ENA_OUTP=1, HPR_ENA_DLY=1, HPR_ENA=1 */
  result = WM8904_WriteRegister(hi2c, WM8904_ANALOG_HP_0, 0x00FF);
  if (result != HAL_OK) {
    return result;
  }

  /* CP_DYN_PWR=1 */
  result = WM8904_WriteRegister(hi2c, WM8904_CLS_W_0, 0x0001);
  if (result != HAL_OK) {
    return result;
  }

  /* CP_ENA=1 */
  // result = WM8904_WriteRegister(hi2c, WM8904_CHRG_PUMP_0, 0x0001);
  // if (result != HAL_OK) {
  //   return result;
  // }

  result = WM8904_SetVolume(&hi2c1, 30);
  if (result != HAL_OK) {
    return result;
  }

  return HAL_OK;
}

HAL_StatusTypeDef WM8904_SetVolume(I2C_HandleTypeDef *hi2c, uint16_t volume)
{
  HAL_StatusTypeDef result;

  result = WM8904_WriteRegister(hi2c, WM8904_ANALOG_OUT1_LEFT, volume);
  if (result != HAL_OK) {
    return result;
  }

  result = WM8904_WriteRegister(hi2c, WM8904_ANALOG_OUT1_RIGHT, volume | 1ul << 7);
  if (result != HAL_OK) {
    return result;
  }

  result = WM8904_WriteRegister(hi2c, WM8904_ANALOG_OUT2_LEFT, volume);
  if (result != HAL_OK) {
    return result;
  }

  result = WM8904_WriteRegister(hi2c, WM8904_ANALOG_OUT2_RIGHT, volume | 1ul << 7);
  if (result != HAL_OK) {
    return result;
  }

  return HAL_OK;
}