/*
 * FeatherHAB
 *
 * This file is part of FeatherHAB.
 *
 * FeatherHab is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * FeatherHab is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with FeatherHAB. If not, see <http://www.gnu.org/licenses/>.
 *
 * Ethan Zonca
 *
 */

#include "stm32f0xx_hal.h"

#include "si446x.h"
#include "config.h"
#include "error.h"
#include "system/gpio.h"
#include "system/sysclk.h"


// Private variables
static SPI_HandleTypeDef hspi1;
static uint8_t si446x_cw_status = 0;


// Private function prototypes
static void __init_spi1(void);


// Initialize Si446x in 2FSK transmit mode
void si446x_init(void)
{
	// init spi port
	__init_spi1();

	GPIO_InitTypeDef GPIO_InitStruct;

	// GPIO: TCXO control
	GPIO_InitStruct.Pin = SI446x_TCXO_EN_PIN;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(SI446x_TCXO_EN_PORT, &GPIO_InitStruct);
	HAL_GPIO_WritePin(SI446x_TCXO_EN_PORT, SI446x_TCXO_EN_PIN, 1);

	// GPIO: VHF radio shutdown control
	GPIO_InitStruct.Pin = SI446x_SHUTDOWN_PIN;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(SI446x_SHUTDOWN_PORT, &GPIO_InitStruct);

	// Perform PoR (takes 20ms) and turn device on
	si446x_reset();
	si446x_reset();

    // Divide SI446x_VCXO_FREQ into its bytes; MSB first
    uint16_t x3 = SI446x_VCXO_FREQ / 0x1000000;
    uint16_t x2 = (SI446x_VCXO_FREQ - x3 * 0x1000000) / 0x10000;
    uint16_t x1 = (SI446x_VCXO_FREQ - x3 * 0x1000000 - x2 * 0x10000) / 0x100;
    uint16_t x0 = (SI446x_VCXO_FREQ - x3 * 0x1000000 - x2 * 0x10000 - x1 * 0x100);

    // Power up radio module                          boot  xtal  _XO_frequency_

    //TCXO
    const char init_command[] = {SI446x_CMD_POWER_UP, 0x01, 0x01, x3, x2, x1, x0};
    si446x_sendcmd(7, init_command, SI446x_CHECK_ACK);

    HAL_Delay(10);

	// Change to SPI Ready state EMZ added for re-init, might help on startup
	uint8_t change_state_commanda[] = {SI446x_CMD_CHANGE_STATE, 0x02}; //  Change to spi ready
    si446x_sendcmd(2, change_state_commanda, SI446x_CHECK_ACK);
    HAL_Delay(10);

    // Radio ready: clear all pending interrupts and get the interrupt status back
	uint8_t get_int_status_command[] = {SI446x_CMD_GET_INT_STATUS, 0x00, 0x00, 0x00};
    si446x_sendcmd(4, get_int_status_command, SI446x_CHECK_ACK);

    HAL_Delay(10);

    // GPIO config: Set all GPIOs LOW; Link NIRQ to CTS; Link SDO to MISO; Max drive strength
	uint8_t gpio_pin_cfg_command[] = {
    		SI446x_CMD_GPIO_PIN_CFG, // Command
			SI446x_GPIO_LOW,      // GPIO0 - Power amp control DAC AD5611 Sync Pin
			SI446x_GPIO_INPUT,    // GPIO1 - Input for modulation
			SI446x_GPIO_LOW,      // GPIO2 - Blue LED
			SI446x_GPIO_NOCHANGE, // GPIO3 - Unused
			SI446x_GPIO_NOCHANGE, // NIRQ
			SI446x_GPIO_NOCHANGE, // 0x11, // SDO
			SI446x_GPIO_NOCHANGE, // Gencfg
	};
    si446x_sendcmd(8, gpio_pin_cfg_command, SI446x_CHECK_ACK);

    HAL_Delay(10);

//	uint8_t tune_xo_cmd[] = {
//    		SI446x_CMD_SET_PROPERTY,
//			SI446x_XO_TUNE_REGISTER_GROUP,
//			0x1, // num data
//			SI446x_XO_TUNE_REGISTER_PROP,
//			SI446x_CRYSTAL_LOAD_TUNING,
//    };
//    si446x_sendcmd(5, tune_xo_cmd, SI446x_CHECK_ACK);

    HAL_Delay(10);

    // Tune to frequency specified
	si446x_setchannel(TUNE_FREQUENCY);

	HAL_Delay(10);

    // Set to 2FSK mode
    uint8_t modemconfig = SI446x_MOD_TYPE_2FSK | SI446x_MOD_TYPE_SOURCE_DIRECTMODE | SI446x_MOD_TYPE_DIRECT_ASYNCH | SI446x_MOD_TYPE_DIRECT_SOURCE_GPIO1; //SI446x_MOD_TYPE_SOURCE_PACKETHANDLER
    uint8_t set_modem_mod_type_command[] = {
    		SI446x_CMD_SET_PROPERTY,
    		SI446x_MOD_TYPE_REGISTER_GROUP,
			0x01, // num data
			SI446x_MOD_TYPE_REGISTER_PROP,
			modemconfig
    };
    si446x_sendcmd(5, set_modem_mod_type_command, SI446x_CHECK_ACK);

    HAL_Delay(10);

	// Set Si446x initial output power, input to power amp (0-0x7F, 0mW - 40mw?)
	uint8_t basepower = SI446x_POWER;
	// FIXME: basepower should be 0x10 for underperforming units and 0x04 for normal units
    uint8_t set_power_level_command[] = {SI446x_CMD_SET_PROPERTY, 0x22, 0x01, 0x01, basepower};
	si446x_sendcmd(5, set_power_level_command, SI446x_CHECK_ACK);


	HAL_Delay(10);

    // Set air data rate
    si446x_setdatarate();
    HAL_Delay(10);

	// Tune TX
	uint8_t change_state_command[] = {SI446x_CMD_CHANGE_STATE, 0x05}; //  Change to TX tune state
    si446x_sendcmd(2, change_state_command, SI446x_CHECK_ACK);
    HAL_Delay(10);

	si446x_cw_status = 0;
}


