/*
 * AS5048.c
 *
 * Created: 27/07/2021 16:13:20
 *  Author: Nick-XMG
 */ 

#include "AS5048.h"

#define AS5048A_CMD_NOP						  (0x0000)
#define AS5048A_READ						  (0x4000)
#define AS5048A_CLEAR_ERROR_FLAG			  (0x0001)
#define AS5048A_PROGRAMMING_CONTROL           (0x0003)
#define AS5048A_OTP_REGISTER_ZERO_POS_HIGH    (0x0016)
#define AS5048A_OTP_REGISTER_ZERO_POS_LOW     (0x0017)
#define AS5048A_DIAG_AGC                      (0x3FFD)
#define AS5048A_MAGNITUDE                     (0x3FFE)
#define AS5048A_ANGLE                         (0x3FFF)


#define	AS5048A_MAX_VALUE					   (8191.0f)

static bool debugPrint = true;

struct SPI_AS5048 AS_1 = {
	.SPI_descr = NULL,
	.io_descr = NULL,
	.flags = 0,
	.state = 0,
	.SS_pin = Angle_SS,
	.tx_buffer = NULL,
	.rx_buffer = NULL,

};

void as5048a_State_Machine(struct SPI_AS5048 *Sn)
{
	volatile uint16_t angle;
	switch(Sn->state) {
		case 0:		// Default un-initialized State
		break;
		case 1:		// State After angleSensor_init.  Configure Sensor
			Sn->state++;
		break;
		case 2:   // Synchronous blocking read
			as5048a_getRotationInDegrees(Sn);
		break;
		case 3:		// Continuously Read Angle (DMA)
		break;
		case 4:		// Error State
		break;
	}
}

void as5048a_init(struct spi_m_sync_descriptor *spi, struct SPI_AS5048 *Sn)
{
	Sn->SPI_descr = spi;
	spi_m_sync_disable(&SPI_1);

	// Set Mode 1 : CPOL = 0; CPHA = 1;
	hri_sercomspi_write_CTRLA_CPOL_bit(&SPI_1, false);
	hri_sercomspi_write_CTRLA_CPHA_bit(&SPI_1, true);

	//Get IO descriptor
	spi_m_sync_get_io_descriptor(Sn->SPI_descr, &Sn->io_descr);
	spi_m_sync_enable(&SPI_1);
	gpio_set_pin_level(Sn->SS_pin, true);
	if(Sn->io_descr == NULL || Sn->SPI_descr == NULL) {
		Sn->state = 4;
	} else {
		Sn->state = 1;
	}
}

uint16_t as5048a_read(struct SPI_AS5048 *Sn, uint16_t address)
{
	uint8_t in_buf[2], out_buf[2];
	uint16_t returnedVal;

	volatile uint16_t command = 0b0100000000000000; // PAR=0 R/W=R
	command = command | address;

	//Add a parity bit on the the MSB
	command |= ((uint16_t)as5048a_CalcEvenParity(command)<<15);
	
	//Split the command into two bytes
	out_buf[1] = command & 0xFF;
	out_buf[0] = ( command >> 8 ) & 0xFF;
	
	/* Send Command */
	gpio_set_pin_level(Sn->SS_pin, false);
	as5048a_transfer16(Sn, command);
	gpio_set_pin_level(Sn->SS_pin, true);
	
	/* Send again to Get Response */
	gpio_set_pin_level(Sn->SS_pin, false);
	returnedVal = as5048a_transfer16(Sn, command);
	gpio_set_pin_level(Sn->SS_pin, true);
	
	// 	printf("In_buff= ");
	// 	/* Print Received data by SPI Master from SPI Slave on Console */
	// 	for (int i = 0; i < 2; i++) {
	// 		printf("%u", in_buf[i]);
	// 	}
	// 	printf("\n\r");
	if(debugPrint) {
		printf("Returned val = %u \n\r", returnedVal);
	}

	
	if (returnedVal & 0x4000) {
		Sn->flags = 1;
	} else {
		Sn->flags = 0;
	}

	//Return the data, stripping the parity and error bits
	return (( ( in_buf[1] & 0xFF ) << 8 ) | ( in_buf[0] & 0xFF )) & ~0xC000;
}

/*
 * getRawRotation
 * 
 * Read Raw Angle Register Value
 */
uint16_t as5048a_getRawRotation(struct SPI_AS5048 *Sn)
{
	volatile uint16_t rawAngle = as5048a_read(Sn, AS5048A_ANGLE);
	return rawAngle;
}

int16_t as5048a_getRotation(struct SPI_AS5048 *Sn)
{
	uint16_t data;
	int16_t rotation;

	data = as5048a_getRawRotation(Sn);
	rotation = (int16_t)(data); //- static_cast<int16_t>(this->position);
	if (rotation > AS5048A_MAX_VALUE) {
		rotation = -((0x3FFF) - rotation); //more than -180
	}

	return rotation;

}

/*
* getRotationInDegrees
*
* Read angle register and return angle in Degrees
*/
float as5048a_getRotationInDegrees(struct SPI_AS5048 *Sn)
{
	uint16_t rawAngle = as5048a_getRotation(Sn);
	float angleDegrees = 360.0 * (rawAngle + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);;

	return angleDegrees;
}


uint16_t as5048a_transfer16(struct SPI_AS5048 *Sn, uint16_t value)
{
	uint8_t in_buf[2], out_buf[2];
	uint16_t wordRead;
	
	out_buf[1] = value & 0xFF;
	out_buf[0] = ( value >> 8 ) & 0xFF;
	struct spi_xfer  xfer;
	xfer.rxbuf = in_buf;
	xfer.txbuf = (uint8_t *)out_buf;
	xfer.size  = 2;
	spi_m_sync_transfer(Sn->SPI_descr, &xfer);
	wordRead = (uint16_t)((in_buf[0] << 8) | in_buf[1]);
	return wordRead;
}

uint8_t as5048a_CalcEvenParity(uint16_t value) {
	uint8_t cnt = 0;
	uint8_t i;
	for (i = 0; i < 16; i++) {
		if (value & 0x1) {
			cnt++;
		}
		value >>= 1;
	}
	return cnt & 0x1;
}