/**
 * @file   AdjTask.c
 * @brief  Source file of adjustment task.
 * @par    Programming Language
 *         C
 */
#include "AdjTask.h"
#include "AdjSpec.h"
#include "spi.h"
#include "timer.h"
#include "i2c.h"

#define ADJ_QUEUE_LEN		16u
#define ADJ_QUEUE_SIZE	sizeof(uint16_t)

#ifdef AX_MAIN
QueueHandle_t ADJ_Queue = NULL;	/* QUEUEハンドル※ドライバ側でexternしているため名前変更禁止 */
#endif
static TaskHandle_t AdjTaskHdl = NULL;

// lisoc_test.cからコピー
static const uint16_t crc_table[16u] =
{
  0x0000u, 0x1021u, 0x2042u, 0x3063u, 0x4084u, 0x50a5u, 0x60c6u, 0x70e7u,
  0x8108u, 0x9129u, 0xa14au, 0xb16bu, 0xc18cu, 0xd1adu, 0xe1ceu, 0xf1efu,
};

static spis_msg_t adjMsg;


// lisoc_test.cからコピー
/**************************************************************************
* Description:  calc CCITT CRC 16
* Input:        pMsg, pointer to the message being checked.
* Output:       none
* Return:       1= pdPASS,    0= pdFAIL
**************************************************************************/
static uint16_t calc_crc(uint8_t *data, uint16_t len)
{
  /* https://blog.csdn.net/j824117879/article/details/82019545 */
  uint16_t crc16;
  uint16_t crc_h4;
  uint16_t crc_l12;

  crc16 = 0x0000u;

  while (len) {
    crc_h4 = (crc16 >> 12u);
    crc_l12 = (crc16 << 4u);
    crc16 = crc_l12 ^ crc_table[crc_h4 ^ (*data >> 4u)];
    crc_h4 = (crc16 >> 12u);
    crc_l12 = (crc16 << 4u);
    crc16 = crc_l12 ^ crc_table[crc_h4 ^ (*data & 0x0fu)];

    data ++;
    len --;
  }

  return crc16;

}


// lisoc_test.cからコピー
/**************************************************************************
* Description:  check SPI slave message CRC
* Input:        pMsg, pointer to the message being checked.
* Output:       none
* Return:       1= pdPASS,    0= pdFAIL
**************************************************************************/
static BaseType_t check_crc(spis_msg_t *pMsg)
{
  /* https://blog.csdn.net/j824117879/article/details/82019545 */
  uint8_t *data;
  uint16_t len;
  uint16_t crc16;

  data = (uint8_t *)pMsg;   /* crc range = len field + cmd field + param field */
  len = 2u * (SPIS_MSG_LEN_SZ + pMsg->len);

  crc16 = calc_crc(data, len);

  if (crc16 == pMsg->data[pMsg->len - SPIS_MSG_CMD_SZ])
  {
    return pdPASS;
  }
  else
  {
    return pdFAIL;
  }

}
 

//
//	調整タスク初期化
//
BaseType_t adjTaskInit(void)
{
	BaseType_t xRet = pdFAIL;
	
	/* Create timers */
	spis_txtmo_tmr = xTimerCreate("TxTmo",				/* 名前 */
																1u,							/* タイマー期間(tick) */
																pdFALSE,				/* AutoReload(pdTRUE:auto-reload / pdFALSE:one-shot) */
																NULL,						/* タイマーID */
																spis_cb_txtmo);	/* コールバック関数ポインタ */
	if (spis_txtmo_tmr == NULL) {
		return xRet;
	}
	
	/* Create message queue */
	ADJ_Queue = xQueueCreate((UBaseType_t)ADJ_QUEUE_LEN,			/* Queueデータ数 */
														(UBaseType_t)ADJ_QUEUE_SIZE);		/* 各データサイズ*/
	if (ADJ_Queue == NULL) {
		return xRet;
	}
	
	/* Create adjTask task */
	xRet = xTaskCreate((TaskFunction_t)adjTask,							/* エントリー関数ポインタ */
											(const char*)"AdjTask",							/* 名前 */
											(uint16_t)512,											/* 使用スタックサイズ */
											(void*)NULL,												/* 関数パラメータポインタ */
											(UBaseType_t)configMAX_PRIORITIES,	/* 優先順位 */
											(TaskHandle_t*)AdjTaskHdl);					/* タスク参照ハンドル */
	if (xRet != pdPASS) {
		return xRet;
	}
	
	return xRet;
}
 

