/* Title: demodulatore */

#include <stdint.h>
#include <math.h>

#ifdef __XC16
#include <dsp.h>
#include <libq.h>
#endif 

#include "../phy/phy.h" // a chi cazzo serve?

#include "../peripheral/adc.h"
#include "../peripheral/gpio.h"

// serve? direi di si
#include "modem_types.h"

#include "demodulator.h"

// buffer dei simboli ricevuti
uint8_t rx_symbol_buffer[MAX_SYMBOL_BUFFER_SIZE];
uint8_t rx_symbol_buffer_index, rx_symbol_buffer_lenght;

uint8_t rx_sample_buffers_buffer[MAX_BUFFERS_PER_SYMBOL], rx_sample_buffer_index;

uint8_t volatile process_input_sample_buffer;

detector_symbol detector_constellation[MAX_MODULATION_ALPHABET_SIZE], symbol_error, min_symbol_distance;

// zona del simbolo da esaminare:
uint8_t rx_symbol_detected_part_offset, rx_symbol_detected_part_length;

// NCO
fractional tanlock_nco_rotor_inc;

// #####################################################

void demodulator_setup(void) {
    // RX
    set_rx_GPIOs();

    // setup rotore NCO demodulatore, posizionalo al centro della costellazione
    tanlock_nco_rotor_inc = get_angular_frequency(find_constellation_center_values(mod.constellation).frequency);

    demodulator_prepare_constellation_symbols(mod.constellation, detector_constellation);
    min_symbol_distance = demodulator_find_min_symbol_distance(detector_constellation);

    // zona del simbolo da esaminare:
    // due quarti centrali del simbolo, da verificare
    // si lavora solo su mezzo simbolo, ma si regge una sfasatura di +-1/4 di simbolo
    rx_symbol_detected_part_offset = mod.buffers_per_symbol / 4;
    rx_symbol_detected_part_length = mod.buffers_per_symbol / 2;
}

// trova valori medii dei parametri di modulazione

symbol find_constellation_center_values(symbol *constellation) {
    uint8_t i;
    symbol max, min, center;
    max.amplitude = max.frequency = max.phase = 0;
    min.frequency = min.phase = min.amplitude = 0xFFFF;

    for (i = 0; i < mod.alphabet_size; i++) {
        if (constellation[i].frequency < min.frequency) min.frequency = constellation[i].frequency;
        if (constellation[i].frequency > max.frequency) max.frequency = constellation[i].frequency;

        if (constellation[i].phase < min.phase) min.phase = constellation[i].phase;
        if (constellation[i].phase > max.phase) max.phase = constellation[i].phase;

        if (constellation[i].amplitude < min.amplitude) min.amplitude = constellation[i].amplitude;
        if (constellation[i].amplitude > max.amplitude) max.amplitude = constellation[i].amplitude;

    }
    center.frequency = min.frequency + ((max.frequency - min.frequency) / 2);
    center.phase = min.phase + ((max.phase - min.phase) / 2);
    center.amplitude = min.amplitude + ((max.amplitude - min.amplitude) / 2);
    return center;
}

int16_t get_angular_frequency(uint16_t frequency) {
    int32_t tmpfreq = (int32_t) frequency << 16;
    return __builtin_divsd(tmpfreq, SAMPLING_RATE);
}

void FIR_init(void) {
#ifdef __DSP_LIB__
    // inizializza filtri passa basso
    FIRDelayInit(&ph_det_lpFilterQ);
    FIRDelayInit(&ph_det_lpFilterI);
    FIRDelayInit(&ph_det_lpFilterFreq);
#endif
}

