/*******************************************************************************
* Copyright © 2019 TRINAMIC Motion Control GmbH & Co. KG
* (now owned by Analog Devices Inc.),
*
* Copyright © 2023 Analog Devices Inc. All Rights Reserved. This software is
* proprietary & confidential to Analog Devices, Inc. and its licensors.
*******************************************************************************/


#include "TMC2209.h"
#include "tmc/hal/UART.h"
#include "tmc/helpers/CRC.h"










typedef struct {
	uint8_t table[256];
	uint8_t polynomial;
	bool isReflected;
} CRCTypeDef;

CRCTypeDef CRCTables[CRC_TABLE_COUNT] = { 0 };

static uint8_t flipByte(uint8_t value);
static uint32_t flipBitsInBytes(uint32_t value);

/* This function generates the Lookup table used for CRC calculations.
 *
 * Arguments:
 *     uint8_t polynomial: The CRC polynomial for which the table will be generated.
 *     bool isReflected: Indicator whether the CRC table will be reflected or not.
 *     uint8_t index: The index of the table to be filled.
 *
 * How it works:
 *     A CRC calculation of a byte can be done by taking the byte to be CRC'd,
 *     shifting it left by one (appending a 0) and - if a 1 has been shifted out -
 *     XOR-ing in the CRC polynomial. After 8 iterations the result will be the
 *     CRC of the Byte.
 *
 *     The function below does this in a compact way, by using all 4 bytes of a
 *     uint32_t to do 4 separate CRC bytes at once.
 *     For this to work without the Byte shifting interfering with adjacent bytes,
 *     the polynomial has the 8th bit (0x100) set. That way, if the shifted-out bit
 *     is 1, the following XOR-ing with the CRC polynomial will set that 1 to a 0,
 *     resulting in the shifted-in 0 for the adjacent byte.
 *     This process will go from the the lowest to the highest byte, resulting in
 *     fully independent byte-wise CRC calculations. For the highest byte, the value
 *     of the shifted-out byte needs to be stored before shifting the bytes (isMSBSet).
 *
 *     The for-loop that iterates over all uint8_t values starts out with the
 *     uint8_t values 3 to 0 stored in one uint32_t: 0x03020100
 *     for each iteration each uint8_t value will increase by 4..
 *     0 -> 4 -> 8 -> C -> ...
 *     1 -> 5 -> 9 -> D -> ...
 *     2 -> 6 -> A -> E -> ...
 *     3 -> 7 -> B -> F -> ...
 *     ..resulting in an increase of the uint32_t by 0x04040404:
 *     0x03020100 -> 0x07060504 -> 0x0B0A0908 -> 0x0F0E0D0C -> ...
 *     The loop ends as soon as we have iterated over all uint8_t values.
 *     We detect that by looking for the byte-wise overflow into the next byte:
 *     0xFFFEFDFC                  <- last uint32_t value to be calculated
 *     0xFF,  0xFE,  0xFD,  0xFC   <- the corresponding uint8_t values
 *     0x103, 0x102, 0x101, 0x100  <- incremented uint8_t values (overflow into the next byte!)
 *     0x04030200                  <- uint32_t value with the overflowed bytes
 *
 *     We have the lower uint8_t values at the lower bytes of the uint32_t.
 *     This allows us to simply store the lowest byte of the uint32_t,
 *     right-shift the uint32_t by 8 and increment the table pointer.
 *     After 4 iterations of that all 4 bytes of the uint32_t are stored in the table.
 */
