#include <math.h>

#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/vreg.h"
#include "hardware/irq.h"

#define CORE_VOLTAGE VREG_VOLTAGE_1_20
#define SYS_CLOCK_KHZ 252000
#define SAMPLE_CLOCK_KHZ 48
#define SAMPLE_BITS 12

static_assert(SYS_CLOCK_KHZ % SAMPLE_CLOCK_KHZ == 0, "Audio clock must divide system clock");
static_assert(1 << SAMPLE_BITS < SYS_CLOCK_KHZ / SAMPLE_CLOCK_KHZ, "Samples larger than PWM period");
static_assert(1 << SAMPLE_BITS >= SYS_CLOCK_KHZ / SAMPLE_CLOCK_KHZ / 2, "Samples less than half of PWM period");

// Range 0 to 0xfff
static uint16_t __time_critical_func(sine_table)[256] = {
	0x800,  0x832,  0x864,  0x896,  0x8c8,  0x8fa,  0x92c,  0x95e,  0x98f,  0x9c0,
	0x9f1,  0xa22,  0xa52,  0xa82,  0xab1,  0xae1,  0xb0f,  0xb3d,  0xb6b,
	0xb98,  0xbc5,  0xbf1,  0xc1c,  0xc47,  0xc71,  0xc9b,  0xcc3,  0xceb,  0xd13,
	0xd39,  0xd5f,  0xd84,  0xda8,  0xdcb,  0xded,  0xe0e,  0xe2f,  0xe4e,
	0xe6c,  0xe8a,  0xea6,  0xec2,  0xedc,  0xef5,  0xf0e,  0xf25,  0xf3b,  0xf50,
	0xf64,  0xf76,  0xf88,  0xf98,  0xfa7,  0xfb5,  0xfc2,  0xfce,  0xfd8,
	0xfe1,  0xfe9,  0xff0,  0xff6,  0xffa,  0xffd,  0xfff,  0xfff,  0xfff,
	0xffd,  0xffa,  0xff6,  0xff0,  0xfe9,  0xfe1,  0xfd8,  0xfce,  0xfc2,  0xfb5,
	0xfa7,  0xf98,  0xf88,  0xf76,  0xf64,  0xf50,  0xf3b,  0xf25,  0xf0e,
	0xef5,  0xedc,  0xec2,  0xea6,  0xe8a,  0xe6c,  0xe4e,  0xe2f,  0xe0e,  0xded,
	0xdcb,  0xda8,  0xd84,  0xd5f,  0xd39,  0xd13,  0xceb,  0xcc3,  0xc9b,
	0xc71,  0xc47,  0xc1c,  0xbf1,  0xbc5,  0xb98,  0xb6b,  0xb3d,  0xb0f,  0xae1,
	0xab1,  0xa82,  0xa52,  0xa22,  0x9f1,  0x9c0,  0x98f,  0x95e,  0x92c,
	0x8fa,  0x8c8,  0x896,  0x864,  0x832,  0x800,  0x7cd,  0x79b,  0x769,  0x737,
	0x705,  0x6d3,  0x6a1,  0x670,  0x63f,  0x60e,  0x5dd,  0x5ad,  0x57d,
	0x54e,  0x51e,  0x4f0,  0x4c2,  0x494,  0x467,  0x43a,  0x40e,  0x3e3,  0x3b8,
	0x38e,  0x364,  0x33c,  0x314,  0x2ec,  0x2c6,  0x2a0,  0x27b,  0x257,
	0x234,  0x212,  0x1f1,  0x1d0,  0x1b1,  0x193,  0x175,  0x159,  0x13d,  0x123,
	0x10a,  0x0f1,  0x0da,  0x0c4,  0x0af,  0x09b,  0x089,  0x077,  0x067,
	0x058,  0x04a,  0x03d,  0x031,  0x027,  0x01e,  0x016,  0x00f,  0x009,  0x005,
	0x002,  0x000,  0x000,  0x000,  0x002,  0x005,  0x009,  0x00f,  0x016,
	0x01e,  0x027,  0x031,  0x03d,  0x04a,  0x058,  0x067,  0x077,  0x089,  0x09b,
	0x0af,  0x0c4,  0x0da,  0x0f1,  0x10a,  0x123,  0x13d,  0x159,  0x175,
	0x193,  0x1b1,  0x1d0,  0x1f1,  0x212,  0x234,  0x257,  0x27b,  0x2a0,  0x2c6,
	0x2ec,  0x314,  0x33c,  0x364,  0x38e,  0x3b8,  0x3e3,  0x40e,  0x43a,
	0x467,  0x494,  0x4c2,  0x4f0,  0x51e,  0x54e,  0x57d,  0x5ad,  0x5dd,  0x60e,
	0x63f,  0x670,  0x6a1,  0x6d3,  0x705,  0x737,  0x769,  0x79b,  0x7cd
};

#define C4 261.63f
#define D4 293.66f
#define E4 329.63f
#define F4 349.23f
#define G4 392.00f
#define A4 440.00f
#define B4 493.88f

const struct {float freq_hz; uint duration_ms;} song[] = {
	{C4    , 500 },
	{D4    , 500 },
	{E4    , 500 },
	{F4    , 500 },
	{G4    , 500 },
	{A4    , 500 },
	{B4    , 500 },
	{C4 * 2, 500 },
	{0     , 1000},
};

static inline void pwm_set_both_channels(uint slice, uint16_t sample) {
	// 16 bit data gets replicated to 32 bits on the bus
	*(io_rw_16*)&pwm_hw->slice[slice].cc = sample;
}

uint audio_pwm_slice;
float note_freq_hz = 440;

void pwm_irq_handler() {
	pwm_clear_irq(audio_pwm_slice);

	// Let Pi = 128 :)
	static float theta = 0;
	theta += note_freq_hz * (256.f / (SAMPLE_CLOCK_KHZ * 1000));
	if (theta >= 256.f)
		theta -= 256.f;

	pwm_set_both_channels(audio_pwm_slice, sine_table[(uint8_t)theta]);
}


int main() {
	vreg_set_voltage(CORE_VOLTAGE);
	sleep_ms(1);
	set_sys_clock_khz(SYS_CLOCK_KHZ, true);

	audio_pwm_slice = pwm_gpio_to_slice_num(PICOSTATION_AUDIO_PWM_R_PIN);
	pwm_config c = pwm_get_default_config();
	pwm_config_set_wrap(&c, SYS_CLOCK_KHZ / SAMPLE_CLOCK_KHZ - 1);
	pwm_init(audio_pwm_slice, &c, false);
	pwm_set_both_channels(audio_pwm_slice, 1u << (SAMPLE_BITS - 1));

	pwm_clear_irq(audio_pwm_slice);
	pwm_set_irq_enabled(audio_pwm_slice, true);
	irq_set_exclusive_handler(PWM_IRQ_WRAP, pwm_irq_handler);


	gpio_set_function(PICOSTATION_AUDIO_PWM_R_PIN, GPIO_FUNC_PWM);
	pwm_set_enabled(audio_pwm_slice, true);
	irq_set_enabled(PWM_IRQ_WRAP, true);
	while (true) {
		for (int note = 0; note < count_of(song); ++note) {
			note_freq_hz = song[note].freq_hz;
			sleep_ms(song[note].duration_ms);
		}
	}
}
