#include <audio.h>

#include <emu.h>
#include <ppu.h>

#include <portaudio.h>
#include <pa_ringbuffer.h>
#include <math.h>

#define SAMPLE_RATE 44100
#define FRAMES_PER_BUFFER 64
#define TIME_PER_SAMPLE 1.0f / (SAMPLE_RATE/1000.0f/1000.0f)
#define TIME_PER_AUDIO_TICK 1.0f / (1048576.0f/1000.0f/1000.0f)
#define AUDIO_TICKS_PER_SAMPLE 1048576.0f / SAMPLE_RATE
#define TIME_PER_WAVE_TICK 1.0f / (2097152.0f/1000.0f/1000.0f)
#define LFSR_BASE_CLOCK 262144.0f

static audio_context ctx;

audio_context *audio_get_context() {
    return &ctx;
}

static float audio_time = 0;
static float wave_time = 0;
static float lfsr_timer = 0;
static float lfsr_clock = LFSR_BASE_CLOCK;
static float lfsr_clock_buffer = LFSR_BASE_CLOCK;

static float left_cap = 0.0f;
static float right_cap = 0.0f;

static int history_timer = 0;
const int history_interval = 5;
static int lfsr_clocks = 0;
static double write_index = 0;

const u8 square_sample_00[8] = {
    0x0,
    0xF,
    0xF,
    0xF,
    0xF,
    0xF,
    0xF,
    0xF
};

const u8 square_sample_01[8] = {
    0x0,
    0x0,
    0xF,
    0xF,
    0xF,
    0xF,
    0xF,
    0xF
};

const u8 square_sample_10[8] = {
    0x0,
    0x0,
    0x0,
    0x0,
    0xF,
    0xF,
    0xF,
    0xF
};

const u8 square_sample_11[8] = {
    0x0,
    0x0,
    0x0,
    0x0,
    0x0,
    0x0,
    0xF,
    0xF
};

const u8 *square_sample[4] = {
    square_sample_00,
    square_sample_01,
    square_sample_10,
    square_sample_11
};

