#include <xs1.h>
#include <platform.h>
#include <xclib.h>
#include <print.h>

#include "xperipherals_i2s.h"

#ifdef XPERIPHERALS_I2S_2

extern clock XPERIPHERALS_I2S_2__CLOCK_NAME;

#if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT >= XPERIPHERALS_I2S_2__ADC_WIRE_COUNT
#define XPERIPHERALS_I2S_2__WIRE_COUNT XPERIPHERALS_I2S_2__DAC_WIRE_COUNT
#else
#define XPERIPHERALS_I2S_2__WIRE_COUNT XPERIPHERALS_I2S_2__ADC_WIRE_COUNT
#endif

#if XPERIPHERALS_I2S_2__BUS_MASTER
    on tile[XPERIPHERALS_I2S_2__TILE_NUMBER] :
    out buffered port:32 _xperipherals_i2s_2__port_wclk = XPERIPHERALS_I2S_2__WCLK_PORT;
#else
    on tile[XPERIPHERALS_I2S_2__TILE_NUMBER] :
    in port _xperipherals_i2s_2__port_wclk = XPERIPHERALS_I2S_2__WCLK_PORT;
#endif

#if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT > 0
    on tile[XPERIPHERALS_I2S_2__TILE_NUMBER] :
    out buffered port:32 _xperipherals_i2s_2__port_dac[XPERIPHERALS_I2S_2__DAC_WIRE_COUNT] = {
        #if   XPERIPHERALS_I2S_2__DAC_WIRE_COUNT == 1
        XPERIPHERALS_I2S_2__DAC0_PORT
        #elif XPERIPHERALS_I2S_2__DAC_WIRE_COUNT == 2
        XPERIPHERALS_I2S_2__DAC0_PORT, XPERIPHERALS_I2S_2__DAC1_PORT
        #elif XPERIPHERALS_I2S_2__DAC_WIRE_COUNT == 3
        XPERIPHERALS_I2S_2__DAC0_PORT, XPERIPHERALS_I2S_2__DAC1_PORT, XPERIPHERALS_I2S_2__DAC2_PORT
        #elif XPERIPHERALS_I2S_2__DAC_WIRE_COUNT == 4
        XPERIPHERALS_I2S_2__DAC0_PORT, XPERIPHERALS_I2S_2__DAC1_PORT, XPERIPHERALS_I2S_2__DAC2_PORT,
        XPERIPHERALS_I2S_2__DAC3_PORT
        #endif
};
#endif

#if XPERIPHERALS_I2S_2__ADC_WIRE_COUNT > 0
    on tile[XPERIPHERALS_I2S_2__TILE_NUMBER] :
    in buffered port:32 _xperipherals_i2s_2__port_adc[XPERIPHERALS_I2S_2__ADC_WIRE_COUNT] = {
        #if XPERIPHERALS_I2S_2__ADC_WIRE_COUNT == 1
        XPERIPHERALS_I2S_2__ADC0_PORT
        #elif XPERIPHERALS_I2S_2__ADC_WIRE_COUNT == 2
        XPERIPHERALS_I2S_2__ADC0_PORT, XPERIPHERALS_I2S_2__ADC1_PORT
        #elif XPERIPHERALS_I2S_2__ADC_WIRE_COUNT == 3
        XPERIPHERALS_I2S_2__ADC0_PORT, XPERIPHERALS_I2S_2__ADC1_PORT, XPERIPHERALS_I2S_2__ADC2_PORT
        #elif XPERIPHERALS_I2S_2__ADC_WIRE_COUNT == 4
        XPERIPHERALS_I2S_2__ADC0_PORT, XPERIPHERALS_I2S_2__ADC1_PORT, XPERIPHERALS_I2S_2__ADC2_PORT,
        XPERIPHERALS_I2S_2__ADC3_PORT
        #endif
};
#endif

static xuint _xperipherals_i2s_2__sync_word[XPERIPHERALS_I2S_2__SLOT_COUNT];

