/*
 * Sound.c
 *
 *  Created on: Oct 1, 2024
 *      Author: DashyFox
 */

#include "Sound.h"
#include "stm32f103xb.h"
#include "SimpleTimer.h"

static uint32_t bpm = 145;          // Default BPM
static uint32_t note_end_time_ch3 = 0;  // End time for channel 3
static uint32_t note_end_time_ch4 = 0;  // End time for channel 4
static Note_t current_note_ch3;
static Note_t current_note_ch4;

// Initialize the sound system
void sound_init(void) {
    // Enable timer clock
    RCC->APB1ENR |= SOUND_TIMER_RCC;

    // Configure GPIO pins for timer output (assuming GPIOB pins for TIM4 CH3 and CH4)
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;

    if (SOUND_CHANNEL_1 == 1 || SOUND_CHANNEL_1 == 2) {
        GPIOB->CRL &= ~(0xF << ((SOUND_CHANNEL_1 - 1) * 4));  // Clear
        GPIOB->CRL |= (0xB << ((SOUND_CHANNEL_1 - 1) * 4));   // Alternate function push-pull
    } else if (SOUND_CHANNEL_1 == 3 || SOUND_CHANNEL_1 == 4) {
        GPIOB->CRH &= ~(0xF << ((SOUND_CHANNEL_1 - 8) * 4));  // Clear
        GPIOB->CRH |= (0xB << ((SOUND_CHANNEL_1 - 8) * 4));   // Alternate function push-pull
    }

    if (SOUND_CHANNEL_2 == 1 || SOUND_CHANNEL_2 == 2) {
        GPIOB->CRL &= ~(0xF << ((SOUND_CHANNEL_2 - 1) * 4));  // Clear
        GPIOB->CRL |= (0xB << ((SOUND_CHANNEL_2 - 1) * 4));   // Alternate function push-pull
    } else if (SOUND_CHANNEL_2 == 3 || SOUND_CHANNEL_2 == 4) {
        GPIOB->CRH &= ~(0xF << ((SOUND_CHANNEL_2 - 8) * 4));  // Clear
        GPIOB->CRH |= (0xB << ((SOUND_CHANNEL_2 - 8) * 4));   // Alternate function push-pull
    }

    // Initialize the timer for PWM output
    SOUND_TIMER->PSC = 0; // Will be set in sound_play_note()
    SOUND_TIMER->ARR = 0xFFFF; // Will be set in sound_play_note()

    // Configure PWM mode on both channels
    if (SOUND_CHANNEL_1 == 3) {
        SOUND_TIMER->CCMR2 &= ~TIM_CCMR2_OC3M;
        SOUND_TIMER->CCMR2 |= (6 << TIM_CCMR2_OC3M_Pos);  // PWM mode 1 for channel 3
        SOUND_TIMER->CCER |= TIM_CCER_CC3E; // Enable output on channel 3
    } else if (SOUND_CHANNEL_1 == 4) {
        SOUND_TIMER->CCMR2 &= ~TIM_CCMR2_OC4M;
        SOUND_TIMER->CCMR2 |= (6 << TIM_CCMR2_OC4M_Pos);  // PWM mode 1 for channel 4
        SOUND_TIMER->CCER |= TIM_CCER_CC4E; // Enable output on channel 4
    }

    if (SOUND_CHANNEL_2 == 3) {
        SOUND_TIMER->CCMR2 &= ~TIM_CCMR2_OC3M;
        SOUND_TIMER->CCMR2 |= (6 << TIM_CCMR2_OC3M_Pos);  // PWM mode 1 for channel 3
        SOUND_TIMER->CCER |= TIM_CCER_CC3E; // Enable output on channel 3
    } else if (SOUND_CHANNEL_2 == 4) {
        SOUND_TIMER->CCMR2 &= ~TIM_CCMR2_OC4M;
        SOUND_TIMER->CCMR2 |= (6 << TIM_CCMR2_OC4M_Pos);  // PWM mode 1 for channel 4
        SOUND_TIMER->CCER |= TIM_CCER_CC4E; // Enable output on channel 4
    }

    // Generate an update event to reload the prescaler and ARR values
    SOUND_TIMER->EGR = TIM_EGR_UG;

    // Enable the timer
    SOUND_TIMER->CR1 |= TIM_CR1_CEN;
}

// Set BPM
void sound_set_bpm(uint32_t new_bpm) {
    bpm = new_bpm;
}