uint8_t tmc_fillCRC8Table(uint8_t polynomial, bool isReflected, uint8_t index)
{
	uint32_t CRCdata;
	// Helper pointer for traversing the result table
	uint8_t *table;

	if(index >= CRC_TABLE_COUNT)
		return 0;

	CRCTables[index].polynomial   = polynomial;
	CRCTables[index].isReflected  = isReflected;
	table = &CRCTables[index].table[0];

	// Extend the polynomial to correct byte MSBs shifting into next bytes
	uint32_t poly = (uint32_t) polynomial | 0x0100;

	// Iterate over all 256 possible uint8_t values, compressed into a uint32_t (see detailed explanation above)
	uint32_t i;
	for(i = 0x03020100; i != 0x04030200; i+=0x04040404)
	{
		// For reflected table: Flip the bits of each input byte
		CRCdata = (isReflected)? flipBitsInBytes(i) : i;

		// Iterate over 8 Bits
		int j;
		for(j = 0; j < 8; j++)
		{
			// Store value of soon-to-be shifted out byte
			uint8_t isMSBSet = (CRCdata & 0x80000000)? 1:0;

			// CRC Shift
			CRCdata <<= 1;

			// XOR the bytes when required, lowest to highest
			CRCdata ^= (CRCdata & 0x00000100)? (poly      ) : 0;
			CRCdata ^= (CRCdata & 0x00010000)? (poly << 8 ) : 0;
			CRCdata ^= (CRCdata & 0x01000000)? (poly << 16) : 0;
			CRCdata ^= (isMSBSet)?             (poly << 24) : 0;
		}

		// For reflected table: Flip the bits of each output byte
		CRCdata = (isReflected)? flipBitsInBytes(CRCdata) : CRCdata;
		// Store the CRC result bytes in the table array
		*table++ = (uint8_t) CRCdata;
		CRCdata >>= 8;
		*table++ = (uint8_t) CRCdata;
		CRCdata >>= 8;
		*table++ = (uint8_t) CRCdata;
		CRCdata >>= 8;
		*table++ = (uint8_t) CRCdata;
	}

	return 1;
}

/* This function calculates the CRC from a data buffer
 *
 * Arguments:
 *     uint8_t *data: A pointer to the data that will be CRC'd.
 *     uint32_t bytes: The length of the data buffer.
 *     uint8_t index: The index of the CRC table to be used.
 */
uint8_t tmc_CRC8(uint8_t *data, uint32_t bytes, uint8_t index)
{
	uint8_t result = 0;
	uint8_t *table;

	if(index >= CRC_TABLE_COUNT)
		return 0;

	table = &CRCTables[index].table[0];

	while(bytes--)
		result = table[result ^ *data++];

	return (CRCTables[index].isReflected)? flipByte(result) : result;
}

uint8_t tmc_tableGetPolynomial(uint8_t index)
{
	if(index >= CRC_TABLE_COUNT)
		return 0;

	return CRCTables[index].polynomial;
}

bool tmc_tableIsReflected(uint8_t index)
{
	if(index >= CRC_TABLE_COUNT)
		return false;

	return CRCTables[index].isReflected;
}

// Helper functions
static uint8_t flipByte(uint8_t value)
{
	// swap odd and even bits
	value = ((value >> 1) & 0x55) | ((value & 0x55) << 1);
	// swap consecutive pairs
	value = ((value >> 2) & 0x33) | ((value & 0x33) << 2);
	// swap nibbles ...
	value = ((value >> 4) & 0x0F) | ((value & 0x0F) << 4);

	return value;
}

/* This helper function switches all bits within each byte.
 * The byte order remains the same:
 * [b31 b30 b29 b28 b27 b26 b25 b24 .. b7 b6 b5 b4 b3 b2 b1 b0]
 *                                  ||
 *                                 \||/
 *                                  \/
 * [b24 b25 b26 b27 b28 b29 b30 b31 .. b0 b1 b2 b3 b4 b5 b6 b7]
 */
static uint32_t flipBitsInBytes(uint32_t value)
{
	// swap odd and even bits
	value = ((value >> 1) & 0x55555555) | ((value & 0x55555555) << 1);
	// swap consecutive pairs
	value = ((value >> 2) & 0x33333333) | ((value & 0x33333333) << 2);
	// swap nibbles ...
	value = ((value >> 4) & 0x0F0F0F0F) | ((value & 0x0F0F0F0F) << 4);

	return value;
}




















typedef union {
    TMC2209TypeDef tmc2209;
} DriverBoards;

DriverBoards driverBoards;
#define TMC2209_CRC(data, length) tmc_CRC8(data, length, 1)

