#include "APU.h"
#include <SDL2/SDL.h>
#include "audio_viewer.h"

namespace APU
{
    #define SAMPLING_FREQ 44100
    #define AUDIO_BUFFER_SIZE 8192
    const float cycles_per_sample = 4194304.0f / SAMPLING_FREQ;

    SDL_AudioDeviceID sdlAudioDevice;
    uint8_t sound_buffer[AUDIO_BUFFER_SIZE];
    uint16_t sound_pos=0;
    uint16_t sound_start=0;
    float t_sound = 0.0f;
    uint32_t samples_generated=0;
    uint32_t samples_time =0;
    uint32_t samples_t=0;

    #define CH1 channels[0]
    #define CH2 channels[1]
    #define CH3 channels[2]
    #define CH4 channels[3]

    #define apu_enabled (NR52&0x80)
    #define DAC1_enabled ((NR12&0xf8)!=0)
    #define DAC2_enabled ((NR22&0xf8)!=0)
    #define DAC3_enabled ((NR30&0x80)!=0)

    uint8_t duty_cycles[4][8] = {
        {1, 1, 1, 1, 1, 1, 1, 0},
        {1, 1, 1, 1, 1, 1, 0, 0},
        {1, 1, 1, 1, 0, 0, 0, 0},
        {1, 1, 0, 0, 0, 0, 0, 0},
    };
    struct channel_t
    {
        bool enabled = false;
        uint8_t length_timer = 0;
        bool length_enable = false;
        uint16_t period_divider = 0;
        uint8_t duty_cycle=0;
        uint8_t duty_step = 0;
        uint8_t volume = 0;
        uint8_t envelope_sweep_timer=0;
    };

    channel_t channels[4];
    uint8_t DIVAPU = 0;

    uint8_t NR10 = 0; // 0xff10 - Sound channel 1 sweep
    uint8_t NR11 = 0; // 0xff11 - Sound channel 1 length timer & duty cycle
    uint8_t NR12 = 0; // 0xff12 - Sound channel 1 volume & envelope
    uint8_t NR13 = 0; // 0xff13 - Sound channel 1 period low
    uint8_t NR14 = 0; // 0xff14 - Sound channel 1 period high & control
    // 0xff15 - Not used?
    uint8_t NR21 = 0; // 0xff16 - Sound channel 2 length timer & duty cycle
    uint8_t NR22 = 0; // 0xff17 - Sound channel 2 volume & envelope
    uint8_t NR23 = 0; // 0xff18 - Sound channel 2 period low
    uint8_t NR24 = 0; // 0xff19 - Sound channel 2 period high & control

    uint8_t NR30 = 0; // 0xff1a - Sound channel 3 DAC enable
    uint8_t NR31 = 0; // 0xff1b - Sound channel 3 length timer
    uint8_t NR32 = 0; // 0xff1c - Sound channel 3 output level
    uint8_t NR33 = 0; // 0xff1d - Sound channel 3 period low
    uint8_t NR34 = 0; // 0xff1e - Sound channel 3 period high & control
    // 0xff1f - Not used?
    uint8_t NR41 = 0; // 0xff20 - Sound channel 4 length timer
    uint8_t NR42 = 0; // 0xff21 - Sound channel 4 volume & envelope
    uint8_t NR43 = 0; // 0xff22 - Sound channel 4 frequency & randomness
    uint8_t NR44 = 0; // 0xff23 - Sound channel 4 control
    uint8_t NR50 = 0; // 0xff24 - Master volume & VIN panning
    uint8_t NR51 = 0; // 0xff25 - Sound panning
    uint8_t NR52 = 0; // 0xff26 - Sound on/off
    // 0xff27-0xff2f - Not used?
    uint8_t WaveRAM[16]; // 0xff30-0xff3f


    void audioCallback(void * userdata, uint8_t * stream, int len)
    {
        for (int i=0;i<len;++i)
        {
            stream[i] = sound_buffer[(sound_start++)&(AUDIO_BUFFER_SIZE-1)];
        }
    }

    void silence()
    {
        SDL_PauseAudioDevice(sdlAudioDevice, 1);
    }

    void resume()
    {
        SDL_PauseAudioDevice(sdlAudioDevice, 0);
    }

    void init()
    {
        SDL_AudioSpec audioSpec{SAMPLING_FREQ, AUDIO_U8, 1, 0, AUDIO_BUFFER_SIZE>>2, 0, 0, &audioCallback, NULL};
        sdlAudioDevice = SDL_OpenAudioDevice(NULL, 0, &audioSpec, NULL, 0);
        resume();
        //samples_time=SDL_GetTicks();
        audio_viewer::init();
    }

    void reset()
    {
        NR10 = 0;
        NR11 = 0;
        NR12 = 0;
        NR13 = 0;
        NR14 = 0;
        NR21 = 0;
        NR22 = 0;
        NR23 = 0;
        NR24 = 0;
        NR30 = 0;
        NR31 = 0;
        NR32 = 0;
        NR33 = 0;
        NR34 = 0;
        NR41 = 0;
        NR42 = 0;
        NR43 = 0;
        NR44 = 0;
        NR50 = 0;
        NR51 = 0;
        NR52 = 0;
        CH1.duty_cycle = 0;
    }

