/*
 * A1335.c
 *
 * Created: 24/07/2021 17:54:33
 *  Author: Nick-XMG
 */ 
#include "A1335.h"

//--- Normal Write Registers ---//
#define	EWA		(0x02U)   // Extended Write Address
#define EWD     (0x04U)   // Extended Write Data
#define EWCS    (0x08U)   // Extended Write Control and Status
#define ERA		(0x0AU)   // Extended Read Address
#define ERCS	(0x0CU)   // Extended Read Control and Status
#define ERD		(0x0EU)   // Extended Read Data
#define CTRL	(0x1EU)   // Device control
#define ANG		(0x20U)   // Current angle and related data
#define STA		(0x22U)   // Device status
#define ERR		(0x24U)  // Device error status
#define XERR	(0x26U)   // Extended error status
#define TSEN	(0x28U)   // Temperature sensor data
#define FIELD	(0x2AU)   // Magnetic field strength
#define ERM		(0x34U)   // Device error status masking
#define XERM	(0x36U)   // Extended error status masking

const uint32_t WRITE = 0x40;
const uint32_t READ = 0x00;
const uint32_t COMMAND_MASK = 0xC0;
const uint32_t ADDRESS_MASK = 0x3F;

//--- Extended Write Registers ---//
#define ORATE	(0xFFD0)  // Output Rate
//--- Errors ---//
#define kNOERROR						0
#define kPRIMARYREADERROR				1
#define kEXTENDEDREADTIMEOUTERROR		2
#define kPRIMARYWRITEERROR				3
#define kEXTENDEDWRITETIMEOUTERROR		4
#define kCRCERROR						5
#define kUNABLETOCHANGEPROCESSORSTATE	6

bool debugPrint = true;

struct SPI_A1335 A1 = {
	.SPI_descr = NULL,
	.io_descr = NULL,
	.state = 0,
	.SS_pin = Angle_SS,
	.tx_buffer = NULL,
	.rx_buffer = NULL,
		
};

void a1335_init(struct spi_m_sync_descriptor *spi, struct SPI_A1335 *Sn)
{
	Sn->SPI_descr = spi;
	// Set Mode 3 : CPOL = 1; CPHA = 1;
	hri_sercomspi_write_CTRLA_CPOL_bit(spi->dev.prvt, true);
	hri_sercomspi_write_CTRLA_CPHA_bit(spi->dev.prvt, true);
	//Get IO descriptor
	spi_m_sync_get_io_descriptor(Sn->SPI_descr, &Sn->io_descr);
	spi_m_sync_enable(Sn->SPI_descr);
	gpio_set_pin_level(Sn->SS_pin, true);
	if(Sn->io_descr == NULL || Sn->SPI_descr == NULL)
	{
		Sn->state = 4;
	} else {
		Sn->state = 1;
	}
}

void a1335_State_Machine(struct SPI_A1335 *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
			a1335_getRotationInDegrees(Sn);
		break;
		case 3:		// Continuously Read Angle (DMA)
		break;	
		case 4:		// Error State
		break;
	}
}

/*
 * getRawRotation
 * 
 * Read Raw Angle Register Value
 */
uint16_t a1335_getRawRotation(struct SPI_A1335 *Sn)
{
	uint16_t rawAngle;
	PrimaryRead(Sn, ANG, &rawAngle);
	return rawAngle;
}

/*
 * getRotationInDegrees
 * 
 * Read angle register and return angle in Degrees
 */
float a1335_getRotationInDegrees(struct SPI_A1335 *Sn)
{
	uint16_t rawAngle = a1335_getRawRotation(Sn);
	float angleDegrees;
	if (CalculateParity(rawAngle))
	{
		angleDegrees = (float)(rawAngle & 0x0FFF) * 360.0 / 4096.0;
		if (debugPrint)
		{
			printf("Angle = ");
			printf("%f", angleDegrees);
			printf(" Degrees\n\r");
		}
	}
	else
	{
		printf("Parity error on Angle read\n\r");
	}
	return angleDegrees;
}

/*
 * CalculateParity
 *
 * From the 16 bit input, calculate the parity
 */
bool CalculateParity(uint16_t input)
{
	uint16_t count = 0;
	
	// Count up the number of 1s in the input
	for (int index = 0; index < 16; ++index)
	{
		if ((input & 1) == 1)
		{
			++count;
		}

		input >>= 1;
	}
	
	// return true if there is an odd number of 1s
	return (count & 1) != 0;
}

/*
 * CalculateCRC
 *
 * Take the 16bit input and generate a 4bit CRC
 * Polynomial = x^4 + x^1 + 1
 * LFSR preset to all 1's
 */