static int audio_callback(const void* input_uffer, void *output_buffer,
                            unsigned long framesPerBuffer,
                            const PaStreamCallbackTimeInfo *time_info,
                            PaStreamCallbackFlags status_flags,
                            void *userData ) {
    float *out = (float *)output_buffer;
    for(int i = 0; i < framesPerBuffer; i++) {
        write_index = write_index + AUDIO_TICKS_PER_SAMPLE;
        ctx.sq1_read_index = (int)write_index;
        if(ctx.sq1_read_index > ctx.sq1_write_index) {
            ctx.sq1_read_index = ctx.sq1_write_index-1;
            write_index = ctx.sq1_read_index;
        }
        if(ppu_get_context()->paused) {
            *out++ = 0;
            *out++ = 0;
            continue;
        }
        float left = 0;
        float right = 0;
        audio_time += TIME_PER_SAMPLE;
        wave_time += TIME_PER_SAMPLE;
        for(;audio_time >= TIME_PER_AUDIO_TICK;audio_time -= TIME_PER_AUDIO_TICK) {
            //ctx.sq1_period_timer++;
            ctx.sq2_period_timer++;

            //if(ctx.sq1_period_timer >= 0x800) {
            //    ctx.sq1_period_timer = ctx.sq1_period_reset;
            //    ctx.sq1_sample = (ctx.sq1_sample + 1) % 8;
            //}
            if(ctx.sq2_period_timer >= 0x800) {
                ctx.sq2_period_timer = ctx.sq2_period_reset;
                ctx.sq2_sample = (ctx.sq2_sample + 1) % 8;
            }
        }
        for(;wave_time >= TIME_PER_WAVE_TICK;wave_time -= TIME_PER_WAVE_TICK) {
            ctx.ch3_period_timer++;
            if(ctx.ch3_period_timer >= 0x800) {
                ctx.ch3_period_timer = ctx.ch3_period_reset;
                ctx.ch3_sample = (ctx.ch3_sample + 1) % 32;
                if((ctx.ch3_sample & 0b1) == 0b1) {
                    ctx.ch3_last_sample = ctx.wave_ram[ctx.ch3_sample >> 1] & 0xF;
                } else {
                    ctx.ch3_last_sample = ctx.wave_ram[ctx.ch3_sample >> 1] >> 4;
                }
            }
        }
        lfsr_timer += TIME_PER_SAMPLE;
        for(;lfsr_timer >= lfsr_clock;lfsr_timer -= lfsr_clock) {
            //if(ctx.ch4_enable) {
                lfsr_clocks++;
                lfsr_clock = lfsr_clock_buffer;
                //lfsr_timer = 0;
                //u16 new = ~((ctx.ch4_lfsr & 0b1) ^ ((ctx.ch4_lfsr & 0b10) >> 1)) & 0b1;
                //ctx.ch4_lfsr &= ~(1 << 15);
                //ctx.ch4_lfsr |= ((new << 15) & 0x8000);
                //printf("new bit: %02X\n", (new << 15));
                //if(ctx.ch4_lfsr_width) {
                //    ctx.ch4_lfsr &= ~(1 << 7);
                //    ctx.ch4_lfsr |= (new << 7);
                //}
                //ctx.ch4_lfsr = ctx.ch4_lfsr >> 1;
                //printf("lfsr: %02X, bit: %d\n", ctx.ch4_lfsr, new);
                unsigned bit_mask = ctx.ch4_lfsr_width ? 0x4040 : 0x4000;
                bool new_bit = (ctx.ch4_lfsr ^ (ctx.ch4_lfsr >> 1) ^ 1) & 1;
                ctx.ch4_lfsr >>= 1;
                if(new_bit) {
                    ctx.ch4_lfsr |= bit_mask;
                } else {
                    ctx.ch4_lfsr &= ~bit_mask;
                }
            //}
        }
        float sq1_val = 0;
        float sq2_val = 0;
        float ch3_val = 0;
        float ch4_val = 0;
        if(ctx.audio_enabled){
            if(ctx.ch1_dac){
                sq1_val = -1;
                if(ctx.sq1_enable) {
                    //sq1_val = ((float)ctx.sq1_volume/15.0f) * (((float)(square_sample[ctx.sq1_duty][ctx.sq1_sample]) - 7.5f)/7.5f);
                    sq1_val = ((float)ctx.sq1_volume/15.0f) * (((float)(ctx.sq1_audio_buffer[ctx.sq1_read_index % 4096]) - 7.5f)/7.5f);
                    if(ctx.ch1_left) {
                        left += sq1_val;
                    }else {
                        left -= 1;
                    }
                    if(ctx.ch1_right) {
                        right += sq1_val;
                    }else {
                        right -= 1;
                    }
                } else {
                    left -= 1;
                    right -= 1;
                }
            }
            if(ctx.ch2_dac){
                sq2_val = -1;
                if(ctx.sq2_enable) {
                    //sq2_val = ((float)ctx.sq2_volume/15.0f) * (((float)(square_sample[ctx.sq2_duty][ctx.sq2_sample]) - 7.5f)/7.5f);
                    if(ctx.ch2_left) {
                        left += sq2_val;
                    }else {
                        left -= 1;
                    }
                    if(ctx.ch2_right) {
                        right += sq2_val;
                    }else {
                        right -= 1;
                    }
                } else {
                    left -= 1;
                    right -= 1;
                }
            }
            if(ctx.ch3_dac){
                ch3_val = -1;
                if(ctx.ch3_enable) {
                    u8 shift = 0;
                    if(ctx.ch3_volume == 0b10) {
                        shift = 1;
                    }
                    if(ctx.ch3_volume == 0b00) {
                        shift = 4;
                    }
                    if(ctx.ch3_volume == 0b11) {
                        shift = 2;
                    }
                    //ch3_val = (((float)(ctx.ch3_last_sample >> shift) - 7.5f)/7.5f);
                    if(ctx.ch3_left) {
                        left += ch3_val;
                        //printf("left: %d\n", ctx.ch3_volume);
                    }else {
                        left -= 1;
                    }
                    if(ctx.ch3_right) {
                        right += ch3_val;
                    }else {
                        right -= 1;
                    }
                } else {
                    left -= 1;
                    right -= 1;
                }
            }
            if(ctx.ch4_dac){
                ch4_val = -1;
                if(ctx.ch4_enable) {
                    //ch4_val = ((ctx.ch4_lfsr & 0b1) == 0b1) ? (((float)ctx.ch4_volume - 7.5f)/7.5f) : -1.0f;
                    if(ctx.ch4_left) {
                        left += ch4_val;
                        //printf("left: %d\n", ctx.ch3_volume);
                    }else {
                        left -= 1;
                    }
                    if(ctx.ch4_right) {
                        right += ch4_val;
                    } else {
                        right -= 1;
                    }
                } else {
                    left -= 1;
                    right -= 1;
                }
            }
        }
        u8 left_vol = ctx.volume_left == 0 ? 1 : ctx.volume_left;
        u8 right_vol = ctx.volume_right == 0 ? 1 : ctx.volume_right;
        left *= (float)left_vol/7.0f;
        right *= (float)right_vol/7.0f;

        //left /= 4;
        //right /= 4;


        bool dacs = ctx.ch1_dac || ctx.ch2_dac || ctx.ch3_dac || ctx.ch4_dac;
        float left_out = 0;
        float right_out = 0;
        if(dacs) {
            left_out = left - left_cap;
            right_out = right - right_cap;
            left_cap = left - left_out * 0.996f;
            right_cap = right - right_out * 0.996f;
        }

        
        left_out /= 4;
        right_out /= 4;

        if(left_out < -1.0f) {
            left_out = -1;
        }
        if(left_out > 1.0f) {
            left_out = 1;
        }
        if(right_out < -1.0f) {
            right_out = -1;
        }
        if(right_out > 1.0f) {
            right_out = 1;
        }
        history_timer++;
        if(history_timer >= history_interval){
            history_timer = 0;
            ctx.sq1_history[ctx.sq1_index++] = sq1_val;
            if (ctx.sq1_index == 384) {
                ctx.sq1_index = 0;
            }

            ctx.sq2_history[ctx.sq2_index++] = sq2_val;
            if (ctx.sq2_index == 384) {
                ctx.sq2_index = 0;
            }

            ctx.ch3_history[ctx.ch3_index++] = ch3_val;
            if (ctx.ch3_index == 384) {
                ctx.ch3_index = 0; 
            }

            ctx.ch4_history[ctx.ch4_index++] = ch4_val;
            if (ctx.ch4_index == 384) {
                ctx.ch4_index = 0;
            }
            ctx.left_history[ctx.left_index++] = left_out;
            if (ctx.left_index == 384) {
                ctx.left_index = 0;
            }

            ctx.right_history[ctx.right_index++] = right_out;
            if (ctx.right_index == 384) {
                ctx.right_index = 0;
            }
        }

        *out++ = left_out;
        *out++ = right_out;
    }
    return paContinue;
}
static PaStream *stream;