    void triggerCH1()
    {
        CH1.enabled = true;
        CH1.length_timer=NR11&0x3f;
        CH1.period_divider = NR13 | ((NR14 &0x7)<<8);
        // envelope timer is reset
        CH1.volume = NR12>>4;
        // sweep does several things (check documentation)
    }

    void triggerCH2()
    {
        CH2.enabled = true;
        CH2.length_timer=NR21&0x3f;
        CH2.period_divider = NR23 | ((NR24 &0x7)<<8);
        // envelope timer is reset
        CH2.volume = NR22>>4;
        // sweep does several things (check documentation)
    }

    void triggerCH3()
    {
        CH3.enabled = true;
        CH3.length_timer=NR31;
        CH3.period_divider = NR33 | ((NR34 &0x7)<<8);
        // envelope timer is reset
        CH3.volume = (NR32>>5)&0x3;
        // sweep does several things (check documentation)
    }

    uint8_t readRegister(uint16_t address)
    {
        switch(address)
        {
            case 0xff10: return NR10; break;
            case 0xff11: return NR11 & 0xc0; break;
            case 0xff12: return NR12; break;
            case 0xff13: return 0x00; break;
            case 0xff14: return NR14 & 0x40; break;

            case 0xff16: return NR21 & 0xc0; break;
            case 0xff17: return NR22; break;
            case 0xff18: return 0x00; break;
            case 0xff19: return NR24 & 0x40; break;

            case 0xff1a: return NR30; break;
            case 0xff1b: return 0x00; break;
            case 0xff1c: return NR32; break;
            case 0xff1d: return 0x00; break;
            case 0xff1e: return NR34 & 0x40; break;

            case 0xff24: return NR50; break;
            case 0xff25: return NR51; break;
            case 0xff26: return NR52; break;

            case 0xff30: return WaveRAM[0]; break;
            case 0xff31: return WaveRAM[1]; break;
            case 0xff32: return WaveRAM[2]; break;
            case 0xff33: return WaveRAM[3]; break;
            case 0xff34: return WaveRAM[4]; break;
            case 0xff35: return WaveRAM[5]; break;
            case 0xff36: return WaveRAM[6]; break;
            case 0xff37: return WaveRAM[7]; break;
            case 0xff38: return WaveRAM[8]; break;
            case 0xff39: return WaveRAM[9]; break;
            case 0xff3a: return WaveRAM[10]; break;
            case 0xff3b: return WaveRAM[11]; break;
            case 0xff3c: return WaveRAM[12]; break;
            case 0xff3d: return WaveRAM[13]; break;
            case 0xff3e: return WaveRAM[14]; break;
            case 0xff3f: return WaveRAM[15]; break;
        }
        return 0x00;
    }

    void writeRegister(uint16_t address, uint8_t value)
    {
        if (!apu_enabled && (address!=0xff26)) return;

        switch(address)
        {
            case 0xff10: NR10 = value; break;
            case 0xff11: NR11 = value; CH1.length_timer=NR11&0x3f; CH1.duty_cycle = NR11>>6; break;
            case 0xff12: NR12 = value; break;
            case 0xff13: NR13 = value; break;
            case 0xff14: NR14 = value; CH1.length_enable=(value&0x40); if (value&0x80) triggerCH1(); break;

            case 0xff16: NR21 = value; CH2.length_timer=NR21&0x3f; CH2.duty_cycle = NR21>>6; break;
            case 0xff17: NR22 = value; break;
            case 0xff18: NR23 = value; break;
            case 0xff19: NR24 = value; CH2.length_enable=(value&0x40); if (value&0x80) triggerCH2(); break;

            case 0xff1a: NR30 = value; break;
            case 0xff1b: NR31 = value; CH3.length_timer=NR31; break;
            case 0xff1c: NR32 = value; break;
            case 0xff1d: NR33 = value; break;
            case 0xff1e: NR34 = value; CH3.length_enable=(value&0x40); if (value&0x80) triggerCH3(); break;

            case 0xff24: NR50 = value; break;
            case 0xff25: NR51 = value; break;
            case 0xff26: if (value&0x80) reset(); NR52 = (value&0x80) | (NR52 & 0x0f); break;

            case 0xff30: WaveRAM[0] = value; break;
            case 0xff31: WaveRAM[1] = value; break;
            case 0xff32: WaveRAM[2] = value; break;
            case 0xff33: WaveRAM[3] = value; break;
            case 0xff34: WaveRAM[4] = value; break;
            case 0xff35: WaveRAM[5] = value; break;
            case 0xff36: WaveRAM[6] = value; break;
            case 0xff37: WaveRAM[7] = value; break;
            case 0xff38: WaveRAM[8] = value; break;
            case 0xff39: WaveRAM[9] = value; break;
            case 0xff3a: WaveRAM[10] = value; break;
            case 0xff3b: WaveRAM[11] = value; break;
            case 0xff3c: WaveRAM[12] = value; break;
            case 0xff3d: WaveRAM[13] = value; break;
            case 0xff3e: WaveRAM[14] = value; break;
            case 0xff3f: WaveRAM[15] = value; break;
        }
    }

