#include <audio.h>
#include <math.h>
#include <windows.h>

#include <portaudio.h>

#define SAMPLE_RATE 44100
#define FRAMES_PER_BUFFER 64
#define TABLE_SIZE 44100

#define M_PI 3.14159265

static float sine[TABLE_SIZE];
static unsigned long ind = 0;
static unsigned long ind2 = 0;

static int sq1_on_time;
static int sq1_off_time;
static float sq1 = 1;
static int sq1_timer = 0;

static int sq2_on_time;
static int sq2_off_time;
static float sq2 = 1;
static int sq2_timer = 0;

static audio_context ctx;

HANDLE mutex;

const u8 square_sample[8] = {
    0x0,
    0x0,
    0x0,
    0x0,
    0x1,
    0x1,
    0x1,
    0x1
};

static int audio_callback(const void* input_uffer, void *output_buffer,
                            unsigned long framesPerBuffer,
                            const PaStreamCallbackTimeInfo *time_info,
                            PaStreamCallbackFlags status_flags,
                            void *userData ) {
    //printf("Audio Callback!\n");
    //audio_context *ctx = (audio_context *) userData;
    float *out = (float *)output_buffer;
    //static float last = 0;
    //float val;
    //if(!ctx.ready) {
    //    val = last;
    //} else {
    //    val = ctx.sq1_value;
    //    last = val;
    //    ctx.ready = false;
    //}
    //PaTime time = time_info->currentTime;
    //sq1_on_time = 1/(ctx->sq1_freq/(float)SAMPLE_RATE) * ctx->sq1_duty;
    //sq1_off_time = 1/(ctx->sq1_freq/(float)SAMPLE_RATE) * (1 - ctx->sq1_duty);

    //sq2_on_time = 1/(ctx->sq2_freq/(float)SAMPLE_RATE) * ctx->sq2_duty;
    //sq2_off_time = 1/(ctx->sq2_freq/(float)SAMPLE_RATE) * (1 - ctx->sq2_duty);
        //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;
        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
        //ctx.sq1_value = - ctx.sq1_value;
    //}
    //printf("val: %f\n", val);
    float left = 0;
    float right = 0;
    for(int i = 0; i < framesPerBuffer; i++) {
        if(ctx.sq1_write_head != ctx.sq1_read_head) {
            if(ctx.ch1_left){
                left = (float)(ctx.sq1_volume * ctx.sq1_audio_buffer[ctx.sq1_read_head])/15.0f;
            }
            if(ctx.ch1_right){
                right = (float)(ctx.sq1_volume * ctx.sq1_audio_buffer[ctx.sq1_read_head])/15.0f;
            }
            ctx.sq1_read_head++;
            if (ctx.sq1_read_head >= 4096) {
                ctx.sq1_read_head = 0;
            }
        }

        if(ctx.sq2_write_head != ctx.sq2_read_head) {
            if(ctx.ch2_left){
                left += (float)(ctx.sq2_volume * ctx.sq2_audio_buffer[ctx.sq2_read_head])/15.0f;
            }
            if(ctx.ch2_right){
                right += (float)(ctx.sq2_volume * ctx.sq2_audio_buffer[ctx.sq2_read_head])/15.0f;
            }
            ctx.sq2_read_head++;
            if (ctx.sq2_read_head >= 4096) {
                ctx.sq2_read_head = 0;
            }
        }
        left *= (float)ctx.volume_left/8.0f;
        right *= (float)ctx.volume_right/8.0f;
        *out++ = left;
        *out++ = right;
    }
    return paContinue;
}

void audio_init(){
    mutex = CreateMutex(NULL, FALSE, NULL);
    PaStreamParameters output_parameters;
    PaStream *stream;
    PaError err;

    ctx.sq1_volume = 1;
    ctx.sq1_duty = 0.5;
    ctx.sq1_freq = 200;

    ctx.sq2_volume = 1;
    ctx.sq2_duty = 0.5;
    ctx.sq2_freq = 400;

    ctx.sq1_period_reset = 0x100;
    ctx.sq1_period_timer = 0x100;
    ctx.sq1_value = -1;
    ctx.sq1_write_head = 0;
    ctx.sq1_read_head = 0;
    ctx.sq2_read_head = 0;
    ctx.sq2_write_head = 0;

    for(int i = 0; i < TABLE_SIZE; i++) {
        sine[i] = (float) sin(((double)i/(double)TABLE_SIZE) * M_PI * 2.);
    }
    
    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,
                        FRAMES_PER_BUFFER,
                        NULL,
                        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));
}

static int change = 1;
static u32 ticks = 0;