void audio_sample();

void audio_init(){
    PaStreamParameters output_parameters;
    PaError err;

    ctx.audio_enabled = false;
    ctx.ch1_left = false;
    ctx.ch1_right = false;
    ctx.ch2_left = false;
    ctx.ch2_right = false;
    ctx.ch3_left = false;
    ctx.ch3_right = false;
    ctx.ch4_left = false;
    ctx.ch4_right = false;

    ctx.volume_left = 0x1;
    ctx.volume_right = 0x1;

    ctx.sq1_duty = 0x0;
    ctx.sq1_volume = 0x0;
    ctx.sq1_sample = 0x0;
    ctx.sq1_period_reset = 0x0;
    ctx.sq1_period_timer = 0x0;
    ctx.sq1_enable = false;
    ctx.sq1_len_enable = false;
    ctx.sq1_sweep_pace = 0x0;
    ctx.sq1_sweep_direction = false;
    ctx.sq1_initial_len = 0x0;
    ctx.sq1_len = 0x0;
    ctx.sq1_initial_volume = 0x0;
    ctx.sq1_env_direction = false;
    ctx.sq1_env_pace = 0x0;
    ctx.sq1_env_timer = 0x0;
    ctx.sq1_sweep_step = 0x0;

    ctx.sq2_duty = 0x0;
    ctx.sq2_volume = 0x0;
    ctx.sq2_sample = 0x0;
    ctx.sq2_period_reset = 0x0;
    ctx.sq2_period_timer = 0x0;
    ctx.sq2_enable = false;
    ctx.sq2_len_enable = false;
    ctx.sq2_initial_len = 0x0;
    ctx.sq2_len = 0x0;
    ctx.sq2_initial_volume = 0x0;
    ctx.sq2_env_direction = false;
    ctx.sq2_env_pace = 0x0;
    ctx.sq2_env_timer = 0x0;
    ctx.sq1_write_index = 1;
    
    err = Pa_Initialize();
    if (err != paNoError) goto error;
    output_parameters.device = Pa_GetDefaultOutputDevice();
    if(output_parameters.device == paNoDevice) {
        fprintf(stderr, "No default audio device!\n");
        goto error;
    }
    output_parameters.channelCount = 2;
    output_parameters.sampleFormat = paFloat32;
    output_parameters.suggestedLatency = Pa_GetDeviceInfo(output_parameters.device)->defaultLowOutputLatency;
    output_parameters.hostApiSpecificStreamInfo = NULL;

    err = Pa_OpenStream(&stream,
                        NULL,
                        &output_parameters,
                        SAMPLE_RATE,
                        paFramesPerBufferUnspecified,
                        paNoFlag,
                        audio_callback,
                        &ctx);
    if(err != paNoError) goto error;
    err = Pa_StartStream(stream);
    if(err != paNoError) goto error;
    return;
error:
    Pa_Terminate();
    fprintf(stderr, "portaudio stream error\n\tError Number: %d\n\tError Message: %s\n", err, Pa_GetErrorText(err));
}

void sq1_sweep() {
    //frequency calculation
    int step = (ctx.sq1_sweep_period >> ctx.sq1_sweep_step);
    step = ctx.sq1_sweep_direction ? -step : step;
    ctx.sq1_calc_period = ctx.sq1_sweep_period + step;
    //overflow check
    if(ctx.sq1_calc_period > 0x7FF) {
        ctx.sq1_enable = false;
		}
}	

static int change = 1;
static u32 ticks = 0;
static u32 start, end = 0;
static u32 period_tick;

