/*
 * afsk_modulator.c
 *
 *  Created on: Oct 10, 2019
 *      Author: curiousmuch
 */

#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "sdkconfig.h"

/* LUT Defines */
#define LUT_SIZE 256		// was 7 bits, but increase to 8 for more phase resolution...
#define DAC_MAX 64			// cc1200 DAC is only 7 bits (+/- 64)

/* Local Global Variables */
static float delta_phi, delta_phi_1, delta_phi_2;		// phase deltas for two tones used in AFSK modulation
static float phase = 0;					// current phase as AFSK modulator must be phase continous
static uint8_t phase_i = 0;				// integer of phase which is used by LUT
static DRAM_ATTR int8_t LUT[LUT_SIZE];		// look up table used for tone generation

/* Private Functions */
// Returns the phase for the LUT based on the current symbol being
// sent
static uint8_t IRAM_ATTR afsk_get_phase(uint8_t tone)
{
	if ( tone )
		delta_phi = delta_phi_1;
	else
		delta_phi = delta_phi_2;

    phase_i = (uint8_t)phase;        // get integer part of our phase

    phase += delta_phi;              // increment phase

    if (phase >= (float)LUT_SIZE)    // handle wraparound
        phase -= (float)LUT_SIZE;

    return phase_i;
}

/* Public Functions */
// This function calculates the phase increment based on the sample rate for
// the two freqs used for AFSK. In addition, the function setups a LUT for
// tone generation
void afsk_mod_init(uint32_t sample_rate, uint32_t freq0, uint32_t freq1)
{
	// calculate phase deltas for 1200 and 2200 frequencies
	delta_phi_1 = (float) freq0 / sample_rate * LUT_SIZE;
	delta_phi_2 = (float) freq1 / sample_rate * LUT_SIZE;

	// calculate LUT
	uint32_t i=0;
	for (i=0; i<LUT_SIZE; ++i)
	{
		LUT[i] = (int8_t)roundf(DAC_MAX * sinf(2.0f * M_PI * (float)i / LUT_SIZE));
	}
}

// returns the amplitude for the current tone, but must be called
// based on the sample frequency
int8_t IRAM_ATTR afsk_get_amplitude(uint8_t tone)
{
	return LUT[afsk_get_phase(tone)];
}