#define TMC2209 (driverBoards.tmc2209)
#define BUFFER_SIZE         32
#define INTR_PRI            6
#define UART_TIMEOUT_VALUE  10
#define WRITE_READ_DELAY    10
// #include "tmc/boards/Board.h"
// #include "tmc/tmc/StepDir.h"
// // => UART wrapper
// extern void tmc2209_readWriteArray(uint8_t channel, uint8_t *data, size_t writeLength, size_t readLength);
// // <= UART wrapper

// // => CRC wrapper
// extern uint8_t tmc2209_CRC8(uint8_t *data, size_t length);
// // <= CRC wrapper

static UART_Config *TMC2209_UARTChannel;

static inline TMC2209TypeDef *motorToIC(uint8_t motor)
{
	UNUSED(motor);

	return &TMC2209;
}

static inline UART_Config *channelToUART(uint8_t channel)
{
	UNUSED(channel);

	return TMC2209_UARTChannel;
}

// => UART wrapper

int32_t UART_readWrite(UART_Config *uart, uint8_t *data, size_t writeLength, uint8_t readLength)
{
	uart->rxtx.clearBuffers();
	uart->rxtx.txN(data, writeLength);
	/* Workaround: Give the UART time to send. Otherwise another write/readRegister can do clearBuffers()
	 * before we're done. This currently is an issue with the IDE when using the Register browser and the
	 * periodic refresh of values gets requested right after the write request.
	 */
	wait(WRITE_READ_DELAY);

	// Abort early if no data needs to be read back
	if (readLength <= 0)
		return 0;

	// Wait for reply with timeout limit
	uint32_t timestamp = 0;
	// while(uart->rxtx.bytesAvailable() < readLength)
	// {
		// if(timeSince(timestamp) > UART_TIMEOUT_VALUE)
		// {
			// Abort on timeout
			// return -1;
		// }
	// }

	uart->rxtx.rxN(data, readLength);

	return 0;
}

void tmc2209_readWriteArray(uint8_t channel, uint8_t *data, size_t writeLength, size_t readLength)
{
	UART_readWrite(channelToUART(channel), data, writeLength, readLength);
}
// <= UART wrapper

// => CRC wrapper
// Return the CRC8 of [length] bytes of data stored in the [data] array.
uint8_t tmc2209_CRC8(uint8_t *data, size_t length)
{
	return TMC2209_CRC(data, length);
}
// <= CRC wrapper

void tmc2209_writeRegister(uint8_t motor, uint16_t address, int32_t value)
{
	tmc2209_writeInt(motorToIC(motor), (uint8_t) address, value);

}

void tmc2209_readRegister(uint8_t motor, uint16_t address, int32_t *value)
{
	*value = tmc2209_readInt(motorToIC(motor), (uint8_t) address);
}


void tmc2209_writeInt(TMC2209TypeDef *tmc2209, uint8_t address, int32_t value)
{
	uint8_t data[8];

	data[0] = 0x05;
	data[1] = tmc2209->slaveAddress;
	data[2] = address | TMC_WRITE_BIT;
	data[3] = (value >> 24) & 0xFF;
	data[4] = (value >> 16) & 0xFF;
	data[5] = (value >> 8 ) & 0xFF;
	data[6] = (value      ) & 0xFF;
	data[7] = tmc2209_CRC8(data, 7);

	tmc2209_readWriteArray(tmc2209->config->channel, &data[0], 8, 0);

	// Write to the shadow register and mark the register dirty
	address = TMC_ADDRESS(address);
	tmc2209->config->shadowRegister[address] = value;
	tmc2209->registerAccess[address] |= TMC_ACCESS_DIRTY;
}

int32_t tmc2209_readInt(TMC2209TypeDef *tmc2209, uint8_t address)
{
	uint8_t data[8] = { 0 };

	address = TMC_ADDRESS(address);

	if (!TMC_IS_READABLE(tmc2209->registerAccess[address]))
		return tmc2209->config->shadowRegister[address];

	data[0] = 0x05;
	data[1] = tmc2209->slaveAddress;
	data[2] = address;
	data[3] = tmc2209_CRC8(data, 3);

	tmc2209_readWriteArray(tmc2209->config->channel, data, 4, 8);

	// Byte 0: Sync nibble correct?
	if (data[0] != 0x05)
		return 0;

	// Byte 1: Master address correct?
	if (data[1] != 0xFF)
		return 0;

	// Byte 2: Address correct?
	if (data[2] != address)
		return 0;

	// Byte 7: CRC correct?
	if (data[7] != tmc2209_CRC8(data, 7))
		return 0;

	return ((uint32_t)data[3] << 24) | ((uint32_t)data[4] << 16) | (data[5] << 8) | data[6];
}