void audio_period_tick() {
    period_tick++;
    if(period_tick % 2 == 0){
        ctx.sq1_period_timer++;

        if(ctx.sq1_period_timer >= 0x800) {
            ctx.sq1_period_timer = ctx.sq1_period_reset;
            ctx.sq1_sample = (ctx.sq1_sample + 1) % 8;
        }
        if(ctx.sq1_write_index > ctx.sq1_read_index){
            ctx.sq1_audio_buffer[(ctx.sq1_write_index++)%4096] = square_sample[ctx.sq1_duty][ctx.sq1_sample];
        } else {
            printf("buffer overflow\n");
        }
    }
}

void audio_tick(){
    u32 prev_ticks = ticks;
    ticks++;
    if((ticks % 2) == 0) {
        if(ctx.sq1_len_enable) {
            ctx.sq1_len++;
            if(ctx.sq1_len >= 64) {
                ctx.sq1_len = 0;
                ctx.sq1_enable = false;
                ctx.ch1_len_fz = true;
            }
        }

        if(ctx.sq2_len_enable) {
            ctx.sq2_len++;
            if(ctx.sq2_len >= 64) {
                ctx.sq2_len = 0;
                ctx.sq2_enable = false;
                ctx.ch2_len_fz = true;
            }
        }

        if(ctx.ch3_len_enable) {
            ctx.ch3_len++;
            if(ctx.ch3_len == 0) {
                ctx.ch3_enable = false;
                ctx.ch3_len_fz = true;
            }
        }

        if(ctx.ch4_len_enable && ctx.ch4_enable) {
            ctx.ch4_len++;
            //printf("Ch4 len: %02X\n", ctx.ch4_len);
            if(ctx.ch4_len >= 64) {
                ctx.ch4_len = 0;
                ctx.ch4_enable = false;
                ctx.ch4_len_fz = true;
            }
        }
    }

    if((prev_ticks & (1 << 2)) && !(ticks & (1 << 2))) {
        if(ctx.sq1_env_pace != 0){
            ctx.sq1_env_timer++;
            if(ctx.sq1_env_timer >= ctx.sq1_env_pace) {
                ctx.sq1_env_timer = 0;
                if((ctx.sq1_env_direction && ctx.sq1_volume != 15) || (!ctx.sq1_env_direction && ctx.sq1_volume != 0)){
                    ctx.sq1_volume += ctx.sq1_env_direction ? 1 : -1;
                    if(ctx.sq1_volume < 0)
                        ctx.sq1_volume = 0;
                    if(ctx.sq1_volume > 15)
                        ctx.sq1_volume = 15;
                }
            }
        }

        if(ctx.sq2_env_pace != 0){
            ctx.sq2_env_timer++;
            if(ctx.sq2_env_timer >= ctx.sq2_env_pace) {
                ctx.sq2_env_timer = 0;
                if((ctx.sq2_env_direction && ctx.sq2_volume != 15) || (!ctx.sq2_env_direction && ctx.sq2_volume != 0)){
                    ctx.sq2_volume += ctx.sq2_env_direction ? 1 : -1;
                    if(ctx.sq2_volume < 0)
                        ctx.sq2_volume = 0;
                    if(ctx.sq2_volume > 15)
                        ctx.sq2_volume = 15;
                }
                //printf("sq2 vol: %01X\n", ctx.sq2_volume);
            }
        }

        //if(ctx.ch4_env_pace != 0){
        ctx.ch4_env_timer--;
        if(ctx.ch4_env_timer <= 0) {
            ctx.ch4_env_timer = ctx.ch4_env_pace_buffer != 0 ? ctx.ch4_env_pace_buffer : 8;
            if(ctx.ch4_env_pace != 0 && ((ctx.ch4_env_direction && ctx.ch4_volume != 15) || (!ctx.ch4_env_direction && ctx.ch4_volume != 0))){
                ctx.ch4_volume += ctx.ch4_env_direction ? 1 : -1;
                //printf("Noise Vol: %02X\n", ctx.ch4_volume);
                if(ctx.ch4_volume < 0)
                    ctx.ch4_volume = 0;
                if(ctx.ch4_volume > 15)
                    ctx.ch4_volume = 15;
            }
        }
        //}

    }

    if((prev_ticks & (1 << 1)) && !(ticks & (1 << 1)) && ctx.sq1_sweep_enabled) {
        ctx.sq1_sweep_timer--;
        if(ctx.sq1_sweep_timer <= 0){//(ctx.sq1_sweep_timer >= ctx.sq1_sweep_pace && ctx.sq1_sweep_pace > 0) || (ctx.sq1_sweep_timer >= 8 && ctx.sq1_sweep_pace == 0)) {
            ctx.sq1_sweep_timer = ctx.sq1_sweep_pace != 0 ? ctx.sq1_sweep_pace : 8;
            if(ctx.sq1_enable && ctx.sq1_sweep_pace > 0 && ctx.sq1_sweep_enabled){
                sq1_sweep();
                if(ctx.sq1_calc_period <= 0x7FF && ctx.sq1_sweep_step != 0) {
                    ctx.sq1_sweep_period = ctx.sq1_calc_period;
                    ctx.sq1_period_reset = ctx.sq1_calc_period;
                    sq1_sweep();
                }
            }
        }
    }
}

