/*
	Copyright 2016 - 2022 Benjamin Vedder	benjamin@vedder.se
	Copyright 2022 Zach O'Brien

	This file is part of the VESC firmware.

	The VESC firmware is free software: you can redistribute it and/or modify
	 it under the terms of the GNU General Public License as published by
	 the Free Software Foundation, either version 3 of the License, or
	 (at your option) any later version.

	 The VESC firmware 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 General Public License for more details.

	 You should have received a copy of the GNU General Public License
	 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "enc_as5x47u.h"

#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
#include "mc_interface.h"
#include "utils_math.h"
#include "spi_bb.h"
#include "timer.h"

#include <string.h>
#include <math.h>

#define AS5x47U_SPI_READ_BIT 								0x4000
#define AS5x47U_SPI_WRITE_BIT 								0x0000

#define AS5x47U_SPI_DIAG_FUSA_ERROR_BIT_POS					10
#define AS5x47U_SPI_DIAG_COF_BIT_POS						2
#define AS5x47U_SPI_DIAG_COMP_LOW_BIT_POS					3
#define AS5x47U_SPI_DIAG_COMP_HIGH_BIT_POS					4

#define AS5x47U_SPI_ERRFL_WDTST_BIT_POS						7
#define AS5x47U_SPI_ERRFL_CRC_ERROR_BIT_POS					6
#define AS5x47U_SPI_ERRFL_MAG_HALF_BIT_POS					1

#define AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK		0x3FFF
#define AS5x47U_SPI_AGC_MASK								0xFF
#define AS5x47U_SPI_WARN_FLAG_MASK							0x8000
#define AS5x47U_SPI_ERROR_FLAG_MASK							0x4000

#define AS5x47U_SPI_ERRFL_ADR								0x0001
#define AS5x47U_SPI_DIAG_ADR								0x3FF5
#define AS5x47U_SPI_MAGN_ADR								0x3FFD
#define AS5x47U_SPI_AGC_ADR									0x3FF9
#define AS5x47U_SPI_POS_ADR									0x3FFF

#define AS5x47U_SPI_READ_ERRFL_MSG			(AS5x47U_SPI_ERRFL_ADR | AS5x47U_SPI_READ_BIT)
#define AS5x47U_SPI_READ_DIAG_MSG			(AS5x47U_SPI_DIAG_ADR  | AS5x47U_SPI_READ_BIT)
#define AS5x47U_SPI_READ_MAGN_MSG			(AS5x47U_SPI_MAGN_ADR  | AS5x47U_SPI_READ_BIT)
#define AS5x47U_SPI_READ_AGC_MSG			(AS5x47U_SPI_AGC_ADR   | AS5x47U_SPI_READ_BIT)
#define AS5x47U_SPI_READ_POS_MSG			(AS5x47U_SPI_POS_ADR   | AS5x47U_SPI_READ_BIT)

#define AS5x47U_SPI_READ_ERRFL_CRC							(0x06)
#define AS5x47U_SPI_READ_DIAG_CRC							(0x6F)
#define AS5x47U_SPI_READ_MAGN_CRC							(0x87)
#define AS5x47U_SPI_READ_AGC_CRC							(0xF3)
#define AS5x47U_SPI_READ_POS_CRC							(0xBD)

#define AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD		5

enum {
	SPI_SEQ_TX_MAG_RX_POS,
	SPI_SEQ_TX_POS_RX_MAG,
	SPI_SEQ_TX_AGC_RX_POS,
	SPI_SEQ_TX_POS_RX_AGC,
	SPI_SEQ_TX_DIAG_RX_POS,
	SPI_SEQ_TX_POS_RX_DIAG,
	SPI_SEQ_TX_ERRFL_RX_POS,
	SPI_SEQ_TX_POS_RX_ERRFL,
	SPI_SEQ_PREV_ERR,
};

// Private functions
static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_last_valid);
static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg,
		uint16_t tx_data, uint8_t tx_crc);
static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData);

static uint8_t enc_as5x47u_crc8(const uint8_t *data, size_t len, const uint8_t inital) {
	// Lookup table generated using poly 0x1D
	static const uint8_t crc8_lookup[] = {
		0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
		0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
		0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
		0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
		0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
		0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
		0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
		0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
		0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
		0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
		0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
		0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
		0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
		0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
		0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
		0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4,
	};
	uint8_t cksum = inital;
	size_t i;
	for (i = 0; i < len; i++) {
		cksum ^= data[i];
		cksum = crc8_lookup[cksum];
	}
	return cksum;
}

void enc_as5x47u_spi_callback(SPIDriver *pspi) {
	if (pspi != NULL && pspi->app_arg != NULL) {
		AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg;
		spiUnselectI(cfg->spi_dev);
	
		// Determine time step for error rate calculation
		float timestep = timer_seconds_elapsed_since(cfg->state.last_update_time);
		if (timestep > 1.0) {
			timestep = 1.0;
		}
		cfg->state.last_update_time = timer_time_now();

		uint8_t rx_crc = cfg->state.rx_buf[2];
		uint8_t calc_crc = enc_as5x47u_crc8(cfg->state.rx_buf, 2, 0xC4) ^ 0xFF;
		uint16_t rx_data = cfg->state.rx_buf[0] << 8 | cfg->state.rx_buf[1];
		if (calc_crc == rx_crc) {
			AS5x47U_determinate_if_connected(cfg, true);
			cfg->state.sensor_diag.is_error =
				(uint8_t)((rx_data & AS5x47U_SPI_ERROR_FLAG_MASK) != 0);

			if (!cfg->state.sensor_diag.is_error) {
				UTILS_LP_FAST(cfg->state.spi_error_rate, 0.0, timestep);

				switch(cfg->state.spi_seq) {
				case SPI_SEQ_TX_MAG_RX_POS:
					// Receive position, then request position while receiving magnitude
					AS5x47U_process_pos(cfg, rx_data);
					cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_MAG;
					AS5x47U_start_spi_exchange_precalc_crc(
						cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC);
					break;

				case SPI_SEQ_TX_POS_RX_MAG:
					// Receive magnitude
					cfg->state.sensor_diag.serial_magnitude = rx_data;
					cfg->state.sensor_diag.magnitude = rx_data
						& AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
					cfg->state.spi_seq = SPI_SEQ_TX_AGC_RX_POS;
					break;

				case SPI_SEQ_TX_AGC_RX_POS:
					// Receive position, then request position while receiving AGC
					AS5x47U_process_pos(cfg, rx_data);
					cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_AGC;
					AS5x47U_start_spi_exchange_precalc_crc(
						cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC);
					break;

				case SPI_SEQ_TX_POS_RX_AGC:
					// Receive AGC
					cfg->state.sensor_diag.serial_AGC_value = rx_data;
					cfg->state.sensor_diag.AGC_value = (uint8_t)rx_data;
					cfg->state.spi_seq = SPI_SEQ_TX_DIAG_RX_POS;
					break;
				case SPI_SEQ_TX_DIAG_RX_POS:
					// Receive position, then request position while requesting diagnostic flags
					AS5x47U_process_pos(cfg, rx_data);
					cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_DIAG;
					AS5x47U_start_spi_exchange_precalc_crc(
							cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC);
					break;

				case SPI_SEQ_TX_POS_RX_DIAG:
					// Receive diagnostic flags
					cfg->state.sensor_diag.serial_diag_flgs = rx_data;
					cfg->state.sensor_diag.is_broken_hall =
							(rx_data >> AS5x47U_SPI_DIAG_FUSA_ERROR_BIT_POS) & 1;
					cfg->state.sensor_diag.is_COF =
							(rx_data >> AS5x47U_SPI_DIAG_COF_BIT_POS) & 1;
					cfg->state.sensor_diag.is_Comp_low =
							(rx_data >> AS5x47U_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
					cfg->state.sensor_diag.is_Comp_high =
							(rx_data >> AS5x47U_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
					cfg->state.spi_seq = SPI_SEQ_TX_ERRFL_RX_POS;
					break;

				case SPI_SEQ_TX_ERRFL_RX_POS:
					// Receive position, then request pos while receiving error flags
					AS5x47U_process_pos(cfg, rx_data);
					cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL;
					AS5x47U_start_spi_exchange_precalc_crc(
							cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC);
					break;
				case SPI_SEQ_TX_POS_RX_ERRFL:
					// Receive error flags
					cfg->state.spi_seq = 0;
					cfg->state.sensor_diag.serial_error_flgs = rx_data;
					cfg->state.sensor_diag.is_wdtst =
							(rx_data >> AS5x47U_SPI_ERRFL_WDTST_BIT_POS) & 1;
					cfg->state.sensor_diag.is_crc_error =
							(rx_data >> AS5x47U_SPI_ERRFL_CRC_ERROR_BIT_POS) & 1;
					cfg->state.sensor_diag.is_mag_half =
							(rx_data >> AS5x47U_SPI_ERRFL_MAG_HALF_BIT_POS) & 1;
					cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS;
					break;
				case SPI_SEQ_PREV_ERR:
					// Not sure what was just received, but just requested ERRFL, so ignore rx
					// data and prepare to receive ERRFL next exchange
					cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL;
					break;
				default:
					// Something went wrong, just start the sequence over.
					cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS;
					break;
				}
			} else {
				// Error flag is set
				AS5x47U_determinate_if_connected(cfg, true); // Encoder is connected...
				UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, timestep); // But there is an error
				++cfg->state.spi_error_cnt;
				// Error flag is set so need to read error register next
				cfg->state.spi_seq = SPI_SEQ_PREV_ERR;
			}
		} else {
			// CRC error
			AS5x47U_determinate_if_connected(cfg, false);
			UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, timestep);
			++cfg->state.spi_error_cnt;
			cfg->state.spi_seq = SPI_SEQ_PREV_ERR;
		}
	}
}

static void as5x47u_spi_err_callback(SPIDriver *pspi) {
	if(pspi != NULL && pspi->app_arg != NULL) {
		AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg;
		// Make sure we won't process the data
		memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf));
	}
}

bool enc_as5x47u_init(AS5x47U_config_t *cfg) {
	if (cfg->spi_dev == NULL) {
		return false;
	}

	memset(&cfg->state, 0, sizeof(AS5x47U_state));

	palSetPadMode(cfg->sck_gpio, cfg->sck_pin,
			PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST);
	palSetPadMode(cfg->miso_gpio, cfg->miso_pin,
			PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST);
	palSetPadMode(cfg->nss_gpio, cfg->nss_pin,
			PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
	palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin,
			PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST);

	cfg->spi_dev->app_arg = (void*)cfg;
	cfg->spi_dev->err_cb = as5x47u_spi_err_callback;

	// Set in encoder_cfg.c, kept here for visability
	// cfg->spi_dev->config->end_cb = enc_as5x47u_spi_callback;

	spiStart(cfg->spi_dev, &(cfg->hw_spi_cfg));

	cfg->state.spi_error_rate = 0.0;

	return true;
}

void enc_as5x47u_deinit(AS5x47U_config_t *cfg) {
	if (cfg->spi_dev != NULL) {
		palSetPadMode(cfg->miso_gpio, cfg->miso_pin, PAL_MODE_INPUT_PULLUP);
		palSetPadMode(cfg->sck_gpio, cfg->sck_pin, PAL_MODE_INPUT_PULLUP);
		palSetPadMode(cfg->nss_gpio, cfg->nss_pin, PAL_MODE_INPUT_PULLUP);
		palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin, PAL_MODE_INPUT_PULLUP);

		spiStop(cfg->spi_dev);

		cfg->state.last_enc_angle = 0.0;
		cfg->state.spi_error_rate = 0.0;
	}
}

void enc_as5x47u_routine(AS5x47U_config_t *cfg) {
	switch(cfg->state.spi_seq) {
	case SPI_SEQ_TX_MAG_RX_POS:
		// Request magnitude
		AS5x47U_start_spi_exchange_precalc_crc(
			cfg, AS5x47U_SPI_READ_MAGN_MSG, AS5x47U_SPI_READ_MAGN_CRC);
		break;

	case SPI_SEQ_TX_AGC_RX_POS:
		// Request AGC
		AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_AGC_MSG,
				AS5x47U_SPI_READ_AGC_CRC);
		break;

	case SPI_SEQ_TX_DIAG_RX_POS:
		// Request diagnostic flags
		AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_DIAG_MSG,
				AS5x47U_SPI_READ_DIAG_CRC);
		break;

	case SPI_SEQ_TX_ERRFL_RX_POS:
		// Request error flags
		AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG,
				AS5x47U_SPI_READ_ERRFL_CRC);
		break;

	default:
	case SPI_SEQ_PREV_ERR:
		// There was a problem of some sort, request ERRFL
		AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG,
				AS5x47U_SPI_READ_ERRFL_CRC);
		break;
	}
}

static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData) {
	cfg->state.spi_val = posData;
	posData &= AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
	cfg->state.last_enc_angle = (float)(posData * 360) / (float)(1 << 14);
}

static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_last_valid) {
	if (!was_last_valid) {
		cfg->state.spi_communication_error_count++;

		if (cfg->state.spi_communication_error_count >=
				AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) {
			cfg->state.spi_communication_error_count =
				AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD;
			cfg->state.sensor_diag.is_connected = 0;
		}
	} else {
		if (cfg->state.spi_communication_error_count) {
			cfg->state.spi_communication_error_count--;
		} else {
			cfg->state.sensor_diag.is_connected = 1;
		}
	}
}

/* TODO: uncomment if needed
static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data) {
	uint8_t tx_to_crc[] = {(tx_data >> 8) & 0xFF, (tx_data & 0xFF)};
	uint8_t tx_crc = enc_as5x47u_crc8(tx_to_crc, 2, 0xC4) ^ 0xFF;
	AS5x47U_start_spi_exchange_precalc_crc(cfg, tx_data, tx_crc);
}
*/

static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg,
		uint16_t tx_data, uint8_t tx_crc) {
	cfg->state.tx_buf[0] = (tx_data >> 8) & 0xFF;
	cfg->state.tx_buf[1] = tx_data & 0xFF;
	cfg->state.tx_buf[2] = tx_crc;
	memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf));

	// There is a weird corner case where the DMA may not read all of the Rx data. This
	// causes the RXNE flag to be set when an exchange starts, causing the first byte of
	// data received to be from the previous exchange. This is corrected by reading the
	// SPI data register, clearing the RXNE flag.
	volatile uint32_t test = cfg->spi_dev->spi->DR;
	(void)test; // get rid of unused warning

	spiSelectI(cfg->spi_dev);
	spiStartExchangeI(cfg->spi_dev, 3, cfg->state.tx_buf, cfg->state.rx_buf);
}