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

#include "xperipherals_i2s.h"

#ifdef XPERIPHERALS_I2S_1

extern clock XPERIPHERALS_I2S_1__CLOCK_NAME;

#if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT >= XPERIPHERALS_I2S_1__ADC_WIRE_COUNT
#define XPERIPHERALS_I2S_1__WIRE_COUNT XPERIPHERALS_I2S_1__DAC_WIRE_COUNT
#else
#define XPERIPHERALS_I2S_1__WIRE_COUNT XPERIPHERALS_I2S_1__ADC_WIRE_COUNT
#endif

#if XPERIPHERALS_I2S_1__BUS_MASTER
    //on tile[XPERIPHERALS_I2S_1__TILE_NUMBER] :
    out buffered port:32 _xperipherals_i2s_1__port_wclk = XPERIPHERALS_I2S_1__WCLK_PORT;
#else
    //on tile[XPERIPHERALS_I2S_1__TILE_NUMBER] :
    in port _xperipherals_i2s_1__port_wclk = XPERIPHERALS_I2S_1__WCLK_PORT;
#endif

#if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT > 0
    //on tile[XPERIPHERALS_I2S_1__TILE_NUMBER] :
    out buffered port:32 _xperipherals_i2s_1__port_dac[XPERIPHERALS_I2S_1__DAC_WIRE_COUNT] = {
        #if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT == 1
            XPERIPHERALS_I2S_1__DAC0_PORT
        #elif XPERIPHERALS_I2S_1__DAC_WIRE_COUNT == 2
            XPERIPHERALS_I2S_1__DAC0_PORT, XPERIPHERALS_I2S_1__DAC1_PORT
        #elif XPERIPHERALS_I2S_1__DAC_WIRE_COUNT == 3
            XPERIPHERALS_I2S_1__DAC0_PORT, XPERIPHERALS_I2S_1__DAC1_PORT, XPERIPHERALS_I2S_1__DAC2_PORT
        #elif XPERIPHERALS_I2S_1__DAC_WIRE_COUNT == 4
            XPERIPHERALS_I2S_1__DAC0_PORT, XPERIPHERALS_I2S_1__DAC1_PORT, XPERIPHERALS_I2S_1__DAC2_PORT,
            XPERIPHERALS_I2S_1__DAC3_PORT
        #endif
    };
#endif

#if XPERIPHERALS_I2S_1__ADC_WIRE_COUNT > 0
    //on tile[XPERIPHERALS_I2S_1__TILE_NUMBER] :
    in buffered port:32 _xperipherals_i2s_1__port_adc[XPERIPHERALS_I2S_1__ADC_WIRE_COUNT] = {
        #if XPERIPHERALS_I2S_1__ADC_WIRE_COUNT == 1
            XPERIPHERALS_I2S_1__ADC0_PORT
        #elif XPERIPHERALS_I2S_1__ADC_WIRE_COUNT == 2
            XPERIPHERALS_I2S_1__ADC0_PORT, XPERIPHERALS_I2S_1__ADC1_PORT
        #elif XPERIPHERALS_I2S_1__ADC_WIRE_COUNT == 3
            XPERIPHERALS_I2S_1__ADC0_PORT, XPERIPHERALS_I2S_1__ADC1_PORT, XPERIPHERALS_I2S_1__ADC2_PORT
        #elif XPERIPHERALS_I2S_1__ADC_WIRE_COUNT == 4
            XPERIPHERALS_I2S_1__ADC0_PORT, XPERIPHERALS_I2S_1__ADC1_PORT, XPERIPHERALS_I2S_1__ADC2_PORT,
            XPERIPHERALS_I2S_1__ADC3_PORT
        #endif
    };
#endif

xuint _xperipherals_i2s_1__sync_word[XPERIPHERALS_I2S_1__SLOT_COUNT];
xuint _xperipherals_i2s_1__slot_index;
xuint _xperipherals_i2s_1__dac_index;
xuint _xperipherals_i2s_1__adc_index;