void enable_square1() {
    if(ctx.ch1_dac){
        ctx.sq1_enable = true;
    }
    ctx.sq1_volume = ctx.sq1_initial_volume;
    ctx.sq1_env_timer = 0;
    ctx.sq1_sweep_period = ctx.sq1_period_reset;
    ctx.sq1_sweep_timer = ctx.sq1_sweep_pace != 0 ? ctx.sq1_sweep_pace : 8;
    ctx.sq1_env_direction = ctx.sq1_env_direction_buffer;
    ctx.sq1_env_pace = ctx.sq1_env_pace_buffer;
    if(ctx.sq1_sweep_step != 0) {
        sq1_sweep();
    }
}

void enable_square2() {
    if(!ctx.ch2_dac){
        return;
    }
    ctx.sq2_enable = true;
    ctx.sq2_volume = ctx.sq2_initial_volume;
    ctx.sq2_env_timer = 0;
    ctx.sq2_sample = 0;
    ctx.sq2_period_timer = ctx.sq2_period_reset;
    ctx.sq2_env_direction = ctx.sq2_env_direction_buffer;
    ctx.sq2_env_pace = ctx.sq2_env_pace_buffer;
}

void enable_wave() {
    if(!ctx.ch3_dac){
        return;
    }
    ctx.ch3_enable = true;
    ctx.ch3_sample = 0;
    //ctx.ch3_volume = ctx.ch3_initial_volume;
    ctx.ch3_period_timer = ctx.ch3_period_reset;
}

void enable_noise() {
    if(!ctx.ch4_dac){
        return;
    }
    start = get_ticks();
    ctx.ch4_enable = true;
    ctx.ch4_env_timer = ctx.ch4_env_pace_buffer != 0 ? ctx.ch4_env_pace_buffer : 8;
    ctx.ch4_env_direction = ctx.ch4_env_direction_buffer;
    ctx.ch4_env_pace = ctx.ch4_env_pace_buffer;
    ctx.ch4_volume = ctx.ch4_initial_volume;
    ctx.ch4_lfsr = 0;
    lfsr_clock = lfsr_clock_buffer;
    lfsr_timer = 0;
}

u8 audio_read(u16 address) {
    //printf("read at %02X\n", address);
    if(address == 0xFF26) {
        u8 value = (ctx.audio_enabled << 7) | (ctx.ch4_enable << 3) | (ctx.ch3_enable << 2) | (ctx.sq2_enable << 1) | (ctx.sq1_enable);
        return value | 0b01110000;
    }
    if(address == 0xFF25) {
        u8 value = 0x0;
        value |= ctx.ch4_left << 7;
        value |= ctx.ch3_left << 6;
        value |= ctx.ch2_left << 5;
        value |= ctx.ch1_left << 4;

        value |= ctx.ch4_right << 3;
        value |= ctx.ch3_right << 2;
        value |= ctx.ch2_right << 1;
        value |= ctx.ch1_right;
        //printf("read at %02X; Value: %02X\n", address, value);
        return value;
    }

    if(address == 0xFF24) {
        u8 value = (ctx.volume_left & 0b111) << 4;
        value |= (ctx.volume_right & 0b111);
        value |= (ctx.vin_left << 7);
        value |= (ctx.vin_right << 3);
        return value;
    }

    if(address == 0xFF10) {
        u8 value = (ctx.sq1_sweep_pace & 0b111) << 4;
        value |= (ctx.sq1_sweep_direction) << 3;
        value |= ctx.sq1_sweep_step & 0b111;
        return value | 0x80;
    }
    if(address == 0xFF11) {
        u8 value = (ctx.sq1_duty & 0b11) << 6;
        return value | 0b00111111;
    }
    if(address == 0xFF12) {
        u8 value = (ctx.sq1_initial_volume & 0b1111) << 4;
        value |= ctx.sq1_env_direction_buffer << 3;
        value |= (ctx.sq1_env_pace_buffer & 0b111);
        return value;
    }
    if(address == 0xFF13) {
        return 0xFF;
    }
    if(address == 0xFF14) {
        return (ctx.sq1_len_enable << 6) | 0b10111111;
    }


    if(address == 0xFF16) {
        u8 value = (ctx.sq2_duty & 0b11) << 6;
        return value | 0b00111111;
    }
    if(address == 0xFF17) {
        u8 value = (ctx.sq2_initial_volume & 0b1111) << 4;
        value |= ctx.sq2_env_direction_buffer << 3;
        value |= (ctx.sq2_env_pace_buffer & 0b111);
        return value;
    }
    if(address == 0xFF18) {
        return 0xFF;
    }
    if(address == 0xFF19) {
        return (ctx.sq2_len_enable << 6) | 0b10111111;
    }

    if(address == 0xFF1A) {
        return (ctx.ch3_dac << 7) | 0b01111111;
    }
    
    if(address == 0xFF1B) {
        return 0xFF;
    }

    if(address == 0xFF1C) {
        return ((ctx.ch3_volume & 0b11) << 5) | 0b10011111;
    }

    if(address == 0xFF1D) {
        return 0xFF;
    }

    if(address == 0xFF1E) {
        return (ctx.ch3_len_enable << 6) | 0b10111111;
    }

    if(address == 0xFF20) {
        return 0xFF;
    }

    if(address == 0xFF21) {
        u8 value = (ctx.ch4_initial_volume & 0b1111) << 4;
        value |= ctx.ch4_env_direction_buffer << 3;
        value |= (ctx.ch4_env_pace_buffer & 0b111);
        return value;
    }

    if(address == 0xFF22) {
        u8 value = (ctx.ch4_clock_shift & 0xF) << 4;
        value |= ctx.ch4_lfsr_width << 3;
        value |= (ctx.ch4_clock_divider & 0b111);
        return value;
    }

    if(address == 0xFF23) {
        return (ctx.ch4_len_enable << 6) | 0b10111111;
    }

    if(BETWEEN(address, 0xFF30, 0xFF3F)) {
        return ctx.wave_ram[address - 0xFF30];
    }
    return 0xFF;
}

