#include <stdlib.h>
#include <stdint.h>

#include "encoding.h"
#include "platform.h"
#include "board.h"
#include "prci_driver.h"

#include "eos.h"
#include "log.h"
#include "event.h"
#include "interrupt.h"

#include "uart.h"
#include "dev/egpio.h"
#include "dev/egpio_priv.h"

#include "dev/hpamp.h"

#include "i2s.h"
#include "i2s_priv.h"

#define I2S_REG_CK(o)       _REG32(I2S_CTRL_ADDR_CK, o)
#define I2S_REG_WS(o)       _REG32(I2S_CTRL_ADDR_WS, o)
#define I2S_REG_SR_SEL(o)   _REG32(I2S_CTRL_ADDR_SR_SEL, o)

#define I2S_PIN_PWM         ((1 << I2S_PIN_CK) | (1 << I2S_PIN_SR_SEL) | (1 << I2S_PIN_WS_MIC) | (1 << I2S_PIN_WS_SPK))

#define MIN(X, Y)           (((X) < (Y)) ? (X) : (Y))
#define MAX(X, Y)           (((X) > (Y)) ? (X) : (Y))

#define EOS_ABUF_IDX_MASK(IDX, SIZE)  ((IDX) & ((SIZE) - 1))

static eos_i2s_handler_t i2s_mic_handler = NULL;
static eos_i2s_handler_t i2s_spk_handler = NULL;
static uint32_t i2s_clk_period;
static uint8_t i2s_mic_volume = 0;  /* 0 - 8    */
static uint8_t i2s_spk_volume = 16; /* 0 - 16   */

EOSABuf _eos_i2s_mic_buf;
EOSABuf _eos_i2s_spk_buf;

uint32_t _eos_i2s_drvr[] = {
    0,  /* I2S_MIC_WM   */
    0,  /* I2S_SPK_WM   */
    0,  /* I2S_MIC_EVT  */
    0,  /* I2S_SPK_EVT  */
 };

#define I2S_MIC_WM      0
#define I2S_SPK_WM      1
#define I2S_MIC_EVT     2
#define I2S_SPK_EVT     3

static void _abuf_init(EOSABuf *buf, uint16_t *array, uint16_t size) {
    buf->idx_r = 0;
    buf->idx_w = 0;
    buf->size = size;
    buf->array = array;
}

static void _abuf_flush(EOSABuf *buf) {
    buf->idx_r = 0;
    buf->idx_w = 0;
}

static uint16_t _abuf_len(EOSABuf *buf) {
    return (uint16_t)(buf->idx_w - buf->idx_r);
}

static uint16_t _abuf_size(EOSABuf *buf) {
    return buf->size;
}

/* mic buffer only */
static int _mbuf_pop(uint16_t *sample) {
    if (_eos_i2s_mic_buf.idx_r == _eos_i2s_mic_buf.idx_w) return EOS_ERR_EMPTY;

    *sample = _eos_i2s_mic_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_mic_buf.idx_r, _eos_i2s_mic_buf.size)];
    _eos_i2s_mic_buf.idx_r += 1;
    return EOS_OK;
}

/* spk buffer only */
static int _sbuf_push(uint16_t sample) {
    if ((uint16_t)(_eos_i2s_spk_buf.idx_w - _eos_i2s_spk_buf.idx_r) == _eos_i2s_spk_buf.size) return EOS_ERR_FULL;

    if (_eos_i2s_spk_buf.idx_r != _eos_i2s_spk_buf.idx_w) {
        _eos_i2s_spk_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_spk_buf.idx_w - 1, _eos_i2s_spk_buf.size)] |= sample >> 15;
    }
    _eos_i2s_spk_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_spk_buf.idx_w, _eos_i2s_spk_buf.size)] = sample << 1;
    _eos_i2s_spk_buf.idx_w += 1;
    return EOS_OK;
}

static void i2s_handle_evt(unsigned char type, unsigned char *buffer, uint16_t len) {
    switch(type & ~EOS_EVT_MASK) {
        case EOS_I2S_ETYPE_MIC:
            if (i2s_mic_handler) {
                i2s_mic_handler(type);
                clear_csr(mstatus, MSTATUS_MIE);
                _eos_i2s_drvr[I2S_MIC_EVT] = 1;
                set_csr(mstatus, MSTATUS_MIE);
            }
            break;

        case EOS_I2S_ETYPE_SPK:
            if (i2s_spk_handler) {
                i2s_spk_handler(type);
                clear_csr(mstatus, MSTATUS_MIE);
                _eos_i2s_drvr[I2S_SPK_EVT] = 1;
                set_csr(mstatus, MSTATUS_MIE);
            }
            break;

        default:
            eos_evtq_bad_handler(type, buffer, len);
            break;
    }
}

