#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <BIOS.h>
#include "ds203_io.h"
#include "rms_measurement.h"
#include "mathutils.h"

// Uhh.. in unconditional trigger mode first 150 samples are rubbish.
// This has something to do with trigger position and "PerCnt" in FPGA2.5
// This is visible in default APP 2.50 also, if you set mode to free-running
#define SKIP_SAMPLES 150   
#define ADC_SIZE (4096-SKIP_SAMPLES)

// Measure RMS of channel A voltage.
// Samplerate is selected so that atleast 4 periods fit in the buffer.
// The implementation uses DFT transform, but only calculates it at one
// frequency.
void measure_rms(int frequency, int *peak_to_peak, float *rms, float *phase)
{
    // Samplerate is 72 MHz / (arr + 1)
    // arr is rounded upwards
    int arr = div_round_up(CPUFREQ, frequency * ADC_SIZE / 4) - 1;
    int samplef = CPUFREQ / (arr + 1);
    
    // For DFT to be accurate, select n to be a multiple of the frequency
    int n = ADC_SIZE / (samplef / frequency) * samplef / frequency;
    
    // Configure ADC
    __Set(TRIGG_MODE, UNCONDITION);
    __Set(T_BASE_PSC, 0);
    __Set(T_BASE_ARR, arr);
    __Set(CH_A_OFFSET, 128);
    
    // Wait for generator waveform to restart
    while (DMA2_Channel4->CNDTR != 1);
    while (DMA2_Channel4->CNDTR == 1);
    
    // Now just measure
    __Set(FIFO_CLR, W_PTR);
    while (!__Get(FIFO_FULL));
    
    // Skip some samples to work around the bug..
    for (int i = 0; i < SKIP_SAMPLES; i++)
    {
        __Read_FIFO();
    }
    
    // Now process the results
    int min = 255;
    int max = 0;
    float __complex__ dft = 0;
    float i_multiplier = 2 * (float)M_PI * frequency / samplef;
    for (int i = 0; i < n; i++)
    {
        int value = __Read_FIFO() & 0xFF;
        
        if (value > max) max = value;
        if (value < min) min = value;
        
        // Calculate DFT transform at the given frequency.
        if (phase != NULL || rms != NULL)
        {
            dft += value * (cosf(i_multiplier * i) - 1.0fi * sinf(i_multiplier * i));
        }
    }
    
    *peak_to_peak = max - min;
    
    dft /= n / 2;
    float abs = sqrtf((__real__ dft) * (__real__ dft) + (__imag__ dft) * (__imag__ dft));
    
    if (rms != NULL) *rms = abs / sqrtf(2);
    if (phase != NULL)
    {
        if (abs > 2.0f)
            *phase = atan2f((__real__ dft), -(__imag__ dft)) * 180.0f / M_PI;
        else
            *phase = NAN;
    }
}

// Tries different ADC ranges with measure_rms until the signal is suitably
// scaled.
int autodetect_range(int frequency)
{
    int range, peak_to_peak;
    
    // Scan from highest sensitivity to lowest
    for (range = 0; range < ADC_RANGE_COUNT; range++)
    {
        __Set(CH_A_RANGE, range);
        DelayMs(2);
        measure_rms(frequency, &peak_to_peak, NULL, NULL);
        
        if (peak_to_peak < 200)
            break;
    }
    return range;
}

// Uses measure_rms, but also converts results to volts and automatically
// adjusts ADC range.
float measure_vrms(int frequency, float *phase)
{
    static int range = ADC_1V;
    int peak_to_peak;
    float rms;
    measure_rms(frequency, &peak_to_peak, &rms, phase);
    
    if (peak_to_peak < 20 || peak_to_peak > 230)
    {
        range = autodetect_range(frequency);
        measure_rms(frequency, &peak_to_peak, &rms, phase);
    }
    
    // Get scaling info from BIOS
    Y_attr* YATTR = (void*)__Get(VERTICAL);
    int scale = YATTR[range].SCALE;
    
    float vrms = (rms * scale) / 1000000.0f;
    return vrms;
}