// Play a note on a specific channel
void sound_play_note(Note_t note, uint8_t channel) {
    if (channel == SOUND_CHANNEL_1) {
        current_note_ch3 = note;
        if (note.frequency == 0) {
            // Pause on channel 3
            SOUND_TIMER->CCER &= ~TIM_CCER_CC3E; // Disable output on channel 3
            note_end_time_ch3 = millis() + note.duration;
        } else {
            // Configure frequency and duty cycle for channel 3
            uint32_t timer_clock = SystemCoreClock / 2; // Assuming APB1 prescaler is 2
            uint32_t prescaler = (timer_clock / (note.frequency * 65536)) + 1;
            uint32_t arr = (timer_clock / (prescaler * note.frequency)) - 1;
            uint32_t ccr = ((arr + 1) * note.duty_cycle) / 100 - 1;

            SOUND_TIMER->PSC = prescaler - 1;
            SOUND_TIMER->ARR = arr;
            SOUND_TIMER->CCR3 = ccr;

            // Configure PWM mode for channel 3
            SOUND_TIMER->CCMR2 &= ~(TIM_CCMR2_OC3M | TIM_CCMR2_OC3PE);
            SOUND_TIMER->CCMR2 |= (TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3PE); // PWM mode 1

            SOUND_TIMER->CCER |= TIM_CCER_CC3E; // Enable output on channel 3
            SOUND_TIMER->EGR = TIM_EGR_UG;      // Update registers
            SOUND_TIMER->CR1 |= TIM_CR1_CEN;    // Enable timer

            if (note.duration == 0) {
                note_end_time_ch3 = 0; // Infinite duration
            } else {
                note_end_time_ch3 = millis() + note.duration;
            }
        }
    } else if (channel == SOUND_CHANNEL_2) {
        current_note_ch4 = note;
        if (note.frequency == 0) {
            // Pause on channel 4
            SOUND_TIMER->CCER &= ~TIM_CCER_CC4E; // Disable output on channel 4
            note_end_time_ch4 = millis() + note.duration;
        } else {
            // Configure frequency and duty cycle for channel 4
            uint32_t timer_clock = SystemCoreClock / 2; // Assuming APB1 prescaler is 2
            uint32_t prescaler = (timer_clock / (note.frequency * 65536)) + 1;
            uint32_t arr = (timer_clock / (prescaler * note.frequency)) - 1;
            uint32_t ccr = ((arr + 1) * note.duty_cycle) / 100 - 1;

            SOUND_TIMER->PSC = prescaler - 1;
            SOUND_TIMER->ARR = arr;
            SOUND_TIMER->CCR4 = ccr;

            // Configure PWM mode for channel 4
            SOUND_TIMER->CCMR2 &= ~(TIM_CCMR2_OC4M | TIM_CCMR2_OC4PE);
            SOUND_TIMER->CCMR2 |= (TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4PE); // PWM mode 1

            SOUND_TIMER->CCER |= TIM_CCER_CC4E; // Enable output on channel 4
            SOUND_TIMER->EGR = TIM_EGR_UG;      // Update registers
            SOUND_TIMER->CR1 |= TIM_CR1_CEN;    // Enable timer

            if (note.duration == 0) {
                note_end_time_ch4 = 0; // Infinite duration
            } else {
                note_end_time_ch4 = millis() + note.duration;
            }
        }
    }
}

// Function to be called periodically to handle note duration
void sound_tick(void) {
    if (note_end_time_ch3 != 0 && millis() >= note_end_time_ch3) {
        // Stop the note on channel 3
        SOUND_TIMER->CCER &= ~TIM_CCER_CC3E; // Disable output on channel 3
        note_end_time_ch3 = 0;
    }
    if (note_end_time_ch4 != 0 && millis() >= note_end_time_ch4) {
        // Stop the note on channel 4
        SOUND_TIMER->CCER &= ~TIM_CCER_CC4E; // Disable output on channel 4
        note_end_time_ch4 = 0;
    }
}

// Create a Note_t from musical note and duration enums
Note_t sound_create_note(MusicalNote_t note_enum, NoteDuration_t duration_enum, uint8_t duty_cycle) {
    Note_t result_note;
    result_note.frequency = note_enum;
    if (duration_enum == 0) {
        result_note.duration = 0; // Infinite duration
    } else {
        // Calculate duration in milliseconds
        uint32_t whole_note_duration = (60000 / bpm) * 4; // Duration of whole note in ms
        result_note.duration = whole_note_duration / duration_enum;
    }
    result_note.duty_cycle = duty_cycle;
    return result_note;
}