// Perform power-on-reset of Si446x. Takes 20ms.
void si446x_reset(void)
{
	si446x_shutdown();
	HAL_Delay(10);
	si446x_wakeup();
	HAL_Delay(10);
}


// Set GPIO pin state on Si446x
void si446x_gpio(uint8_t gpio, uint8_t state, uint8_t doack)
{
	// GPIO invalid
	if(gpio > 7)
		return;

	// Default to not changing any GPIO
	uint8_t gpio_pin_cfg_command[] = {
			SI446x_CMD_GPIO_PIN_CFG, // Command
			SI446x_GPIO_NOCHANGE, // GPIO0 - Power amp control DAC AD5611 Sync Pin
			SI446x_GPIO_NOCHANGE, // GPIO1 - Input for modulation
			SI446x_GPIO_NOCHANGE, // GPIO2 - Blue
			SI446x_GPIO_NOCHANGE, // GPIO3 - Unused
			SI446x_GPIO_NOCHANGE, // NIRQ
			SI446x_GPIO_NOCHANGE, // 0x11, // SDO
			SI446x_GPIO_NOCHANGE, // Gencfg
	};

	// Set requested GPIO to requested state
	gpio_pin_cfg_command[gpio+1] = state;

	si446x_sendcmd(8, gpio_pin_cfg_command, doack);
}


// Set over-air data rate
void si446x_setdatarate(void)
{
    // Set data rate (unsure if this actually affects direct modulation)
                         //        set prop   group     numprops  startprop   data
	uint8_t set_data_rate_command[] = {SI446x_CMD_SET_PROPERTY,     0x20,     0x03,     0x03,       0x0F, 0x42, 0x40};
    si446x_sendcmd(7, set_data_rate_command, SI446x_CHECK_ACK);
}