uint8_t CalculateCRC(uint16_t input)
{
	bool CRC0 = true;
	bool CRC1 = true;
	bool CRC2 = true;
	bool CRC3 = true;
	int  i;
	bool DoInvert;
	uint16_t mask = 0x8000;
	
	for (i = 0; i < 16; ++i)
	{
		DoInvert = ((input & mask) != 0) ^ CRC3;         // XOR required?

		CRC3 = CRC2;
		CRC2 = CRC1;
		CRC1 = CRC0 ^ DoInvert;
		CRC0 = DoInvert;
		mask >>= 1;
	}

	return (CRC3 ? 8U : 0U) + (CRC2 ? 4U : 0U) + (CRC1 ? 2U : 0U) + (CRC0 ? 1U : 0U);
}

/*
 * PrimaryRead
 * 
 * Read from the primary serial registers
 */
uint16_t PrimaryRead(struct SPI_A1335 *Sn, uint16_t address, uint16_t* value)
{
	
	uint8_t in_buf[2], out_buf[2];
	uint16_t returnedVal;

	uint16_t command = ((address & ADDRESS_MASK) | READ) << 8;
	//Split the command into two bytes
	out_buf[1] = command & 0xFF;
	out_buf[0] = ( command >> 8 ) & 0xFF;
	gpio_set_pin_level(Sn->SS_pin, false);
	a1335_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 = a1335_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");		
	*value = returnedVal;
	
	
	if(debugPrint) {
		printf("Returned val = %u \n\r", returnedVal);
	}

	
    return kNOERROR;
}

uint16_t a1335_transfer16(struct SPI_A1335 *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;
}

/*
 * PrimaryWrite
 * 
 * Write to the primary serial registers
 */
uint16_t PrimaryWrite(uint16_t cs, uint16_t address, uint16_t value)
{
    //if (includeCRC)
    //{
        //SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));
//
        //// On the Teensy, SPI0_CTAR0 is used to describe the SPI transaction for transfer
        //// while SPI0_CTAR1 is used to describe the SPI transaction for transfer16.
        //// To do a 20 bit transfer, change the length of the transaction to 4 bits for transfer
        //// and do a transfer16 followed by a transfer.
        ////uint32_t oldSPI0_CTAR0 = SPI0_CTAR0;
       //// SPI0_CTAR0 = (SPI0_CTAR0 & 0x87FFFFFF) | SPI_CTAR_FMSZ(3);    //  using SPI0_CTAR0 to send 4 bits
//
        //// Combine the register address and the command into one byte
        //uint16_t command = (((address & ADDRESS_MASK) | WRITE) << 8)  | ((value >> 8) & 0x0FF);
        //uint8_t crcCommand = CalculateCRC(command);
//
        //// take the chip select low to select the device:
        //digitalWrite(cs, LOW);
//
        //SPI.transfer16(command); // Send most significant byte of register data
        //SPI.transfer(crcCommand); // Send the crc
//
        //// take the chip select high to de-select:
        //digitalWrite(cs, HIGH);
//
        //command = ((((address + 1) & ADDRESS_MASK) | WRITE) << 8 ) | (value & 0x0FF);
        //crcCommand = CalculateCRC(command);
//
        //// take the chip select low to select the device:
        //digitalWrite(cs, LOW);
//
        //SPI.transfer16(command); // Send least significant byte of register data
        //SPI.transfer(crcCommand); // Send the crc
//
        //// take the chip select high to de-select:
        //digitalWrite(cs, HIGH);
//
        //// Restore the 8 bit description
        ////SPI0_CTAR0 = oldSPI0_CTAR0;
//
        //SPI.endTransaction();
    //}
    //else
    //{
        //SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));
//
        //// Combine the register address and the command into one byte
        //uint16_t command = ((address & ADDRESS_MASK) | WRITE) << 8;
//
        //// take the chip select low to select the device:
        //digitalWrite(cs, LOW);
//
        //SPI.transfer16(command | ((value >> 8) & 0x0FF)); // Send most significant byte of register data
//
        //// take the chip select high to de-select:
        //digitalWrite(cs, HIGH);
//
        //command = (((address + 1) & ADDRESS_MASK) | WRITE) << 8;
        //// take the chip select low to select the device:
        //digitalWrite(cs, LOW);
//
        //SPI.transfer16(command | (value & 0x0FF)); // Send least significant byte of register data
//
        //// take the chip select high to de-select:
        //digitalWrite(cs, HIGH);
//
        //SPI.endTransaction();
    //}

    return kNOERROR;
}

/*
 * ExtendedRead
 * 
 * Read from the SRAM, EEPROM, AUX or Extended Registers
 */