void xperipherals_i2s_1__initialize( void )
{
    for( int i = 0; i < XPERIPHERALS_I2S_1__SLOT_COUNT; ++i ) {

        #if XPERIPHERALS_I2S_1__SYNC_FORMAT == 0 // PCM

            if( i == 0 ) _xperipherals_i2s_1__sync_word[i] = 0xFFFFFFFF;
            else if( i < XPERIPHERALS_I2S_1__SLOT_COUNT-1 ) _xperipherals_i2s_1__sync_word[i] = 0;
            else _xperipherals_i2s_1__sync_word[i] = 0;

        #elif XPERIPHERALS_I2S_1__SYNC_FORMAT == 1 // I2S

            if( i == 0 ) _xperipherals_i2s_1__sync_word[i] = 0x80000000;
            else if( i < XPERIPHERALS_I2S_1__SLOT_COUNT-1 ) _xperipherals_i2s_1__sync_word[i] = 0;
            else _xperipherals_i2s_1__sync_word[i] = 0x7FFFFFFF;

        #elif XPERIPHERALS_I2S_1__SYNC_FORMAT == 2 // TDM

            if( i == 0 ) _xperipherals_i2s_1__sync_word[i] = 0;
            else if( i < XPERIPHERALS_I2S_1__SLOT_COUNT-1 ) _xperipherals_i2s_1__sync_word[i] = 0;
            else _xperipherals_i2s_1__sync_word[i] = 0x80000000;

        #endif
    }
    _xperipherals_i2s_1__slot_index = 0;
    _xperipherals_i2s_1__dac_index = 0;
    _xperipherals_i2s_1__adc_index = 0;
}