// Block write data to the Si446x SPI interface, up to 128 byte length
void si446x_senddata(uint8_t* data, uint8_t len)
{
	uint8_t dummy[128];
	SI446x_SELECT;
	HAL_SPI_TransmitReceive(&hspi1, data, dummy, len, SI446x_TIMEOUT);
	SI446x_DESELECT;
}


// Delay approximately 20us
static void delay_cycles(void)
{
	uint32_t delay_cycles = 180;
	while(delay_cycles>0)
	{
		asm("NOP");
		delay_cycles--;
	}
}


// Send a command to the radio (Blocking)
// Avoid calling this during code runtime as it will block for a significant period of time due to delays
void si446x_sendcmd(uint8_t tx_len, uint8_t* data, uint8_t doack)
{
    SI446x_SELECT;

    delay_cycles();

    uint8_t dummyrx[25];
    if(tx_len >=25)
    {
    	SI446x_DESELECT;
    	return;
    }

    // using transmit receive to transmit data because it actually blocks until the data is sent
    // an additional byte is added on to the transmission so we can receive the CTS byte

    volatile HAL_StatusTypeDef res = HAL_SPI_TransmitReceive(&hspi1, data, dummyrx, tx_len+1, SI446x_TIMEOUT);

    if(res != HAL_OK)
    {
    	SI446x_DESELECT;
    	return;
    }

    SI446x_DESELECT;

    // If checking for the ACK, perform a SPI read and see if the command was acknowledged
    if(doack)
    {
		delay_cycles();

		SI446x_SELECT;

		int reply = 0x00;
		uint8_t tx_requestack[2];
		tx_requestack[0] = SI446x_CMD_READ_CMD_BUFF;
		tx_requestack[1] = 0x00;

		volatile uint16_t attempts = 0;

		// Keep trying receive until it returns 0xFF (successful ACK)
		while (reply != 0xFF)
		{
			// Attempt to receive two bytes from the Si446x which should be an ACK
			uint8_t tmprx[2] = {0,0};
			res = HAL_SPI_TransmitReceive(&hspi1, tx_requestack, tmprx, 2, SI446x_TIMEOUT);
			if(res != HAL_OK)
			{
				//error_assert_silent(ERR_VHF_SPIBUSY);
		    	break; // Break out, deinit, and exit
			}
			reply = tmprx[1];

			// Cycle chip select line on and off
			if (reply != 0xFF)
			{
				delay_cycles();
				SI446x_DESELECT;
				delay_cycles();
				SI446x_SELECT;
				delay_cycles();
				//HAL_GPIO_TogglePin(LED_ACT);
			}

			// Maximum number of attempts exceeded
			if(attempts > 1024)
			{
				//error_assert_silent(ERR_VHF_TIMEOUT);
				volatile uint32_t test = 344;
				break; // Break out, deinit and exit
			}
			attempts++;
		}
    }

    // Turn off activity LED
    //HAL_GPIO_WritePin(LED_ACT, GPIO_PIN_RESET);

    SI446x_DESELECT;
    delay_cycles();
}