static void i2s_set_cmp(void) {
    int spk_ws_offset;

    spk_ws_offset = i2s_spk_volume - 16 + i2s_mic_volume;

    /* adjust spk ws relative to mic ws */
    if (spk_ws_offset <= 0) {
        spk_ws_offset += 32;
        GPIO_REG(GPIO_OUTPUT_XOR) &= ~(1 << I2S_PIN_WS_SPK);
    } else {
        GPIO_REG(GPIO_OUTPUT_XOR) |=  (1 << I2S_PIN_WS_SPK);
    }
    I2S_REG_WS(PWM_CMP2) = spk_ws_offset * i2s_clk_period - i2s_clk_period / 2;
    I2S_REG_WS(PWM_CMP3) = (32 + spk_ws_offset) * i2s_clk_period - i2s_clk_period / 2;

    I2S_REG_SR_SEL(PWM_CMP1) = (1 + i2s_mic_volume) * i2s_clk_period + i2s_clk_period / 4;
    I2S_REG_SR_SEL(PWM_CMP2) = (17 + i2s_mic_volume) * i2s_clk_period - i2s_clk_period / 4;
}

extern void _eos_i2s_start_pwm(unsigned int);

int eos_i2s_init(void) {
    eos_evtq_set_handler(EOS_EVT_I2S, i2s_handle_evt);

    I2S_REG_CK(PWM_CFG)         = 0;
    I2S_REG_WS(PWM_CFG)         = 0;
    I2S_REG_SR_SEL(PWM_CFG)     = 0;

    clear_csr(mstatus, MSTATUS_MIE);
    GPIO_REG(GPIO_OUTPUT_VAL)   |=  (1 << I2S_PIN_SR_SEL);
    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << I2S_PIN_SR_CK);
    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << I2S_PIN_SD_OUT);
    set_csr(mstatus, MSTATUS_MIE);

    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << I2S_PIN_SR_SEL);
    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << I2S_PIN_SR_CK);

    GPIO_REG(GPIO_INPUT_EN)     |=  (1 << I2S_PIN_SD_IN);
    GPIO_REG(GPIO_INPUT_EN)     |=  (1 << I2S_PIN_SD_OUT);
    GPIO_REG(GPIO_INPUT_EN)     |=  (1 << I2S_PIN_INT);

    return EOS_OK;
}

int eos_i2s_start(uint32_t sample_rate, int mode) {
    unsigned int scale_ck;
    int rv;

    i2s_clk_period = PRCI_get_cpu_freq() / (sample_rate * 64);
    i2s_clk_period &= ~0x01;    /* clear last bit */

    /* if half of clock period does not fit in 8 bits we need to scale clock */
    scale_ck = 0;
    if ((i2s_clk_period >> 1) & ~0xFF) {
        scale_ck = 1;
        i2s_clk_period &= ~0x03;    /* clear last two bits */
    }

    I2S_REG_CK(PWM_CMP0)        = i2s_clk_period >> (1 + scale_ck); /* master clock: double bit clock frequency */
    I2S_REG_CK(PWM_CMP1)        = I2S_REG_CK(PWM_CMP0) / 2;
    I2S_REG_CK(PWM_CMP2)        = 0;
    I2S_REG_CK(PWM_CMP3)        = 0;

    I2S_REG_WS(PWM_CMP0)        = i2s_clk_period * 64 - 1;
    I2S_REG_WS(PWM_CMP1)        = i2s_clk_period * 32;

    if (mode == EOS_I2S_MODE_STEREO) {
        I2S_REG_SR_SEL(PWM_CMP0) = i2s_clk_period * 32 - 1;
    } else {
        I2S_REG_SR_SEL(PWM_CMP0) = i2s_clk_period * 64 - 1;
    }
    I2S_REG_SR_SEL(PWM_CMP3)    = 0;
    i2s_set_cmp();

    I2S_REG_CK(PWM_COUNT)       = I2S_REG_CK(PWM_CMP1);             /* master clock starts high */
    I2S_REG_WS(PWM_COUNT)       = 0;
    if (mode == EOS_I2S_MODE_MONO_R) {
        I2S_REG_SR_SEL(PWM_COUNT) = i2s_clk_period * 32;
    } else {
        I2S_REG_SR_SEL(PWM_COUNT) = 0;
    }

    eos_uart_disable();
    rv = eos_egpio_i2s_start();
    if (rv) {
        eos_i2s_stop();
        return rv;
    }

    GPIO_REG(GPIO_IOF_SEL)      |=  I2S_PIN_PWM;
    GPIO_REG(GPIO_IOF_EN)       |=  I2S_PIN_PWM;

    GPIO_REG(GPIO_FALL_IE)      |=  (1 << I2S_PIN_INT);
    clear_csr(mstatus, MSTATUS_MIE);
    GPIO_REG(GPIO_OUTPUT_VAL)   |=  (1 << I2S_PIN_SR_CK);
    set_csr(mstatus, MSTATUS_MIE);

    /* XXX should set stereo pin to 1 !!! */

    eos_intr_set_priority(I2S_IRQ_ID, IRQ_PRIORITY_I2S);
    eos_intr_enable(I2S_IRQ_ID);

    /* initialise headphones if present */
    if (!eos_egpio_get_val(EGPIO_PIN_HP_NDET)) {
        rv = eos_hpamp_init();
        if (rv) {
            eos_i2s_stop();
            return rv;
        }
    }

    clear_csr(mstatus, MSTATUS_MIE);
    _eos_i2s_start_pwm(scale_ck);
    set_csr(mstatus, MSTATUS_MIE);
    /*
    I2S_REG_CK(PWM_CFG)         = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP | scale_ck;
    I2S_REG_WS(PWM_CFG)         = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP | PWM_CFG_CMP2GANG;
    I2S_REG_SR_SEL(PWM_CFG)     = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP | PWM_CFG_CMP1GANG;
    */
}