void reset_buffers(){
    ctx.ch1_dac = 0;
    ctx.ch2_dac = 0;
    ctx.ch3_dac = 0;
    ctx.ch1_left = false;
    ctx.ch1_right = false;
    ctx.ch2_left = false;
    ctx.ch2_right = false;
    ctx.ch3_left = false;
    ctx.ch3_right = false;
    ctx.ch4_left = false;
    ctx.ch4_right = false;
    ctx.volume_left = 0;
    ctx.volume_right = 0;
    ctx.vin_left = 0;
    ctx.vin_right = 0;
    ctx.sq1_duty = 0;
    ctx.sq1_volume = 0;
    ctx.sq1_sample = 0;
    ctx.sq1_period_reset = 0;
    ctx.sq1_period_timer = 0;
    ctx.sq1_enable = 0;
    ctx.sq1_len_enable = 0;
    ctx.sq1_sweep_pace = 0;
    ctx.sq1_sweep_direction = 0;
    ctx.sq1_initial_len = 0;
    ctx.sq1_len = 0;
    ctx.sq1_initial_volume = 0;
    ctx.sq1_env_direction = 0;
    ctx.sq1_env_direction_buffer = 0;
    ctx.sq1_env_pace = 0;
    ctx.sq1_env_pace_buffer = 0; 
    ctx.sq1_env_timer = 0;
    ctx.sq1_sweep_step = 0;
    ctx.sq1_sweep_timer = 0;
    ctx.sq1_sweep_enabled = 0;
    ctx.sq1_sweep_period = 0;
    ctx.sq1_calc_period = 0;
    ctx.sq2_duty = 0;
    ctx.sq2_volume = 0;
    ctx.sq2_sample = 0;
    ctx.sq2_period_reset = 0;
    ctx.sq2_period_timer = 0;
    ctx.sq2_enable = 0;
    ctx.sq2_len_enable = 0;
    ctx.sq2_initial_len = 0;
    ctx.sq2_len = 0;
    ctx.sq2_initial_volume = 0;
    ctx.sq2_env_direction = 0;
    ctx.sq2_env_direction_buffer = 0;
    ctx.sq2_env_pace = 0;
    ctx.sq2_env_pace_buffer = 0;
    ctx.sq2_env_timer = 0;
    ctx.ch3_enable = 0;
    ctx.ch3_dac = 0;
    ctx.ch3_initial_len = 0;
    ctx.ch3_len = 0;
    ctx.ch3_volume = 0;
    ctx.ch3_initial_volume = 0;
    ctx.ch3_period_timer = 0;
    ctx.ch3_period_reset = 0;
    ctx.ch3_len_enable = 0;
    ctx.ch3_last_sample = 0;
    ctx.ch3_sample = 0;
    ctx.ch4_enable = 0;
    ctx.ch4_initial_len = 0;
    ctx.ch4_len = 0;
    ctx.ch4_initial_volume = 0;
    ctx.ch4_volume = 0;
    ctx.ch4_env_pace = 0;
    ctx.ch4_env_pace_buffer = 0;
    ctx.ch4_env_timer = 0;
    ctx.ch4_env_direction = 0;
    ctx.ch4_env_direction_buffer = 0;
    ctx.ch4_clock_shift = 0;
    ctx.ch4_lfsr_width = 0;
    ctx.ch4_clock_divider = 0;
    ctx.ch4_len_enable = 0;
    ctx.ch4_lfsr = 0;
}

