#include <stdint.h>
#include <arm_math.h>
#include <stm32g4xx_ll_cordic.h>

#include "bassofono.h"
#include "tx.h"

q31_t tx_nco1_increment;
uint8_t tx_signal, tx_signal_last;

// TX ADC
volatile uint8_t tx_adc_buffer_pronto;
uint16_t tx_adc_buffer[TX_ADC_DIMENSIONE_BUFFER];

// TX filtro audio
arm_fir_instance_q31 tx_filtro_audio_struttura;
q31_t tx_filtro_audio_stato[TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO + TX_FILTRO_AUDIO_NUMERO_TAP - 1];
int32_t tx_filtro_audio_coefficienti[TX_FILTRO_AUDIO_NUMERO_TAP];

// TX IF
q31_t tx_audio_buffer_fitrato[TX_ADC_DIMENSIONE_BUFFER], tx_I_buffer[TX_ADC_DIMENSIONE_BUFFER], tx_Q_buffer[TX_ADC_DIMENSIONE_BUFFER];

// TX DAC
volatile uint8_t tx_dac_buffer_mezzo_vuoto, tx_dac_buffer_toggle;
q31_t tx_dac_buffer[2][TX_DAC_BUFFER_SIZE];

// FILTRO
//arm_fir_instance_q31 tx_audio_filter_I_struct, tx_audio_filter_Q_struct;
// q31_t tx_audio_filter_I_state[TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO + TX_FILTRO_AUDIO_NUMERO_TAP - 1];
// q31_t tx_audio_filter_Q_state[TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO + TX_FILTRO_AUDIO_NUMERO_TAP - 1];


// =============

void trasmetti(void){
	if(TX_TYPE == TX_TYPE_DAC){
		if(tx_adc_buffer_pronto){
//			HAL_GPIO_WritePin(GPIO1_GPIO_Port, GPIO1_Pin, GPIO_PIN_SET);
			tx_filtra_audio(tx_audio_buffer_fitrato, TX_ADC_DIMENSIONE_BUFFER, tx_adc_buffer);
			if (canale.modulazione == MOD_DC) modulatore_dc(tx_I_buffer, tx_Q_buffer, TX_ADC_DIMENSIONE_BUFFER, tx_audio_buffer_fitrato);
			else if(canale.modulazione == MOD_LSB || canale.modulazione == MOD_USB) modulatore_ssb(tx_I_buffer, tx_Q_buffer, TX_ADC_DIMENSIONE_BUFFER, tx_audio_buffer_fitrato, NCO2_INCREMENT);
		        else if (canale.modulazione == MOD_AM) modulatore_am(tx_I_buffer, tx_Q_buffer, TX_ADC_DIMENSIONE_BUFFER, tx_audio_buffer_fitrato);
                        else if (canale.modulazione == MOD_CWL || canale.modulazione == MOD_CWU) modulatore_cw(tx_I_buffer, TX_ADC_DIMENSIONE_BUFFER);
			tx_adc_buffer_pronto = 0;
//		   	HAL_GPIO_WritePin(GPIO1_GPIO_Port, GPIO1_Pin, GPIO_PIN_RESET);
                }
		if(tx_dac_buffer_mezzo_vuoto){
			tx_mixer(tx_dac_buffer[tx_dac_buffer_toggle], TX_DAC_BUFFER_SIZE, tx_I_buffer, tx_Q_buffer, tx_nco1_increment);
                        tx_dac_buffer_mezzo_vuoto = 0;
                }
        }
	// genera tono CW
        if(rx_dac_buffer_mezzo_vuoto && (canale.modulazione == MOD_CWL || canale.modulazione == MOD_CWU)){
                genera_tono(rx_dac_buffer[rx_dac_buffer_toggle], RX_DAC_BUFFER_SIZE);
                rx_dac_buffer_mezzo_vuoto = 0;
        }
}

// trasforma da uint12 a q31 e filtra audio in ingresso
void tx_audio_filter_init(void){
	arm_fir_interpolate_init_q15 (&tx_audio_filter_I_struct, TX_FILTRO_AUDIO_FATTORE_INTERPOLAZIONE, TX_FILTRO_AUDIO_NUMERO_TAP, tx_audio_filter_coeffs, &tx_audio_filter_I_state[0], TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO);
	arm_fir_interpolate_init_q15 (&tx_audio_filter_Q_struct, TX_FILTRO_AUDIO_FATTORE_INTERPOLAZIONE, TX_FILTRO_AUDIO_NUMERO_TAP, tx_audio_filter_coeffs, &tx_audio_filter_Q_state[0], TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO);
}

void tx_filtro_audio_inizializza(void){
        arm_fir_init_q31 (&tx_filtro_audio_struttura, TX_FILTRO_AUDIO_NUMERO_TAP, tx_filtro_audio_coefficienti, &tx_filtro_audio_stato[0], TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO);
}


void tx_filtra_audio(q31_t *out, uint16_t dimensione, uint16_t *in){
	uint16_t indice;
	q31_t tmp_buffer[TX_ADC_DIMENSIONE_BUFFER];
	
	while(indice < dimensione){
		tmp_buffer[indice] = ((int32_t)in[indice] - 2048) * 2;
		indice++;
	}
	arm_fir_q31(&tx_filtro_audio_struttura, tmp_buffer, out, TX_FILTRO_AUDIO_DIMENSIONE_BLOCCO);
}