void eos_i2s_stop(void) {
    int rv;

    I2S_REG_CK(PWM_CFG)         = 0;
    I2S_REG_WS(PWM_CFG)         = 0;
    I2S_REG_SR_SEL(PWM_CFG)     = 0;
    I2S_REG_CK(PWM_COUNT)       = 0;
    I2S_REG_WS(PWM_COUNT)       = 0;
    I2S_REG_SR_SEL(PWM_COUNT)   = 0;

    eos_intr_disable(I2S_IRQ_ID);

    GPIO_REG(GPIO_FALL_IE)      &= ~(1 << I2S_PIN_INT);
    clear_csr(mstatus, MSTATUS_MIE);
    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~(1 << I2S_PIN_SR_CK);
    set_csr(mstatus, MSTATUS_MIE);

    GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << I2S_PIN_WS_SPK);
    GPIO_REG(GPIO_IOF_EN)       &= ~I2S_PIN_PWM;
    eos_uart_enable();
    rv = eos_egpio_i2s_stop();
    if (rv) EOS_LOG(EOS_LOG_ERR, "I2S STOP: EGPIO ERR:%d\n", rv);

    _eos_i2s_drvr[I2S_MIC_EVT]  = 0;
    _eos_i2s_drvr[I2S_SPK_EVT]  = 0;
    _eos_i2s_drvr[I2S_MIC_WM]   = 0;
    _eos_i2s_drvr[I2S_SPK_WM]   = 0;
    i2s_mic_handler = NULL;
    i2s_spk_handler = NULL;
    _abuf_init(&_eos_i2s_mic_buf, NULL, 0);
    _abuf_init(&_eos_i2s_spk_buf, NULL, 0);
}

int eos_i2s_running(void) {
    return !!(GPIO_REG(GPIO_IOF_EN) & (1 << I2S_PIN_CK));
}

int eos_i2s_set_lsgain(int gain) {
    return eos_egpio_set_val(EGPIO_PIN_LSGAIN_SEL, gain);
}

void eos_i2s_mic_set_handler(eos_i2s_handler_t handler, uint16_t wm) {
    clear_csr(mstatus, MSTATUS_MIE);
    if ((i2s_mic_handler == NULL) && handler) _eos_i2s_drvr[I2S_MIC_EVT] = 1;
    if (i2s_mic_handler && (handler == NULL)) _eos_i2s_drvr[I2S_MIC_EVT] = 0;
    _eos_i2s_drvr[I2S_MIC_WM] = wm;
    i2s_mic_handler = handler;
    set_csr(mstatus, MSTATUS_MIE);
}

void eos_i2s_mic_set_buf(uint16_t *mic_arr, uint16_t mic_arr_size) {
    clear_csr(mstatus, MSTATUS_MIE);
    _abuf_init(&_eos_i2s_mic_buf, mic_arr, mic_arr_size);
    set_csr(mstatus, MSTATUS_MIE);
}

uint16_t *eos_i2s_mic_get_buf(void) {
    return _eos_i2s_mic_buf.array;
}

uint16_t eos_i2s_mic_len(void) {
    uint16_t rv;

    clear_csr(mstatus, MSTATUS_MIE);
    rv = _abuf_len(&_eos_i2s_mic_buf);
    set_csr(mstatus, MSTATUS_MIE);

    return rv;
}