void tmc2209_init(TMC2209TypeDef *tmc2209, uint8_t channel, uint8_t slaveAddress, ConfigurationTypeDef *tmc2209_config, const int32_t *registerResetState)
{
	tmc2209->slaveAddress = slaveAddress;

	tmc2209->config               = tmc2209_config;
	tmc2209->config->callback     = NULL;
	tmc2209->config->channel      = channel;
	tmc2209->config->configIndex  = 0;
	tmc2209->config->state        = CONFIG_READY;

	for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
	{
		tmc2209->registerAccess[i]      = tmc2209_defaultRegisterAccess[i];
		tmc2209->registerResetState[i]  = registerResetState[i];
	}
}

static void writeConfiguration(TMC2209TypeDef *tmc2209)
{
	uint8_t *ptr = &tmc2209->config->configIndex;
	const int32_t *settings;

	if(tmc2209->config->state == CONFIG_RESTORE)
	{
		settings = tmc2209->config->shadowRegister;
		// Find the next restorable register
		while((*ptr < TMC2209_REGISTER_COUNT) && !TMC_IS_RESTORABLE(tmc2209->registerAccess[*ptr]))
		{
			(*ptr)++;
		}
	}
	else
	{
		settings = tmc2209->registerResetState;
		// Find the next resettable register
		while((*ptr < TMC2209_REGISTER_COUNT) && !TMC_IS_RESETTABLE(tmc2209->registerAccess[*ptr]))
		{
			(*ptr)++;
		}
	}

	if(*ptr < TMC2209_REGISTER_COUNT)
	{
		tmc2209_writeInt(tmc2209, *ptr, settings[*ptr]);
		(*ptr)++;
	}
	else // Finished configuration
	{
		if(tmc2209->config->callback)
		{
			((tmc2209_callback)tmc2209->config->callback)(tmc2209, tmc2209->config->state);
		}

		tmc2209->config->state = CONFIG_READY;
	}
}

void tmc2209_periodicJob(TMC2209TypeDef *tmc2209, uint32_t tick)
{
	UNUSED(tick);

	if(tmc2209->config->state != CONFIG_READY)
	{
		writeConfiguration(tmc2209);
		return;
	}
}

void tmc2209_setRegisterResetState(TMC2209TypeDef *tmc2209, const int32_t *resetState)
{
	for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
		tmc2209->registerResetState[i] = resetState[i];
}

void tmc2209_setCallback(TMC2209TypeDef *tmc2209, tmc2209_callback callback)
{
	tmc2209->config->callback = (tmc_callback_config) callback;
}

uint8_t tmc2209_reset(TMC2209TypeDef *tmc2209)
{
	if(tmc2209->config->state != CONFIG_READY)
		return false;

	// Reset the dirty bits and wipe the shadow registers
	for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
	{
		tmc2209->registerAccess[i] &= ~TMC_ACCESS_DIRTY;
		tmc2209->config->shadowRegister[i] = 0;
	}

	tmc2209->config->state        = CONFIG_RESET;
	tmc2209->config->configIndex  = 0;

	return true;
}

uint8_t tmc2209_restore(TMC2209TypeDef *tmc2209)
{
	if(tmc2209->config->state != CONFIG_READY)
		return false;

	tmc2209->config->state        = CONFIG_RESTORE;
	tmc2209->config->configIndex  = 0;

	return true;
}

uint8_t tmc2209_get_slave(TMC2209TypeDef *tmc2209)
{
	return tmc2209->slaveAddress;
}

void tmc2209_set_slave(TMC2209TypeDef *tmc2209, uint8_t slaveAddress)
{
	tmc2209->slaveAddress = slaveAddress;
}