// miscela I&Q con NCO
void tx_mixer(q31_t *out, uint16_t dimensione_out, q31_t *I_in, q31_t *Q_in, int32_t incremento_nco){
        uint16_t indice_out = 0;
        uint16_t indice_IQ = 0;
        int32_t sin, cos, tmp;
        static int32_t accumulatore_fase;
	// perche?
        LL_CORDIC_WriteData(CORDIC, accumulatore_fase);
	// sovracampiona I e Q
	// arm_fir_interpolate_q31 prende troppa memoria
//       	arm_fir_interpolate_q31(&tx_audio_filter_I_struct, I_in, I_tmp, TX_AUDIO_FILTER_BLOCK_SIZE);
//	arm_fir_interpolate_q31(&tx_audio_filter_Q_struct, Q_in, Q_tmp, TX_AUDIO_FILTER_BLOCK_SIZE);

        while(indice_out < dimensione_out){
        	indice_IQ = indice_out/TX_FILTRO_RF_FATTORE_INTERPOLAZIONE; // indice IQ ingresso va da 0 a 64

                tmp = LL_CORDIC_ReadData(CORDIC);
                accumulatore_fase += incremento_nco;
                LL_CORDIC_WriteData(CORDIC, accumulatore_fase);
                sin = (int16_t)tmp;
                cos = tmp>>16;
			// out[indice_out] = indice_out;
//			out[indice_out] = (cos>>4) + 2048;
//			out[indice_out] = (sin/16);
			// out[indice_out] = I_in[indice_IQ];
			out[indice_out] = ((I_in[indice_IQ] * sin) + (Q_in[indice_IQ] * cos))/0x1000;

		// interpolazione grezza, da rifare
//		out[indice_out] = (((I_in[indice_IQ] * sin) + (Q_in[indice_IQ] * cos)) >> 20) + 2048;
                indice_out++;
        }
        tmp = LL_CORDIC_ReadData(CORDIC);
}

// modulatori

void modulatore_dc(q31_t *out_I, q31_t *out_Q, uint16_t dimensione, q31_t *in){
	uint16_t indice = 0;
        static double angle;
	while(indice < dimensione){
//	        angle += 0.15;
//		if(angle >= 6.28) angle = 0;

		out_I[indice] = in[indice];
		out_Q[indice] = 0;
//		out[indice] = indice * 32;
//		out[indice] = (int32_t)sin(indice) * 2000;
//		out[indice] = 0;
                indice++;
        }
}


void modulatore_ssb(q31_t *out_I, q31_t *out_Q, uint16_t dimensione, q31_t *in, q31_t nco_freq){
        uint16_t i;
        static int32_t accumulatore_fase;
        int32_t sin, cos, tmp;
        LL_CORDIC_WriteData(CORDIC, accumulatore_fase);
        i = 0;
        while(i < dimensione){
                tmp = LL_CORDIC_ReadData(CORDIC);
                accumulatore_fase += nco_freq;
                LL_CORDIC_WriteData(CORDIC, accumulatore_fase);
                sin = (int16_t)tmp;
                cos = tmp>>16;
                out_I = sat_mult_q31(sin, in[i]);
                out_Q = sat_mult_q31(cos, in[i]);
                i++;
        }
        tmp = LL_CORDIC_ReadData(CORDIC);

}

void modulatore_am(q31_t *out_I, q31_t *out_Q, uint16_t dimensione, q31_t *in){
        // SQRT (I * I + Q * Q)
        uint16_t i;
        i = 0;
        while(i < dimensione){
                out_I[i] = in[i];
                out_Q[i] = 0;
                i++;
        }
}

void modulatore_fm(q31_t *out_I, q31_t *out_Q, uint16_t dimensione, q31_t *in){

}

void modulatore_cw(q31_t *out, uint16_t dimensione){
	uint16_t indice = 0;
	while(indice < dimensione){
		out[indice] = 0x80000000;
	}
}


uint16_t u12_sine(void){
        static double angle;
        angle += CW_TONE_INCREMENT;
         if(angle >= 6.28) angle = 0;
        return (uint16_t)((sin(angle) * CW_TONE_VOLUME) + 0x7ff);
}

void genera_tono(q31_t *buffer, uint16_t buffer_dimensione){
	uint16_t i;
	while(i < buffer_dimensione){
		buffer[i] = u12_sine();
		i++;
	}
}


uint8_t measure_log_abs_mean_tx(q31_t *samples, uint16_t dimensione){
        int32_t measured_signal = 0;
        uint16_t index = 0;
        uint8_t log_sig = 31;

        q31_t abs;
        while(index < dimensione){
		samples[index] = (int32_t)samples[index]-2048;
                abs = (samples[index] > 0) ? samples[index] : (q31_t)__QSUB(0, samples[index]);
                measured_signal += (abs >> 6);
                index++;
        }
        while (log_sig) {
          if((measured_signal >> log_sig) & 1) break;
          log_sig--;
        }
        return log_sig;
}

void set_tx_nco1_freq(int32_t freq){
        if(TX_TYPE == TX_TYPE_DAC){
	        int64_t tmp_increment;
        	if(canale.modulazione == MOD_USB || canale.modulazione == MOD_CWU) freq += (TX_NCO_FREQUENCY);
	        else if(canale.modulazione == MOD_LSB || canale.modulazione == MOD_CWL) freq -= (TX_NCO_FREQUENCY);
        	tmp_increment = ((int64_t)freq * 0x100000000LL)/TX_DAC_SAMPLE_RATE;
		// perche >>1 ?
	        tx_nco1_increment = (int32_t)tmp_increment>>1;
        } else if(TX_TYPE == TX_TYPE_SI5351){
                si5351_set_frequency(canale.frequenza, 0);
                si5351_set_frequency(canale.frequenza, 1);
        }
}