void xperipherals_i2s_1__configure( clock bit_clock )
{
    #if XPERIPHERALS_I2S_1__BUS_MASTER
        configure_out_port_no_ready( _xperipherals_i2s_1__port_wclk, bit_clock, 0 );
    #else
        configure_in_port_no_ready( _xperipherals_i2s_1__port_wclk, bit_clock );
    #endif

    #if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT > 0
        for( int i = 0; i < XPERIPHERALS_I2S_1__DAC_WIRE_COUNT; ++i ) {
            configure_out_port_no_ready( _xperipherals_i2s_1__port_dac[i], bit_clock, 0 );
        }
    #endif
    #if XPERIPHERALS_I2S_1__ADC_WIRE_COUNT > 0
        for( int i = 0; i < XPERIPHERALS_I2S_1__ADC_WIRE_COUNT; ++i ) {
            configure_in_port_no_ready( _xperipherals_i2s_1__port_adc[i], bit_clock );
        }
    #endif
    _xperipherals_i2s_1__slot_index = 0;
    _xperipherals_i2s_1__dac_index = 0;
    _xperipherals_i2s_1__adc_index = 0;
}

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

    //if( bclk_is_active )
    //{
    //    stop_clock( clk_audio_bclk );
    //    configure_clock_src( clk_audio_bclk, p_bclk );
    //    start_clock( clk_audio_bclk );
    //}

    clearbuf( _xperipherals_i2s_1__port_wclk );
    for( int i=0; i < XPERIPHERALS_I2S_1__DAC_WIRE_COUNT; ++i ) clearbuf(_xperipherals_i2s_1__port_dac[i]);
    for( int i=0; i < XPERIPHERALS_I2S_1__ADC_WIRE_COUNT; ++i ) clearbuf(_xperipherals_i2s_1__port_adc[i]);

    #if XPERIPHERALS_I2S_1__BUS_MASTER
    {
        if( !bclk_is_active )
        {
            _xperipherals_i2s_1__port_wclk <: 0;
            #pragma loop unroll
            #if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT > 0
            for( int i=0; i < XPERIPHERALS_I2S_1__DAC_WIRE_COUNT; ++i )
                _xperipherals_i2s_1__port_dac[i] <: 0;
            #endif
        }
        else
        {
            unsigned int time;
            _xperipherals_i2s_1__port_wclk <: 0 @ time;
            _xperipherals_i2s_1__port_wclk @ (time+32) <: 0;        
            #if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT > 0
            #pragma loop unroll
            for( int i=0; i < XPERIPHERALS_I2S_1__DAC_WIRE_COUNT; ++i )
                _xperipherals_i2s_1__port_dac[i] @ (time+32) <: 0;
            #endif
            #if XPERIPHERALS_I2S_1__ADC_WIRE_COUNT > 0        
            #pragma loop unroll
            for( int i=0; i < XPERIPHERALS_I2S_1__ADC_WIRE_COUNT; ++i )
                //asm("setpt res[%0], %1"::"r"(_xperipherals_i2s_1__port_adc[i]),"r"(time+31));
                asm("setpt res[%0], %1"::"r"(_xperipherals_i2s_1__port_adc[i]),"r"(time+31));
            #endif
        }
    }
    #else
    {
        unsigned int time;
        #if I2S_TDM_2__SYNC_FORMAT == 0 // PCM
            _xperipherals_i2s_1__port_wclk when pinseq(0) :> void;
            _xperipherals_i2s_1__port_wclk when pinseq(1) :> void @ time; time += 63;
        #elif I2S_TDM_2__SYNC_FORMAT == 1 // I2S
            _xperipherals_i2s_1__port_wclk when pinseq(1) :> void;
            _xperipherals_i2s_1__port_wclk when pinseq(0) :> void @ time; time += 64;
        #elif I2S_TDM_2__SYNC_FORMAT == 2 // TDM
            _xperipherals_i2s_1__port_wclk when pinseq(1) :> void;
            _xperipherals_i2s_1__port_wclk when pinseq(0) :> void @ time; time += 63;
        #endif

        #if XPERIPHERALS_I2S_1__DAC_WIRE_COUNT > 0
        #pragma loop unroll
        for( int i=0; i < XPERIPHERALS_I2S_1__DAC_WIRE_COUNT; ++i )
            _xperipherals_i2s_1__port_dac[i] @ (time) <: 0;
        #endif
        #if XPERIPHERALS_I2S_1__ADC_WIRE_COUNT > 0
        #pragma loop unroll
        for( int i=0; i < XPERIPHERALS_I2S_1__ADC_WIRE_COUNT; ++i )
            asm("setpt res[%0], %1"::"r"(_xperipherals_i2s_1__port_adc[i]),"r"(time+31));
        #endif

        /*
        configure_in_port_no_ready( p_lrclk, clk_audio_bclk );
        #if I2S_TDM__DAC_WIRE_COUNT > 0
            for( int i = 0; i < I2S_TDM__DAC_WIRE_COUNT; ++i )
                configure_out_port_no_ready( p_i2s_dac[i], clk_audio_bclk, 0 );
        #endif
        #if I2S_TDM__ADC_WIRE_COUNT > 0
            for( int i = 0; i < I2S_TDM__ADC_WIRE_COUNT; ++i )
                configure_in_port_no_ready( p_i2s_adc[i], clk_audio_bclk );
        #endif

        stop_clock( clk_audio_bclk );
        configure_clock_src( clk_audio_bclk, p_bclk );
        start_clock( clk_audio_bclk );

        //p_bclk <: 0x80000000;
        //sync(p_bclk);
        clearbuf( p_bclk );
        clearbuf( p_lrclk );
        for( int i = 0; i < I2S_TDM__DAC_WIRE_COUNT; ++i ) clearbuf( p_i2s_dac[i] );
        for( int i = 0; i < I2S_TDM__ADC_WIRE_COUNT; ++i ) clearbuf( p_i2s_adc[i] );

        int time;

        #if I2S_TDM__SYNC_FORMAT == 0 // PCM
            p_lrclk when pinseq(0) :> void;
            p_lrclk when pinseq(1) :> void @ time; time += 63;
        #elif I2S_TDM__SYNC_FORMAT == 1 // I2S
            p_lrclk when pinseq(1) :> void;
            p_lrclk when pinseq(0) :> void @ time; time += 64;
        #elif I2S_TDM__SYNC_FORMAT == 2 // TDM
            p_lrclk when pinseq(1) :> void;
            p_lrclk when pinseq(0) :> void @ time; time += 63;
        #endif
        for( int i=0; i < I2S_TDM__DAC_WIRE_COUNT; ++i )
            p_i2s_dac[i] @ (time+1) <: 0xFFFFFFFF;
        for( int i=0; i < I2S_TDM__ADC_WIRE_COUNT; ++i )
            asm("setpt res[%0], %1"::"r"(p_i2s_adc[i]),"r"(time));
        */
    }
    #endif
}