void xperipherals_i2s_2__initialize( void )
{
    for( int i = 0; i < XPERIPHERALS_I2S_2__SLOT_COUNT; ++i ) {

        #if XPERIPHERALS_I2S_2__SYNC_FORMAT == 0 // PCM

            if( i == 0 ) _xperipherals_i2s_2__sync_word[i] = 0xFFFFFFFF;
            else if( i < XPERIPHERALS_I2S_2__SLOT_COUNT-1 ) _xperipherals_i2s_2__sync_word[i] = 0;
            else _xperipherals_i2s_2__sync_word[i] = 0;

        #elif XPERIPHERALS_I2S_2__SYNC_FORMAT == 1 // I2S

            if( i == 0 ) _xperipherals_i2s_2__sync_word[i] = 0x80000000;
            else if( i < XPERIPHERALS_I2S_2__SLOT_COUNT-1 ) _xperipherals_i2s_2__sync_word[i] = 0;
            else _xperipherals_i2s_2__sync_word[i] = 0x7FFFFFFF;

        #elif XPERIPHERALS_I2S_2__SYNC_FORMAT == 2 // TDM

            if( i == 0 ) _xperipherals_i2s_2__sync_word[i] = 0;
            else if( i < XPERIPHERALS_I2S_2__SLOT_COUNT-1 ) _xperipherals_i2s_2__sync_word[i] = 0;
            else _xperipherals_i2s_2__sync_word[i] = 0x80000000;

        #endif
    }
}

void xperipherals_i2s_2__configure( clock bit_clock )
{
    #if XPERIPHERALS_I2S_2__BUS_MASTER
    configure_out_port_no_ready( _xperipherals_i2s_2__port_wclk, bit_clock, 0 );
    #else
    configure_in_port_no_ready( _xperipherals_i2s_2__port_wclk, bit_clock );
    #endif

    #if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT > 0
        for( int i = 0; i < XPERIPHERALS_I2S_2__DAC_WIRE_COUNT; ++i ) {
            configure_out_port_no_ready( _xperipherals_i2s_2__port_dac[i], bit_clock, 0 );
        }
    #endif
    #if XPERIPHERALS_I2S_2__ADC_WIRE_COUNT > 0
        for( int i = 0; i < XPERIPHERALS_I2S_2__ADC_WIRE_COUNT; ++i ) {
            configure_in_port_no_ready( _xperipherals_i2s_2__port_adc[i], bit_clock );
        }
    #endif        
}

static inline xuint _xperipherals_i2s_2__pack_dac_sample( xuint sample )
{
    #if XPERIPHERALS_I2S_2__DAC_WORD_SIZE == 32
        return bitrev( sample );
    #elif XPERIPHERALS_I2S_2__DAC_WORD_SIZE == 24
        #if XPERIPHERALS_I2S_2__DAC_WORD_JUST == 0 // left
            return bitrev( sample & 0xFFFFFF00 );
        #elif XPERIPHERALS_I2S_2__DAC_WORD_JUST == 1 // right
            return bitrev( sample >> 8 );
        #endif
    #elif XPERIPHERALS_I2S_2__DAC_WORD_SIZE == 16
        #if XPERIPHERALS_I2S_2__DAC_WORD_JUST == 0 // left
            return bitrev( sample & 0xFFFF0000 );
        #elif XPERIPHERALS_I2S_2__DAC_WORD_JUST == 1 // right
            return bitrev( sample >> 16 );
        #endif
    #endif
}

static inline xuint _xperipherals_i2s_2__unpack_adc_sample( xuint sample )
{
    #if XPERIPHERALS_I2S_2__ADC_WORD_SIZE == 32
        return bitrev( sample );
    #elif XPERIPHERALS_I2S_2__ADC_WORD_SIZE == 24
        #if XPERIPHERALS_I2S_2__ADC_WORD_JUST == 0 // left
            return bitrev( sample ) & 0xFFFFFF00;
        #elif XPERIPHERALS_I2S_2__ADC_WORD_JUST == 1 // right
            return bitrev( sample ) << 8;
        #endif
    #elif XPERIPHERALS_I2S_2__ADC_WORD_SIZE == 16
        #if XPERIPHERALS_I2S_2__ADC_WORD_JUST == 0 // left
            return bitrev( sample ) & 0xFFFF0000;
        #elif XPERIPHERALS_I2S_2__ADC_WORD_JUST == 1 // right
            return bitrev( sample ) << 16;
        #endif
    #endif
}