//
//	調整タスク処理
//　（lisoc_test.c内調整タスクと同等）
//
void adjTask(void *param)
{
	BaseType_t xRet = pdFAIL;
	BaseType_t retMsg = pdFALSE;
	uint16_t rcvCmd;
	uint16_t rspMsg[4u];
	uint16_t len;
	uint16_t i;
	uint32_t asicAdr;
	uint32_t asicDat;
	uint16_t sensAdr;
	uint8_t sensDat;
	uint8_t vcmAdr;
	uint16_t vcmDat;
	
	spis_rx();
	
	while(1) {
		xRet = xQueueReceive(ADJ_Queue, &rcvCmd, portMAX_DELAY);
		if (xRet == pdPASS) {
			switch (rcvCmd) {
				case SPIS_RX_MSG_RCVD:
					len = spis_rxbuf_rd();
					if ((len >= SPIS_MSG_CMD_SZ) && (len <= SPIS_MSG_PAYLOAD_SZ)) {
						/* Move message from Ring buffer to local buffer */
						adjMsg.len = len;
						adjMsg.cmd = spis_rxbuf_rd();
						for (i = 0u; i < len - SPIS_MSG_CMD_SZ + SPIS_MSG_CRC_SZ; i++) {
							adjMsg.data[i] = spis_rxbuf_rd();
						}
						
						/* Check CRC */
						retMsg = check_crc(&adjMsg);
						if (retMsg == pdTRUE) {
							if ((adjMsg.cmd == (uint16_t)SPIS_REQ_RD_SYS_PAR) ||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_RD_ISP_PAR) ||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_RD_SENS_PAR)	||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_RD_VCM_PAR)) {
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_RD_ISP_PAR) {		// ISP read
											asicAdr = (adjMsg.data[0] << 16) | adjMsg.data[1];
											asicDat = getSpec(asicAdr);
											
											/* Send reply message */
											rspMsg[0u] = adjMsg.cmd + 1u;
											rspMsg[1u] = (asicDat & 0xFFFF0000) >> 16;
											rspMsg[2u] = (asicDat & 0x0000FFFF);
											rspMsg[3u] = calc_crc((uint8_t *)&rspMsg[0u], 6);
											xRet = spis_tx(&rspMsg[0u], 4u, SPIS_TX_TMO_MS);
											if (xRet == pdFAIL) {
												/* TBD Failsafe processing */
											}
										}
										
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_RD_SENS_PAR) {		// SENSOR read
											sensAdr = adjMsg.data[0];
											i2c_reg16_read(LISOC_I2C0, sensAdr, &sensDat);
											
											/* Send reply message */
											rspMsg[0u] = adjMsg.cmd + 1u;
											rspMsg[1u] = sensDat;
											rspMsg[2u] = calc_crc((uint8_t *) &rspMsg[0u], 4);
											xRet = spis_tx(&rspMsg[0u], 3u, SPIS_TX_TMO_MS);
											if (xRet == pdFAIL) {
												/* TBD Failsafe processing */
											}
										}

										if (adjMsg.cmd == (uint16_t)SPIS_REQ_RD_VCM_PAR) {		// VCM read
											vcmAdr = adjMsg.data[0] >> 8;
											i2c_vcm_read(LISOC_I2C0, vcmAdr, &vcmDat);
											
											/* Send reply message */
											rspMsg[0u] = adjMsg.cmd + 1u;
											rspMsg[1u] = vcmDat;
											rspMsg[2u] = calc_crc((uint8_t *) &rspMsg[0u], 4);
											xRet = spis_tx(&rspMsg[0u], 3u, SPIS_TX_TMO_MS);
											if (xRet == pdFAIL) {
												/* TBD Failsafe processing */
											}
										}
							}
							
							if ((adjMsg.cmd == (uint16_t)SPIS_REQ_WR_SYS_PAR) ||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_WR_ISP_PAR) ||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_WR_RISr_PAR)	||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_WR_SENS_PAR)	||
									(adjMsg.cmd == (uint16_t)SPIS_REQ_WR_VCM_PAR)) {
										/* TBD */
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_WR_ISP_PAR) {			// ISP write
											asicAdr = (adjMsg.data[0] << 16) | adjMsg.data[1];
											asicDat = (adjMsg.data[2] << 16) | adjMsg.data[3];
											setSpec(asicAdr, asicDat);
										}
										
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_WR_SENS_PAR) {			// SENSOR write
											sensAdr = adjMsg.data[0];
											sensDat = adjMsg.data[1] & 0xFF;
											i2c_reg16_write(LISOC_I2C0, sensAdr, sensDat);
										}
										
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_WR_VCM_PAR) {			// VCM write
											vcmDat = adjMsg.data[0];
											i2c_vcm_write(LISOC_I2C0, vcmDat);
										}
										
										if (adjMsg.cmd == (uint16_t)SPIS_REQ_WR_RISr_PAR) {			// RISr write
											asicAdr = (adjMsg.data[0] << 16) | adjMsg.data[1];
											for (i = 0; i < (adjMsg.len - 3) / 2; i++) {
												asicDat = (adjMsg.data[2 + 2 * i] << 16) | adjMsg.data[3 + 2 * i];
												setSpec(asicAdr, asicDat);
												asicAdr += 4;
											}
										}
										
										/* Send reply message */
										rspMsg[0u] = adjMsg.cmd + 1u;
										rspMsg[1u] = calc_crc((uint8_t *)&rspMsg[0u], 2);
										xRet = spis_tx(&rspMsg[0u], 2u, SPIS_TX_TMO_MS);
										if (xRet == pdFAIL) {
											/* TBD Failsafe processing */
										}
							}
							
							/* Send the message to ISP task */
							xRet = xQueueSend(ISP_Queue,		/* Queueハンドル */
																&adjMsg.cmd,	/* 送信データポインタ */
																0);						/* 最大待ち時間 */
						} else {
							/* Send reply message */
							rspMsg[0u] = SPIS_RSP_CRC_ERR;
							rspMsg[1u] = calc_crc((uint8_t*) & rspMsg[0u], sizeof(uint16_t));
							xRet = spis_tx(&rspMsg[0u], 2u, SPIS_TX_TMO_MS);
						}
					} else {
						/* Send reply message */
						rspMsg[0u] = SPIS_RSP_FIFO_ERR;
						rspMsg[1u] = calc_crc((uint8_t*) & rspMsg[0u], sizeof(uint16_t));
						xRet = spis_tx(&rspMsg[0u], 2u, SPIS_TX_TMO_MS);
					}
					break;
				case SPIS_RX_LEN_ERR:
					/* Send reply message */
					rspMsg[0u] = SPIS_RSP_LEN_ERR;
					rspMsg[1u] = calc_crc((uint8_t*) & rspMsg[0u], sizeof(uint16_t));
					xRet = spis_tx(&rspMsg[0u], 2u, SPIS_TX_TMO_MS);
					break;
				case SPIS_RX_FIFO_OVRFLW:
					/* Send reply message */
					rspMsg[0u] = SPIS_RSP_FIFO_ERR;
					rspMsg[1u] = calc_crc((uint8_t*) & rspMsg[0u], sizeof(uint16_t));
					xRet = spis_tx(&rspMsg[0u], 2u, SPIS_TX_TMO_MS);
					break;
				case SPIS_TX_MSG_SENT:
					/* Stop Tx time-out timer */
					xTimerStop(spis_txtmo_tmr, 0);
				
					/* Switch back to Rx mode. Get ready for new Rx message */
					spis_rx();
					break;
				case SPIS_TX_MSG_TMO:
					if (spis_state == SPIS_ST_TX) {
						/* Switch back to Rx mode. Get ready for new Rx message */
						spis_rx();
					} else {
						/*Spurious message. Ignore it and do nothing */
					}
				default:
					break;
			}
		}
	}
}