void demodulator_sample_buffer_demodulate(fractional *buffer, detector_symbol* outsymbol) {
    uint8_t i;
    fractional demodulator_frequency_diff = 0, demodulator_phase_diff = 0;
    static fractional freq_lp[1];
    _Q15_16 frequency_accu = 0, phase_accu = 0, amplitude_accu = 0;

    for (i = 0; i < SAMPLES_PER_BUFFER; i++) {
        // ampiezza
        amplitude_accu += _Q15abs(buffer[i]);

        // fase
        demodulator_phase_diff = demodulator_phase_detector(buffer[i]);
        phase_accu += demodulator_phase_diff;

        // frequenza
        demodulator_frequency_diff = demodulator_frequency_detector(demodulator_phase_diff);

#ifdef __DSP_LIB__
        FIR(1, &freq_lp[0], &demodulator_frequency_diff, &ph_det_lpFilterFreq);
#endif
        frequency_accu = frequency_accu + freq_lp[0];
    }

    // media
    outsymbol->amplitude = __builtin_divsd(amplitude_accu, SAMPLES_PER_BUFFER);
    outsymbol->phase = __builtin_divsd(phase_accu, SAMPLES_PER_BUFFER);
    outsymbol->frequency = __builtin_divsd(frequency_accu, SAMPLES_PER_BUFFER);
}

fractional demodulator_phase_detector(fractional sig_in) {
    static fractional tanlock_nco_rotor_pos;
    static fractional Q_mix_lp[1], I_mix_lp[1];
    // MIX
    fractional I_mix, Q_mix, phasediff;
    // moltiplica per segnale iniettato 171 cicli

    // perche' 15? perche i campioni sono da 12 bit
    I_mix = _Q16shr((__builtin_mulss(sig_in, _Q15sinPI(tanlock_nco_rotor_pos))), 16);
    Q_mix = _Q16shr((__builtin_mulss(sig_in, _Q15cosPI(tanlock_nco_rotor_pos))), 16);

#ifdef __DSP_LIB__
    // filtra l'i/q 270 cicli
    FIR(1, &I_mix_lp[0], &I_mix, &ph_det_lpFilterI);
    FIR(1, &Q_mix_lp[0], &Q_mix, &ph_det_lpFilterQ);
#endif 

    // incrementa il rotore dell'oscillatore locale
    tanlock_nco_rotor_pos += tanlock_nco_rotor_inc; // non deve saturare!

    // calcola arcotangente, circa 420 cicli
    phasediff = _Q15atan2circle(I_mix_lp[0], Q_mix_lp[0]);
    //  phasediff = _Q15atan2circle(I_mix, Q_mix);

    return phasediff;
}

fractional demodulator_frequency_detector(fractional phase) {
    static fractional demodulator_last_phase, demodulator_last_delta;
    fractional demodulator_phase_delta = 0, demodulator_phase_delta_tmp = 0;

    demodulator_phase_delta_tmp = _Q15sub(phase, demodulator_last_phase);

    // antiskew
    if (_Q15abs(demodulator_phase_delta_tmp) < MAX_PHASE_SKEW) demodulator_phase_delta = demodulator_phase_delta_tmp;
    else {
        demodulator_phase_delta = demodulator_last_delta;
    }

    // storici
    demodulator_last_phase = phase;
    demodulator_last_delta = demodulator_phase_delta;

    return demodulator_phase_delta;
}

// ####### symbolame ##########

void demodulator_prepare_constellation_symbols(symbol *constellation, detector_symbol *demod_constellation) {
    uint8_t i;
    for (i = 0; i < mod.alphabet_size; i++) {
        demod_constellation[i].frequency = _Q15sub(get_angular_frequency(constellation[i].frequency), tanlock_nco_rotor_inc);
        demod_constellation[i].phase = constellation[i].phase;
        demod_constellation[i].amplitude = constellation[i].amplitude;
    }
}

