#include <pico/stdlib.h>
#include <pico/stdio_usb.h>

#include <hardware/pwm.h>

#include <stdio.h>

#define IR_BIAS_PIN 3
#define IR_RX_PIN 6
#define IR_FB_PIN 7

#define HP_GND_PIN 13
#define HP_LEFT_PIN 14
#define HP_RIGHT_PIN 15

#define HP_GND_SLICE 6
#define HP_GND_CHAN PWM_CHAN_B

#define HP_LR_SLICE 7

#define HP_PWM_RATE (16 * 48 * 1000)
#define HP_PWM_SCALE (SYS_CLK_HZ / HP_PWM_RATE)
#define HP_PWM_MID (HP_PWM_SCALE / 2)

int main()
{
	stdio_usb_init();

	for (int i = 0; i < 30; i++) {
		if (stdio_usb_connected())
			break;

		sleep_ms(100);
	}

	printf("\nWelcome to PDM Test!\n");

	gpio_disable_pulls(HP_GND_PIN);
	gpio_disable_pulls(HP_LEFT_PIN);
	gpio_disable_pulls(HP_RIGHT_PIN);

	gpio_set_function(HP_GND_PIN, GPIO_FUNC_PWM);
	gpio_set_function(HP_LEFT_PIN, GPIO_FUNC_PWM);
	gpio_set_function(HP_RIGHT_PIN, GPIO_FUNC_PWM);

	gpio_set_drive_strength(HP_GND_PIN, GPIO_DRIVE_STRENGTH_2MA);
	gpio_set_drive_strength(HP_LEFT_PIN, GPIO_DRIVE_STRENGTH_2MA);
	gpio_set_drive_strength(HP_RIGHT_PIN, GPIO_DRIVE_STRENGTH_2MA);

	/* LR at 48 kHz */
	pwm_config pc = pwm_get_default_config();
	pwm_config_set_clkdiv_int(&pc, 1);
	pwm_config_set_phase_correct(&pc, false);
	pwm_config_set_wrap(&pc, HP_PWM_SCALE - 1);
	pwm_init(HP_LR_SLICE, &pc, false);

	/* GND at high rate, 50:50 for virtual ground */
	pwm_config_set_wrap(&pc, 1);
	pwm_init(HP_GND_SLICE, &pc, false);
	pwm_set_chan_level(HP_GND_SLICE, HP_GND_CHAN, 1);

	/* Start at 50:50. */
	pwm_set_both_levels(HP_LR_SLICE, HP_PWM_MID, HP_PWM_MID);

	/* Start both */
	pwm_set_enabled(HP_GND_SLICE, true);
	pwm_set_enabled(HP_LR_SLICE, true);

	while (true) {
		for (int i = 0; i < 50; i++) {
			sleep_us(2272);
			pwm_set_both_levels(HP_LR_SLICE, HP_PWM_MID + 1, HP_PWM_MID + 1);

			sleep_us(2272);
			pwm_set_both_levels(HP_LR_SLICE, HP_PWM_MID - 1, HP_PWM_MID - 1);
		}

		for (int i = 0; i < 55; i++) {
			sleep_us(2020);
			pwm_set_both_levels(HP_LR_SLICE, HP_PWM_MID + 1, HP_PWM_MID + 1);

			sleep_us(2020);
			pwm_set_both_levels(HP_LR_SLICE, HP_PWM_MID - 1, HP_PWM_MID - 1);
		}

		sleep_ms(500);
	}
}