#include <math.h>
#include <BIOS.h>
#include "ds203_io.h"
#include "signal_generator.h"
#include "mathutils.h"

// Fill a sine table using float math
// Not very fast, but fast enough
void generate_sine(uint16_t *buffer, int size, int repeats)
{
    const float multiplier = (2 * (float)M_PI) * repeats / size;
    float angle = 0.0f;
    for (int i = 0; i < size; i++)
    {
        float value = sinf(angle);
        buffer[i] = (uint16_t)(value * 2047 + 2047);
        
        // Excessive angles slow down sinf, try to keep it small
        angle += multiplier;
        if (angle > 2*(float)M_PI)
            angle -= 2*(float)M_PI;
    }
}

#define SINE_SIZE 8000
static uint16_t sine_table[SINE_SIZE];

// Configure wave out to give sine wave at defined frequency
int set_sine_frequency(int frequency)
{
    // Default DAC timer prescaler in SYS1.50 is 20x.
    // However, there is no point in this as we can get much more
    // accurate frequencies using 1x prescaler.
    TIM7->PSC = 0;
    
    // DAC maximum sample frequency is 1MHz, so for highest frequencies
    // we need more repeats.
    uint32_t repeats = div_round_up((uint32_t)SINE_SIZE * frequency, 1000000);
    uint32_t samplef = (uint32_t)SINE_SIZE * frequency / repeats;
    
    // Round samplef downwards to the actual value we can get using
    // the prescaler.
    // real sample freq is 72 MHz / (ARR + 1)
    // arr is rounded upwards so that we do not exceed SINE_SIZE.
    int arr = div_round_up(CPUFREQ, samplef) - 1;
    if (arr > 65535) arr = 65535;
    samplef = CPUFREQ / (arr + 1);
    
    // Now, generate the sine table
    int sine_count = div_round(samplef * repeats, frequency);
    
    if (sine_count > SINE_SIZE)
        sine_count = SINE_SIZE;
    
    generate_sine(sine_table, sine_count, repeats);
    
    // Have to disable DMA2 Channel4 to set ANALOG_CNT
    // This is a bug in SYS1.50
    DMA2_Channel4->CCR &= ~DMA_CCR1_EN;
    __Set(ANALOG_CNT, sine_count);
    __Set(ANALOG_PTR, (u32)sine_table);
    DMA2_Channel4->CCR |= DMA_CCR1_EN;
    
    // Now set ARR and we are done
    __Set(ANALOG_ARR, arr);
    
    // Return actual frequency
    return div_round(samplef * repeats, sine_count);
}

int set_square_frequency(int frequency)
{
    int divider = div_round_up(CPUFREQ, frequency);
    if (divider % 2 != 0) divider--;
    
    __Set(DIGTAL_PSC, 0);
    __Set(DIGTAL_ARR, divider - 1);
    __Set(DIGTAL_CCR, divider / 2);
    
    return div_round(CPUFREQ, divider);
}