    uint8_t DIVAPU_envelope_sweep = 0;
    uint8_t DIVAPU_length = 0;
    uint8_t DIVAPU_CH1_freq_sweep = 0;

    void incDIVAPU()
    {
        DIVAPU++;
        DIVAPU_length++;
        if (DIVAPU_length==2) {
            DIVAPU_length=0;
            if (CH1.enabled && CH1.length_enable) {
                if (CH1.length_timer==63) {
                    CH1.enabled = false;
                    CH1.length_timer=0;
                } else {
                    CH1.length_timer++;
                }
            }
            if (CH2.enabled && CH2.length_enable) {
                if (CH2.length_timer==63) {
                    CH2.enabled = false;
                    CH2.length_timer=0;
                } else {
                    CH2.length_timer++;
                }
            }
            if (CH3.enabled && CH3.length_enable) {
                if (CH3.length_timer==255) {
                    CH3.enabled = false;
                    CH3.length_timer = 0;
                } else {
                    CH3.length_timer++;
                }
            }
        }
        DIVAPU_CH1_freq_sweep++;
        if (DIVAPU_CH1_freq_sweep==4) {
            DIVAPU_CH1_freq_sweep=0;
            // Do the freq sweep thing
        }
        DIVAPU_envelope_sweep++;
        if (DIVAPU_envelope_sweep==8) {
            DIVAPU_envelope_sweep=0;
            if ( CH1.enabled && (NR12&0x7) ) {   // If sweep pace != 0, envelope sweep is enabled
                CH1.envelope_sweep_timer++;
                if ( CH1.envelope_sweep_timer == (NR12&0x07) ) {    // if timer == envelope sweep, increase or decrease volume
                    CH1.envelope_sweep_timer=0;
                    if (NR12&0x8) {    // bit set increases, reset decreases
                        if (CH1.volume<0x0f) CH1.volume++;
                    } else {
                        if (CH1.volume>0) CH1.volume--;
                    }
                }
            }
            if ( CH2.enabled && (NR22&0x7) ) {   // If sweep pace != 0, envelope sweep is enabled
                CH2.envelope_sweep_timer++;
                if ( CH2.envelope_sweep_timer == (NR22&0x07) ) {    // if timer == envelope sweep, increase or decrease volume
                    CH2.envelope_sweep_timer=0;
                    if (NR22&0x8) {    // bit set increases, reset decreases
                        if (CH2.volume<0x0f) CH2.volume++;
                    } else {
                        if (CH2.volume>0) CH2.volume--;
                    }
                }
            }

            // Do the envelope sweep thing
        }
    }

    uint32_t dots = 0;
    uint32_t dotsCH3 = 0;
    void update(uint32_t dt)
    {
        dots += dt;
        while (dots>=4) {
            dots -= 4;
            CH1.period_divider++;
            if (CH1.period_divider==2048) {
                CH1.period_divider = NR13 | ((NR14 &0x7)<<8);
                CH1.duty_step++;
                if (CH1.duty_step==8) CH1.duty_step=0;
            }
            CH2.period_divider++;
            if (CH2.period_divider==2048) {
                CH2.period_divider = NR23 | ((NR24 &0x7)<<8);
                CH2.duty_step++;
                if (CH2.duty_step==8) CH2.duty_step=0;
            }
        }
        dotsCH3 += dt;
        while (dotsCH3>=2) {
            dotsCH3 -= 2;
            CH3.period_divider++;
            if (CH3.period_divider==2048) {
                CH3.period_divider = NR33 | ((NR34 &0x7)<<8);
                CH3.duty_step++;
                if (CH3.duty_step==32) CH3.duty_step=0;
            }
        }

        t_sound += dt;
        samples_t += dt;
        if (t_sound>=cycles_per_sample) {
            t_sound-=cycles_per_sample;

            uint16_t sampleCH1 = 0;
            if (apu_enabled && DAC1_enabled) sampleCH1 = (duty_cycles[CH1.duty_cycle][CH1.duty_step]*CH1.volume)<<2;

            uint16_t sampleCH2 = 0;
            if (apu_enabled && DAC2_enabled) sampleCH2 = (duty_cycles[CH2.duty_cycle][CH2.duty_step]*CH2.volume)<<2;

            uint16_t sampleCH3 = 0;
            if (apu_enabled && DAC3_enabled) {
                uint8_t step = CH3.duty_step>>1;
                uint8_t actual_sample = (CH3.duty_step&1 ? WaveRAM[step]&0x0f : WaveRAM[step]>>4)<<2;
                sampleCH3 = CH3.volume==0 ? 0 : CH3.volume==1 ? actual_sample : CH3.volume==2 ? actual_sample>>1 : actual_sample>>2;
            }

            uint8_t sample = (sampleCH1+sampleCH2+sampleCH3)&0xff;

            sound_buffer[(sound_pos++)&(AUDIO_BUFFER_SIZE-1)] = sample;
            audio_viewer::addsample(sample);
            //if (ear) last_1 = sound_pos;
        }
     }
}