// Set transmit frequency of Si446x
void si446x_setchannel(uint32_t frequency)
{

    // Set the output divider according to recommended ranges given in si446x datasheet
    uint32_t outdiv = 4;
    uint32_t band = 0;
    if (frequency < 705000000UL) { outdiv = 6;  band = 1;};
    if (frequency < 525000000UL) { outdiv = 8;  band = 2;};
    if (frequency < 353000000UL) { outdiv = 12; band = 3;};
    if (frequency < 239000000UL) { outdiv = 16; band = 4;};
    if (frequency < 177000000UL) { outdiv = 24; band = 5;};

    uint32_t f_pfd = 2 * SI446x_VCXO_FREQ / outdiv;

    uint32_t n = ((uint32_t)(frequency / f_pfd)) - 1;

    float ratio = (float)frequency / (float)f_pfd;
    float rest  = ratio - (float)n;

    uint32_t m = (uint32_t)(rest * 524288UL);

    // Set the band parameter
    uint32_t sy_sel = 8;
    uint8_t set_band_property_command[] = {SI446x_CMD_SET_PROPERTY, 0x20, 0x01, 0x51, (band + sy_sel)};
    si446x_sendcmd(5, set_band_property_command, SI446x_CHECK_ACK);

    // Set the pll parameters
    uint32_t m2 = m / 0x10000;
    uint32_t m1 = (m - m2 * 0x10000) / 0x100;
    uint32_t m0 = (m - m2 * 0x10000 - m1 * 0x100);

    // Assemble parameter string
    uint8_t set_frequency_property_command[] = {SI446x_CMD_SET_PROPERTY, 0x40, 0x04, 0x00, n, m2, m1, m0};
    si446x_sendcmd(8, set_frequency_property_command, SI446x_CHECK_ACK);

    // Set frequency deviation
    // ...empirically 0xF9 looks like about 5khz. Sketchy.

    //                           set prop   group     numprops  startprop   data                     // Was 0x0F 00 for ~40kHz dev, switched to 56khzish? dev

    //2DF5 is correct for 56khz
    uint8_t set_frequency_separation[] = {SI446x_CMD_SET_PROPERTY, 0x20,     0x03,      0x0a,     0x00, 0x03, 0x00};
    si446x_sendcmd(7, set_frequency_separation, SI446x_CHECK_ACK);

}


// Turn CW transmit on
void si446x_cw_on(void)
{
    // Change to TX state
	uint8_t change_state_command[] = {SI446x_CMD_CHANGE_STATE, 0x07};
    si446x_sendcmd(2, change_state_command, SI446x_CHECK_ACK);
    si446x_cw_status = 1;
}


// Turn CW transmit off
void si446x_cw_off(void)
{
    // Change to ready state
	uint8_t change_state_command[] = {SI446x_CMD_CHANGE_STATE, 0x03};
    si446x_sendcmd(2, change_state_command, SI446x_CHECK_ACK);
    si446x_cw_status = 0;
}


// Returns 1 if CW is on or 0 if CW is off
inline uint8_t si446x_tx_status(void)
{
	return si446x_cw_status;
}


// Initialize SPI port for generation of direct modulation for Si446x
static void __init_spi1(void)
{

	GPIO_InitTypeDef GPIO_InitStruct;

	// GPIO: VHF chip select
	GPIO_InitStruct.Pin = SI446x_CS_PIN;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_LOW;
	HAL_GPIO_Init(SI446x_CS_PORT, &GPIO_InitStruct);
	SI446x_DESELECT;

	// SPI pins
	__SPI1_CLK_ENABLE();
	GPIO_InitStruct.Pin = SI446x_SCK_PIN|SI446x_MOSI_PIN|SI446x_MISO_PIN;
	GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
	GPIO_InitStruct.Alternate = GPIO_AF0_SPI1;
	HAL_GPIO_Init(SI446x_SCK_PORT, &GPIO_InitStruct);

	// SPI peripheral
	hspi1.Instance = SPI1;
	hspi1.Init.Mode = SPI_MODE_MASTER;

	hspi1.Init.Direction = SPI_DIRECTION_2LINES;

	hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
	hspi1.Init.CLKPolarity = SPI_POLARITY_LOW; // Double-check, this is usually high, but might be low for this chip
	hspi1.Init.CLKPhase = SPI_PHASE_1EDGE; // was 1edge before (rising edge of clock)
	hspi1.Init.NSS = SPI_NSS_SOFT;
	hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
	hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
	hspi1.Init.TIMode = SPI_TIMODE_DISABLED;
	hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
	hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLED;
	HAL_SPI_Init(&hspi1);
}


// Place the Si446x into shutdown
void si446x_shutdown(void)
{
    HAL_GPIO_WritePin(SI446x_SHUTDOWN, GPIO_PIN_SET);
}


// Wake up the Si446x from shutdown
void si446x_wakeup(void)
{
    HAL_GPIO_WritePin(SI446x_SHUTDOWN, GPIO_PIN_RESET);
}


// Accessor for SPI1  handle
SPI_HandleTypeDef* spi1_get(void)
{
	return &hspi1;
}