void audio_tick(){
    u32 prev_ticks = ticks;
    ticks++;
    if(!(ticks & 0b1)) {
        if(ctx.sq1_len_enable) {
            ctx.sq1_len++;
            if(ctx.sq1_len >= 64) {
                ctx.sq1_enable = false;
            }
        }

        if(ctx.sq2_len_enable) {
            ctx.sq2_len++;
            if(ctx.sq2_len >= 64) {
                ctx.sq2_enable = false;
                printf("sq2 timer");
            }
        }
    }

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

        if(ctx.sq2_env_pace != 0){
            ctx.sq2_env_timer++;
            if(ctx.sq2_env_timer >= ctx.sq2_env_pace) {
                ctx.sq2_volume += ctx.sq2_env_direction ? 1 : -1;
                if(ctx.sq2_volume < 0)
                    ctx.sq2_volume = 0;
                if(ctx.sq2_volume > 7)
                    ctx.sq2_volume = 7;
                ctx.sq2_env_timer = 0;
            }
        }

    }
}

static u32 updates;

void audio_period_tick(){
    updates++;
    //DWORD dwaitResult = WaitForSingleObject(mutex, INFINITE);
    //if(dwaitResult == WAIT_ABANDONED)
    //    return;

    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;
        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
        //ctx.ready = true;
    }
    if(ctx.sq2_period_timer >= 0x800) {
        ctx.sq2_period_timer = ctx.sq2_period_reset;
        ctx.sq2_sample = (ctx.sq2_sample + 1) % 8;
        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
        //ctx.ready = true;
    }
    if(updates >= 24) {
        updates = 0;
        //u32 read = ctx.read_head;
        //u32 write = ctx.write_head;
        //if(read > write) {
        //    write += 4096;
        //}
        //if(write - read <= 64){
        //    return;
        //}
        if(ctx.sq1_enable){
            ctx.sq1_audio_buffer[ctx.sq1_write_head++] = square_sample[ctx.sq1_sample];
            if(ctx.sq1_write_head >= 4096) {
                ctx.sq1_write_head = 0;
            }
        }
        if(ctx.sq2_enable){
            ctx.sq2_audio_buffer[ctx.sq2_write_head++] = square_sample[ctx.sq2_sample];
            if(ctx.sq2_write_head >= 4096) {
                ctx.sq2_write_head = 0;
            }
        }
    }
    //ReleaseMutex(mutex);
}

void enable_square1() {
    ctx.sq1_enable = true;
    if(ctx.sq1_len >= 64) {
        ctx.sq1_len = ctx.sq1_initial_len;
    }
    ctx.sq1_volume = ctx.sq1_initial_volume;
    ctx.sq1_env_timer = 0;

}

void enable_square2() {
    //ctx.sq2_enable = true;
    if(ctx.sq2_len >= 64) {
        ctx.sq2_len = ctx.sq2_initial_len;
    }
    ctx.sq2_volume = ctx.sq2_initial_volume;
    ctx.sq2_env_timer = 0;

}

u8 audio_read(u16 address) {
    return 0x00;
}

void audio_write(u16 address, u8 value){
    if(address == 0xFF26) {
        ctx.audio_enabled = value & 0x80;
    }
    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;
    }

    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;
    }

    if(address == 0xFF10) {
        ctx.sq1_sweep_pace = (value >> 4) & 0b111;
        ctx.sq1_sweep_direction = value & 0x08;
        ctx.sq1_sweep_step = value & 0b111;
    }
    if(address == 0xFF11) {
        ctx.sq1_duty = value >> 6;
        ctx.sq1_initial_len = value & 0x3F;
    }
    if(address == 0xFF12) {
        ctx.sq1_initial_volume = value >> 4;
        ctx.sq1_env_direction = value & 0x8;
        ctx.sq1_env_pace = value & 0b111;
    }
    if(address == 0xFF13) {
        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0xF0) | value;
    }
    if(address == 0xFF14) {
        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0x0F) | ((value & 0b111) << 8);
        ctx.sq1_len_enable = value & 0x40;
        if(value & 0x80) {
            enable_square1();
        }
    }


    if(address == 0xFF16) {
        ctx.sq2_duty = value >> 6;
        ctx.sq2_initial_len = value & 0x3F;
    }
    if(address == 0xFF17) {
        ctx.sq2_initial_volume = value >> 4;
        ctx.sq2_env_direction = value & 0x8;
        ctx.sq2_env_pace = value * 0b111;
    }
    if(address == 0xFF18) {
        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0xF0) | value;
    }
    if(address == 0xFF19) {
        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0x0F) | ((value & 0b111) << 8);
        ctx.sq2_len_enable = value & 0x40;
        if(value & 0x80) {
            enable_square2();
        }
    }
}