void xperipherals_i2s_2__synchronize( xbool bclk_is_active )
{
    // TODO/FIXME: Sync on port output completion if ports have been active already?

    clearbuf( _xperipherals_i2s_2__port_wclk );
    for( int i=0; i < XPERIPHERALS_I2S_2__DAC_WIRE_COUNT; ++i ) clearbuf(_xperipherals_i2s_2__port_dac[i]);
    for( int i=0; i < XPERIPHERALS_I2S_2__ADC_WIRE_COUNT; ++i ) clearbuf(_xperipherals_i2s_2__port_adc[i]);

    #if XPERIPHERALS_I2S_2__BUS_MASTER
    {
        if( !bclk_is_active )
        {
            _xperipherals_i2s_2__port_wclk <: 0;
            #pragma loop unroll
            #if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT > 0
            for( int i=0; i < XPERIPHERALS_I2S_2__DAC_WIRE_COUNT; ++i )
                _xperipherals_i2s_2__port_dac[i] <: 0;
            #endif
        }
        else
        {
            //p_lrclk <: 0 @ tmp;
            //tmp += 100;
            //for(int i = 0; i < I2S_WIRES_DAC; i++) p_i2s_dac[i] @ tmp <: 0;
            //p_lrclk @ tmp <: 0x7FFFFFFF;

            unsigned int time;
            _xperipherals_i2s_2__port_wclk <: 0 @ time;
            _xperipherals_i2s_2__port_wclk @ (time+32) <: 0;
        
            #if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT > 0
            #pragma loop unroll
            for( int i=0; i < XPERIPHERALS_I2S_2__DAC_WIRE_COUNT; ++i )
                _xperipherals_i2s_2__port_dac[i] @ (time+32) <: 0;
            #endif
            #if XPERIPHERALS_I2S_2__ADC_WIRE_COUNT > 0        
            #pragma loop unroll
            for( int i=0; i < XPERIPHERALS_I2S_2__ADC_WIRE_COUNT; ++i )
                asm("setpt res[%0], %1"::"r"(_xperipherals_i2s_2__port_adc[i]),"r"(time+31));
            #endif
        }
    }
    #else
    {
        //p_lrclk when pinseq(0) :> void @ tmp; tmp+=97;
        //for(int i = 0; i < I2S_WIRES_DAC; i++) p_i2s_dac[i] @ tmp <: 0;
        //for(int i = 0; i < I2S_WIRES_ADC; i++) asm("setpt res[%0], %1"::"r"(p_i2s_adc[i]),"r"(tmp+31));
        
        unsigned int time;
        #if I2S_TDM_2__SYNC_FORMAT == 0 // PCM
            _xperipherals_i2s_2__port_wclk when pinseq(0) :> void;
            _xperipherals_i2s_2__port_wclk when pinseq(1) :> void @ time; time += 63;
        #elif I2S_TDM_2__SYNC_FORMAT == 1 // I2S
            _xperipherals_i2s_2__port_wclk when pinseq(1) :> void;
            _xperipherals_i2s_2__port_wclk when pinseq(0) :> void @ time; time += 64;
        #elif I2S_TDM_2__SYNC_FORMAT == 2 // TDM
            _xperipherals_i2s_2__port_wclk when pinseq(1) :> void;
            _xperipherals_i2s_2__port_wclk when pinseq(0) :> void @ time; time += 63;
        #endif

        #if XPERIPHERALS_I2S_2__DAC_WIRE_COUNT > 0
        #pragma loop unroll
        for( int i=0; i < XPERIPHERALS_I2S_2__DAC_WIRE_COUNT; ++i )
            _xperipherals_i2s_2__port_dac[i] @ (time) <: 0;
        #endif
        #if XPERIPHERALS_I2S_2__ADC_WIRE_COUNT > 0
        #pragma loop unroll
        for( int i=0; i < XPERIPHERALS_I2S_2__ADC_WIRE_COUNT; ++i )
            asm("setpt res[%0], %1"::"r"(_xperipherals_i2s_2__port_adc[i]),"r"(time+31));
        #endif
    }
    #endif
}

void xperipherals_i2s_2__transfer_beg( xuint slot )
{
    #if XPERIPHERALS_I2S_2__BUS_MASTER
        _xperipherals_i2s_2__port_wclk <: _xperipherals_i2s_2__sync_word[slot];
    #else
    #endif
}

void xperipherals_i2s_2__transfer_dac( xuint wire, xuint sample )
{
    _xperipherals_i2s_2__port_dac[wire] <: _xperipherals_i2s_2__pack_dac_sample( sample );
}
        
void xperipherals_i2s_2__transfer_adc( xuint wire, xuint& sample )
{
    _xperipherals_i2s_2__port_adc[wire] :> sample;
    sample = _xperipherals_i2s_2__unpack_adc_sample( sample );
}

#endif