void xperipherals_i2s_1__transfer_beg( void )
{
    #if XPERIPHERALS_I2S_1__BUS_MASTER
    _xperipherals_i2s_1__port_wclk <: _xperipherals_i2s_1__sync_word[_xperipherals_i2s_1__slot_index];
    #endif
    _xperipherals_i2s_1__dac_index = _xperipherals_i2s_1__adc_index = 0;
}

void xperipherals_i2s_1__transfer_dac( xsint dac_sample )
{
    xuint sample = dac_sample;
    #if XPERIPHERALS_I2S_1__DAC_WORD_SIZE == 32
        sample = bitrev( sample );
    #elif XPERIPHERALS_I2S_1__DAC_WORD_SIZE == 24
        #if XPERIPHERALS_I2S_1__DAC_WORD_JUST == 0 // left
            sample = bitrev( sample & 0xFFFFFF00 );
        #elif XPERIPHERALS_I2S_1__DAC_WORD_JUST == 1 // right
            sample = bitrev( sample >> 8 );
        #endif
    #elif XPERIPHERALS_I2S_1__DAC_WORD_SIZE == 16
        #if XPERIPHERALS_I2S_1__DAC_WORD_JUST == 0 // left
            sample = bitrev( sample & 0xFFFF0000 );
        #elif XPERIPHERALS_I2S_1__DAC_WORD_JUST == 1 // right
            sample = bitrev( sample >> 16 );
        #endif
    #endif
    _xperipherals_i2s_1__port_dac[_xperipherals_i2s_1__dac_index++] <: sample;
}

void xperipherals_i2s_1__transfer_adc( xsint& adc_sample )
{
    xuint sample;
    _xperipherals_i2s_1__port_adc[_xperipherals_i2s_1__adc_index++] :> sample;
    #if XPERIPHERALS_I2S_1__ADC_WORD_SIZE == 32
        sample = bitrev( sample );
    #elif XPERIPHERALS_I2S_1__ADC_WORD_SIZE == 24
        #if XPERIPHERALS_I2S_1__ADC_WORD_JUST == 0 // left
            sample = bitrev( sample ) & 0xFFFFFF00;
        #elif XPERIPHERALS_I2S_1__ADC_WORD_JUST == 1 // right
            sample = bitrev( sample ) << 8;
        #endif
    #elif XPERIPHERALS_I2S_1__ADC_WORD_SIZE == 16
        #if XPERIPHERALS_I2S_1__ADC_WORD_JUST == 0 // left
            sample = bitrev( sample ) & 0xFFFF0000;
        #elif XPERIPHERALS_I2S_1__ADC_WORD_JUST == 1 // right
            sample = bitrev( sample ) << 16;
        #endif
    #endif
    adc_sample = sample;
}

void xperipherals_i2s_1__transfer_end( void )
{
    if( ++_xperipherals_i2s_1__slot_index == XPERIPHERALS_I2S_1__SLOT_COUNT ) {
        _xperipherals_i2s_1__slot_index = 0;
    }
}

#endif