void audio_write(u16 address, u8 value){
    if(BETWEEN(address, 0xFF30, 0xFF3F)) {
        ctx.wave_ram[address - 0xFF30] = value;
    }

    if(address == 0xFF26) {
        ctx.audio_enabled = (value & 0x80) == 0x80;
        if(!ctx.audio_enabled) {
            //printf("audio disabled\n");
            reset_buffers();
        }
    }

    if(!ctx.audio_enabled){
        return;
    }

    if(address == 0xFF25) {
        ctx.ch4_left = value & 0b10000000;
        ctx.ch3_left = value & 0b01000000;
        ctx.ch2_left = value & 0b00100000;
        ctx.ch1_left = value & 0b00010000;

        ctx.ch4_right = value & 0b00001000;
        ctx.ch3_right = value & 0b00000100;
        ctx.ch2_right = value & 0b00000010;
        ctx.ch1_right = value & 0b00000001;
        //printf("write at %02X; Value: %02X\n", address, value);
    }

    if(address == 0xFF24) {
        ctx.volume_left = (value >> 4) & 0b111;
        //if(ctx.volume_left == 0) ctx.volume_left = 1;
        ctx.volume_right = value & 0b111;
        //if(ctx.volume_right == 0) ctx.volume_right = 1;
        ctx.vin_left = value & 0x80;
        ctx.vin_right = value & 0x8;
    }

    if(address == 0xFF10) {
        ctx.sq1_sweep_pace = (value >> 4) & 0b111;
        ctx.sq1_sweep_direction = value & 0x08;
        ctx.sq1_sweep_step = value & 0b111;
    	ctx.sq1_sweep_enabled = ((ctx.sq1_sweep_pace != 0) || (ctx.sq1_sweep_step != 0));
    }
    if(address == 0xFF11) {
        ctx.sq1_duty = value >> 6;
        ctx.sq1_initial_len = value & 0x3F;
        ctx.sq1_len = ctx.sq1_initial_len;
        ctx.ch1_len_fz = false;
        //printf("initial len: %02X\n", ctx.sq1_initial_len);
    }
    if(address == 0xFF12) {
        ctx.sq1_initial_volume = (value >> 4) & 0x0F;
        ctx.sq1_env_direction_buffer = (value & 0x8) == 0x8;
        ctx.sq1_env_pace_buffer = value & 0b111;
        if(ctx.sq1_env_direction_buffer == 0 && ctx.sq1_initial_volume == 0) {
            ctx.sq1_enable = false;
            ctx.ch1_dac = false;
        } else {
            ctx.ch1_dac = true;
        }
    }
    if(address == 0xFF13) {
        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0xF00) | value;
    }
    if(address == 0xFF14) {
        if(ctx.sq1_len == 64) {
            ctx.sq1_len = 0;
        }
        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0x0FF) | ((value & 0b111) << 8);
        bool len_en = (value & 0x40) == 0x40;
        if((ticks % 2) == 0 && !ctx.ch1_len_fz && !ctx.sq1_len_enable && len_en){
            ctx.sq1_len++;
            if(ctx.sq1_len >= 64) {
                ctx.sq1_len = 0;
                ctx.sq1_enable = false;
                ctx.ch1_len_fz = true;
            }
        } 
        if((ticks % 2) == 0 && ctx.ch1_len_fz && (value & 0x80) == 0x80){
            ctx.ch1_len_fz = false;
            if(len_en){
                ctx.sq1_len++;
            }
        }
        ctx.sq1_len_enable = len_en;
        if(value & 0x80) {
            enable_square1();
        }
    }


    if(address == 0xFF16) {
        ctx.sq2_duty = value >> 6;
        ctx.sq2_initial_len = value & 0x3F;
        ctx.sq2_len = ctx.sq2_initial_len;
        ctx.ch2_len_fz = false;
    }
    if(address == 0xFF17) {
        ctx.sq2_initial_volume = (value >> 4) & 0x0F;
        ctx.sq2_env_direction_buffer = (value & 0x8) == 0x8;
        ctx.sq2_env_pace_buffer = value & 0b111;
        if(ctx.sq2_env_direction_buffer == 0 && ctx.sq2_initial_volume == 0) {
            ctx.sq2_enable = false;
            ctx.ch2_dac = false;
        } else {
            ctx.ch2_dac = true;
        }
    }
    if(address == 0xFF18) {
        u16 prev_period = ctx.sq2_period_reset;
        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0xF00) | value;
        //printf("period: %03X, old_period: %03X\n", ctx.sq2_period_reset, prev_period);
    }
    if(address == 0xFF19) {
        if(ctx.sq2_len == 64) {
            ctx.sq2_len = 0;
        }
        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0x0FF) | ((value & 0b111) << 8);
        bool len_en = (value & 0x40) == 0x40;
        if((ticks % 2) == 0 && !ctx.ch2_len_fz && !ctx.sq2_len_enable && len_en){
            ctx.sq2_len++;
            if(ctx.sq2_len >= 64) {
                ctx.sq2_len = 0;
                ctx.sq2_enable = false;
                ctx.ch2_len_fz = true;
            }
        } 
        if((ticks % 2) == 0 && ctx.ch2_len_fz && (value & 0x80) == 0x80){
            ctx.ch2_len_fz = false;
            if(len_en){
                ctx.sq2_len++;
            }
        }
        ctx.sq2_len_enable = len_en;
        if(value & 0x80) {
            enable_square2();
        }
    }

    if(address == 0xFF1A) {
        ctx.ch3_dac = value >> 7;
        if(!ctx.ch3_dac){
            ctx.ch3_enable = false;
            //printf("ch3 off\n");
        }
    }
    
    if(address == 0xFF1B) {
        ctx.ch3_initial_len = value;
        ctx.ch3_len = ctx.ch3_initial_len;
        ctx.ch3_len_fz = false;
    }

    if(address == 0xFF1C) {
        ctx.ch3_volume = (value >> 5) & 0b11;
        //printf("Ch3 vol: %d\n", ctx.ch3_volume);
    }

    if(address == 0xFF1D) {
        ctx.ch3_period_reset = (ctx.ch3_period_reset & 0xF00) | value;
    }

    if(address == 0xFF1E) {
        ctx.ch3_period_reset = (ctx.ch3_period_reset & 0x0FF) | ((value & 0b111) << 8);
        bool len_en = (value & 0x40) == 0x40;
        if((ticks % 2) == 0 && !ctx.ch3_len_fz && !ctx.ch3_len_enable && len_en){
            if(ctx.ch3_len >= 255) {
                ctx.ch3_len = 0;
                ctx.ch3_enable = false;
                ctx.ch3_len_fz = true;
            } else {
                ctx.ch3_len++;
            }
        } 
        if((ticks % 2) == 0 && ctx.ch3_len_fz && (value & 0x80) == 0x80){
            ctx.ch3_len_fz = false;
            if(len_en){
                ctx.ch3_len++;
            }
        }
        ctx.ch3_len_enable = len_en;
        if(value & 0x80) {
            enable_wave();
        }
    }

    if(address == 0xFF20) {
        ctx.ch4_initial_len = value & 0b111111;
        ctx.ch4_len = ctx.ch4_initial_len;
        ctx.ch4_len_fz = false;
        //printf("ch4 len: %02X\n", ctx.ch4_len);
        printf("Write Ch4 Len: %02X\n", value);
    }

    if(address == 0xFF21) {
        ctx.ch4_initial_volume = (value >> 4) & 0x0F;
        ctx.ch4_env_direction_buffer = (value & 0x8) == 0x8;
        ctx.ch4_env_pace_buffer = value & 0b111;
        if(ctx.ch4_env_direction_buffer == 0 && ctx.ch4_initial_volume == 0) {
            ctx.ch4_enable = false;
            ctx.ch4_dac = false;
        } else {
            ctx.ch4_dac = true;
        }
        printf("Write Ch4 env: %02X\n", value);
    }

    if(address == 0xFF22) {
        ctx.ch4_clock_shift = (value >> 4) & 0xF;
        ctx.ch4_lfsr_width = (value & 0x08);
        ctx.ch4_clock_divider = value & 0b111;
        float div = (ctx.ch4_clock_divider == 0 ? 0.5f : (float)ctx.ch4_clock_divider);
        float lfsr_rate = (LFSR_BASE_CLOCK) / (div * (float)(1 << ctx.ch4_clock_shift));
        lfsr_clock_buffer = 1.0f / (lfsr_rate/1000.0f/1000.0f);
        //printf("Ch4 clock: Shift: %01X, Width: %d, Div: %f (%01X), Rate: %f, Clock %f\n", ctx.ch4_clock_shift, ctx.ch4_lfsr_width, div, ctx.ch4_clock_divider, lfsr_rate, lfsr_clock);
        printf("Write Ch4 Clock: %02X\n", value);
    }

    if(address == 0xFF23) {
        if(ctx.ch4_len == 64) {
            ctx.ch4_len = 0;
        }
        bool len_en = (value & 0x40) == 0x40;
        if((ticks % 2) == 0 && !ctx.ch4_len_fz && !ctx.ch4_len_enable && len_en){
            ctx.ch4_len++;
            if(ctx.ch4_len >= 64) {
                ctx.ch4_len = 0;
                ctx.ch4_enable = false;
                ctx.ch4_len_fz = true;
            }
        } 
        if((ticks % 2) == 0 && ctx.ch4_len_fz && (value & 0x80) == 0x80){
            ctx.ch4_len_fz = false;
            if(len_en){
                ctx.ch4_len++;
            }
        }
        ctx.ch4_len_enable = len_en;
        //printf("ch4 trigger: LenEn %d\n", len_en);
        if(value & 0x80) {
            enable_noise();
        }
        printf("Write Ch4 Trigger: %02X\n", value);
    }
}