uint16_t eos_i2s_mic_read(uint16_t *smpl_buf, uint16_t buf_size) {
    int i;
    uint16_t smpl_len;

    clear_csr(mstatus, MSTATUS_MIE);
    smpl_len = MIN(buf_size, _abuf_len(&_eos_i2s_mic_buf));
    set_csr(mstatus, MSTATUS_MIE);

    if (smpl_len == 0) return 0;

    for (i=0; i<smpl_len; i++) {
        smpl_buf[i] = _eos_i2s_mic_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_mic_buf.idx_r + i, _eos_i2s_mic_buf.size)];
    }

    clear_csr(mstatus, MSTATUS_MIE);
    _eos_i2s_mic_buf.idx_r += smpl_len;
    set_csr(mstatus, MSTATUS_MIE);

    return smpl_len;
}

int eos_i2s_mic_pop(uint16_t *sample) {
    int rv;

    clear_csr(mstatus, MSTATUS_MIE);
    rv = _mbuf_pop(sample);
    set_csr(mstatus, MSTATUS_MIE);

    return rv;
}

int eos_i2s_mic_get_vol(void) {
    return i2s_mic_volume;
}

void eos_i2s_mic_set_vol(int vol) {
    if ((vol < 0) || (vol > 8)) return;

    i2s_mic_volume = vol;

    clear_csr(mstatus, MSTATUS_MIE);
    i2s_set_cmp();
    set_csr(mstatus, MSTATUS_MIE);
}

void eos_i2s_spk_set_handler(eos_i2s_handler_t handler, uint16_t wm) {
    clear_csr(mstatus, MSTATUS_MIE);
    if ((i2s_spk_handler == NULL) && handler) _eos_i2s_drvr[I2S_SPK_EVT] = 1;
    if (i2s_spk_handler && (handler == NULL)) _eos_i2s_drvr[I2S_SPK_EVT] = 0;
    _eos_i2s_drvr[I2S_SPK_WM] = wm;
    i2s_spk_handler = handler;
    set_csr(mstatus, MSTATUS_MIE);
}

void eos_i2s_spk_set_buf(uint16_t *spk_arr, uint16_t spk_arr_size) {
    clear_csr(mstatus, MSTATUS_MIE);
    _abuf_init(&_eos_i2s_spk_buf, spk_arr, spk_arr_size);
    set_csr(mstatus, MSTATUS_MIE);
}

uint16_t *eos_i2s_spk_get_buf(void) {
    return _eos_i2s_spk_buf.array;
}

uint16_t eos_i2s_spk_len(void) {
    uint16_t rv;

    clear_csr(mstatus, MSTATUS_MIE);
    rv = _abuf_len(&_eos_i2s_spk_buf);
    set_csr(mstatus, MSTATUS_MIE);

    return rv;
}

uint16_t eos_i2s_spk_write(uint16_t *smpl_buf, uint16_t buf_len) {
    int i;
    uint16_t sample;
    uint16_t abuf_size, abuf_len, smpl_len;

    clear_csr(mstatus, MSTATUS_MIE);
    abuf_size = _abuf_size(&_eos_i2s_spk_buf);
    abuf_len = _abuf_len(&_eos_i2s_spk_buf);
    smpl_len = MIN(buf_len, abuf_size - abuf_len);
    if (smpl_len && abuf_len) {
        _eos_i2s_spk_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_spk_buf.idx_w - 1, _eos_i2s_spk_buf.size)] |= smpl_buf[0] >> 15;
    }
    set_csr(mstatus, MSTATUS_MIE);

    if (smpl_len == 0) return 0;

    for (i=0; i<smpl_len; i++) {
        sample = smpl_buf[i] << 1;
        if (i + 1 < smpl_len) {
            sample |= (smpl_buf[i + 1] >> 15);
        }
        _eos_i2s_spk_buf.array[EOS_ABUF_IDX_MASK(_eos_i2s_spk_buf.idx_w + i, _eos_i2s_spk_buf.size)] = sample;
    }

    clear_csr(mstatus, MSTATUS_MIE);
    _eos_i2s_spk_buf.idx_w += smpl_len;
    set_csr(mstatus, MSTATUS_MIE);

    return smpl_len;
}

int eos_i2s_spk_push(uint16_t sample) {
    int rv;

    clear_csr(mstatus, MSTATUS_MIE);
    rv = _sbuf_push(sample);
    set_csr(mstatus, MSTATUS_MIE);

    return rv;
}

int eos_i2s_spk_get_vol(void) {
    return i2s_spk_volume;
}

void eos_i2s_spk_set_vol(int vol) {
    if ((vol < 0) || (vol > 16)) return;

    i2s_spk_volume = vol;

    clear_csr(mstatus, MSTATUS_MIE);
    i2s_set_cmp();
    set_csr(mstatus, MSTATUS_MIE);
}

void eos_i2s_hp_change(int hp_det) {
    if (hp_det) {
        int rv;

        rv = eos_hpamp_init();
        if (rv) {
            EOS_LOG(EOS_LOG_ERR, "I2S HP CHANGE: PCM1770 INIT ERR:%d\n", rv);
            return;
        }
    }
}