uint16_t ExtendedRead(uint16_t cs, uint16_t address, uint32_t value)
{
    //uint16_t results;
    //uint16_t readFlags;
    //uint32_t timeout;
    //uint16_t valueMSW;
    //uint16_t valueLSW;
    //uint32_t currentTime;
//
    //// Write the address to the Extended Read Address register
    //results = PrimaryWrite(cs, 0x0A, address & 0xFFFF);
//
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
    //// Initiate the extended read
    //results = PrimaryWrite(cs, 0x0C, 0x8000);
        //
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
    //timeout = millis() + 100L;
//
    //do  // Wait for the read to be complete
    //{
        //results = PrimaryRead(cs, 0x0C, readFlags);
    //
        //if (results != kNOERROR)
        //{
            //return results;
        //}
//
        //// Make sure the read is not taking too long
        //currentTime = millis();
        //if (timeout < currentTime)
        //{
            //return kEXTENDEDREADTIMEOUTERROR;
        //}
    //} while ((readFlags & 0x0001) != 0x0001);
    //
    //// Read the most significant word from the extended read data
    //results = PrimaryRead(cs, 0x0E, valueMSW);
//
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
    //// Read the least significant word from the extended read data
    //results = PrimaryRead(cs, 0x10, valueLSW);
//
    //// Combine them
    //value = ((uint32_t)valueMSW << 16) + valueLSW;
//
    //return results;
}

/*
 * ExtendedWrite
 * 
 * Write to the SRAM, EEPROM, AUX or Extended Access Commands
 */
uint16_t ExtendedWrite(uint16_t cs, uint16_t address, uint32_t value)
{
    //uint16_t results;
    //uint16_t writeFlags;
    //uint32_t timeout;
//
	//// Write into the extended address register
    //results = PrimaryWrite(cs, 0x02, address & 0xFFFF);
    //
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
	//// Write the MSW (Most significant word) into the high order write data register
    //results = PrimaryWrite(cs, 0x04, (value >> 16) & 0xFFFF);
        //
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
	//// Write the LSW (Least significant word) into the low order write data register
    //results = PrimaryWrite(cs, 0x06, value & 0xFFFF);
        //
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
	//// Start the write process
    //results = PrimaryWrite(cs, 0x08, 0x8000);
        //
    //if (results != kNOERROR)
    //{
        //return results;
    //}
//
    //// If writing to the EEPROM, generate the Program
    //// Pulses on Vcc
    //if ((address >= 0x300) && (address < 0x320))
    //{
        //// Send the Program Pulses
        //// Need hardware which this example does not have.
    //}
//
    //timeout = millis() + 100;
//
	//// Wait for the write to complete
    //do
    //{
        //results = PrimaryRead(cs, 0x08, writeFlags);
    //
        //if (results != kNOERROR)
        //{
            //return results;
        //}
//
        //if (timeout < millis())
        //{
            //return kEXTENDEDWRITETIMEOUTERROR;
        //}
    //} while ((writeFlags & 0x0001) != 0x0001);
//
    //return results;
}

/*
 * SetProcessorStateToIdle
 * 
 * Change the processor state to idle
 * This is needed to write EEPROM, change ORATE or perform certain self tests
 */
uint16_t SetProcessorStateToIdle(uint8_t cs)
{
    //uint16_t results;
    //uint16_t value;
 //
    //// Write the enter idle state command into the control register
    //results = PrimaryWrite(cs, 0x1F, 0x8046);
    //
    //if (results == kNOERROR)
    //{
        //delay(1);
//
        //// Read the status register to see if the processor is in the idle state
        //results = PrimaryRead(cs, 0x22, value);
        //
        //if (results == kNOERROR)
        //{
            //if ((value & 0x00FF) != 0x0010)
            //{
                //return kUNABLETOCHANGEPROCESSORSTATE;
            //}
        //}
    //}
  //
    //return results;
}

/*
 * SetProcessorStateToRun
 * 
 * Change the processor state to run
 * This is needed to process angles
 */
uint16_t SetProcessorStateToRun(uint8_t cs)
{
    //uint16_t results;
    //uint16_t value;
 //
    //// Write the enter idle state command into the control register
    //results = PrimaryWrite(cs, 0x1F, 0xC046);
    //
    //if (results == kNOERROR)
    //{
        //delay(1);
//
        //// Read the status register to see if the processor is in the idle state
        //results = PrimaryRead(cs, 0x22, value);
        //
        //if (results == kNOERROR)
        //{
            //if ((value & 0x00FF) != 0x0011)
            //{
                //return kUNABLETOCHANGEPROCESSORSTATE;
            //}
        //}
    //}
  //
    //return results;
}