detector_symbol demodulator_find_min_symbol_distance(detector_symbol *demod_constellation) {
    // find minimum distance between symbols
    uint8_t i, k;
    detector_symbol tmp_distance, min_distance;
    min_distance.frequency = min_distance.phase = min_distance.amplitude = 0x7FFF;
    for (i = 0; i < mod.alphabet_size; i++) {
        for (k = 0; k < mod.alphabet_size; k++) {
            if (k != i) {
                tmp_distance.frequency = _Q15abs(_Q15sub(demod_constellation[i].frequency, demod_constellation[k].frequency));
                if (tmp_distance.frequency < min_distance.frequency) min_distance.frequency = tmp_distance.frequency;

                tmp_distance.phase = _Q15sub(demod_constellation[i].phase, demod_constellation[k].phase);
                tmp_distance.amplitude = _Q15sub(demod_constellation[i].amplitude, demod_constellation[k].amplitude);
            }
        }
    }
    return min_distance;
}

// trova il simbolo piu vicino nella costellazione

uint8_t demodulator_find_nearest_symbol(detector_symbol *in_symbol, detector_symbol *target_constellation, detector_symbol *moderror) {
    detector_symbol distance, min_distance = {0x7FFF, 0x7FFF, 0x7FFF};
    uint8_t i, nearest_symbol = 0;
    for (i = 0; i < mod.alphabet_size; i++) {
        if (mod.frequency_keying) {
            distance.frequency = _Q15abs(_Q15sub(target_constellation[i].frequency, in_symbol->frequency));
            if (distance.frequency < min_distance.frequency) {
                min_distance.frequency = distance.frequency;
                nearest_symbol = i;
                moderror->frequency = distance.frequency;
            }
        }
    }

    return nearest_symbol;
}

// trova deviazione dal simbolo

uint8_t find_symbol_deviation(uint8_t *serie, uint8_t mean, uint8_t len) {
    int16_t sum_deviation = 0;
    uint8_t i;
    for (i = 0; i < len; ++i)
        sum_deviation += (serie[i] - mean)*(serie[i] - mean);
    return sqrt(sum_deviation / len);
}
// valore ricorrente

uint8_t demodulator_most_frequent_value(uint8_t *serie, uint8_t range, uint8_t len) {
    uint8_t i, prezzemolo = 0, prezzemolo_occurrences = 0;
    uint8_t occurrencez[MAX_MODULATION_ALPHABET_SIZE] = {0};
    // conta ricorrenze per ogni simbolo
    for (i = 0; i < len; i++) {
        occurrencez[serie[i]]++;
    }

    // trova il simbolo con la ricorrenza maggiore
    for (i = 0; i < range; i++) {
        if (occurrencez[i] > prezzemolo_occurrences) {
            prezzemolo = i;
            prezzemolo_occurrences = occurrencez[i];
        }
    }
    return prezzemolo;
}

fractional _Q15atan2circle(fractional Y, fractional X) {
    // ARCTAN a cerchio pieno
    fractional atang = 0, offset = 0, temp = 0;
    fractional X_sign = X;

    if (Y <= 0) {
        temp = Y;
        Y = X;
        X = _Q15neg(temp);
        offset = _Q15add(offset, 16384);
    }

    if (_Q15abs(Y) <= _Q15abs(X)) {
        if (X_sign < 0) {
            temp = _Q15add(X, Y);
            Y = _Q15sub(Y, X);
        } else {
            temp = _Q15sub(X, Y);
            Y = _Q15add(Y, X);
        }

        X = temp;
        offset = _Q15add(offset, 8192);
    }
    // circa 300 cy
#ifdef _libq_h_
    atang = _Q15atanYByXByPI(Y, X); // vuole il large code model ? no! 2.2.2 del man
#endif
    if (atang < 0) {
        atang = _Q15sub(atang, offset);
    } else {
        atang = _Q15add(atang, offset);
    }
    return atang;
}

// devel
/*
fractional delay_phase_detector(fractional sig_in) {
    fractional phased_sig_in, phasediff;
    phased_sig_in = delay(sig_in, 2, delay_buffer);
    phasediff = _Q15atan2circle(phased_sig_in, sig_in);
    return phasediff;
}
 */