/**
 * @section License
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2017, Thomas Barth, barth-dev.de
 *               2017, Jaime Breva, jbreva@nayarsystems.com
 * 				 2018, Michael Wagner, mw@iot-make.de
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "CAN.h"

#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"

// #include "esp_intr.h"
#include "esp_intr_alloc.h"
#include "soc/dport_reg.h"
#include <math.h>

#include "driver/gpio.h"

#include "can_regdef.h"
#include "CAN_config.h"

// CAN Filter - no acceptance filter
static CAN_filter_t __filter = { Dual_Mode, 0, 0, 0, 0, 0Xff, 0Xff, 0Xff, 0Xff };

static void CAN_read_frame_phy();
static void CAN_isr(void *arg_p);
static int CAN_write_frame_phy(const CAN_frame_t *p_frame);
static SemaphoreHandle_t sem_tx_complete;

static void CAN_isr(void *arg_p) {

	// Interrupt flag buffer
	__CAN_IRQ_t interrupt;
	BaseType_t higherPriorityTaskWoken = pdFALSE;

	// Read interrupt status and clear flags
	interrupt = MODULE_CAN->IR.U;

	// Handle RX frame available interrupt
	if ((interrupt & __CAN_IRQ_RX) != 0)
		CAN_read_frame_phy(&higherPriorityTaskWoken);

	// Handle TX complete interrupt
	// Handle error interrupts.
	if ((interrupt & (__CAN_IRQ_TX | __CAN_IRQ_ERR //0x4
	                  | __CAN_IRQ_DATA_OVERRUN     // 0x8
	                  | __CAN_IRQ_WAKEUP           // 0x10
	                  | __CAN_IRQ_ERR_PASSIVE      // 0x20
	                  | __CAN_IRQ_ARB_LOST         // 0x40
	                  | __CAN_IRQ_BUS_ERR          // 0x80
	                  )) != 0) {
		xSemaphoreGiveFromISR(sem_tx_complete, &higherPriorityTaskWoken);
	}

	// check if any higher priority task has been woken by any handler
	if (higherPriorityTaskWoken)
		portYIELD_FROM_ISR();
}

static void CAN_read_frame_phy(BaseType_t *higherPriorityTaskWoken) {

	// byte iterator
	uint8_t __byte_i;

	// frame read buffer
	CAN_frame_t __frame;

	// check if we have a queue. If not, operation is aborted.
	if (CAN_cfg.rx_queue == NULL) {
		// Let the hardware know the frame has been read.
		MODULE_CAN->CMR.B.RRB = 1;
		return;
	}

	// get FIR
	__frame.FIR.U = MODULE_CAN->MBX_CTRL.FCTRL.FIR.U;

	// check if this is a standard or extended CAN frame
	// standard frame
	if (__frame.FIR.B.FF == CAN_frame_std) {

		// Get Message ID
		__frame.MsgID = _CAN_GET_STD_ID;

		// deep copy data bytes
		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i];
	}
	// extended frame
	else {

		// Get Message ID
		__frame.MsgID = _CAN_GET_EXT_ID;

		// deep copy data bytes
		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i];
	}

	// send frame to input queue
	xQueueSendToBackFromISR(CAN_cfg.rx_queue, &__frame, higherPriorityTaskWoken);

	// Let the hardware know the frame has been read.
	MODULE_CAN->CMR.B.RRB = 1;
}

static int CAN_write_frame_phy(const CAN_frame_t *p_frame) {

	// byte iterator
	uint8_t __byte_i;

	// copy frame information record
	MODULE_CAN->MBX_CTRL.FCTRL.FIR.U = p_frame->FIR.U;

	// standard frame
	if (p_frame->FIR.B.FF == CAN_frame_std) {

		// Write message ID
		_CAN_SET_STD_ID(p_frame->MsgID);

		// Copy the frame data to the hardware
		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i] = p_frame->data.u8[__byte_i];

	}
	// extended frame
	else {

		// Write message ID
		_CAN_SET_EXT_ID(p_frame->MsgID);

		// Copy the frame data to the hardware
		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i] = p_frame->data.u8[__byte_i];
	}

	// Transmit frame
	MODULE_CAN->CMR.B.TR = 1;

	return 0;
}

int CAN_init() {

	// Time quantum
	double __tq;

	// enable module
	DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_CAN_CLK_EN);
	DPORT_SET_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST);
	DPORT_CLEAR_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST);

	// configure TX pin
	gpio_set_level(CAN_cfg.tx_pin_id, 1);
	gpio_set_direction(CAN_cfg.tx_pin_id, GPIO_MODE_OUTPUT);
	gpio_matrix_out(CAN_cfg.tx_pin_id, CAN_TX_IDX, 0, 0);
	gpio_pad_select_gpio(CAN_cfg.tx_pin_id);

	// configure RX pin
	gpio_set_direction(CAN_cfg.rx_pin_id, GPIO_MODE_INPUT);
	gpio_matrix_in(CAN_cfg.rx_pin_id, CAN_RX_IDX, 0);
	gpio_pad_select_gpio(CAN_cfg.rx_pin_id);

	// set to PELICAN mode
	MODULE_CAN->CDR.B.CAN_M = 0x1;

	// synchronization jump width is the same for all baud rates
	MODULE_CAN->BTR0.B.SJW = 0x1;

	// TSEG2 is the same for all baud rates
	MODULE_CAN->BTR1.B.TSEG2 = 0x1;

	// select time quantum and set TSEG1
	switch (CAN_cfg.speed) {
	case CAN_SPEED_1000KBPS:
		MODULE_CAN->BTR1.B.TSEG1 = 0x4;
		__tq = 0.125;
		break;

	case CAN_SPEED_800KBPS:
		MODULE_CAN->BTR1.B.TSEG1 = 0x6;
		__tq = 0.125;
		break;

	case CAN_SPEED_200KBPS:
		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
		MODULE_CAN->BTR1.B.TSEG2 = 0x5;
		__tq = 0.25;
		break;

	default:
		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
		__tq = ((float) 1000 / CAN_cfg.speed) / 16;
	}

	// set baud rate prescaler
	MODULE_CAN->BTR0.B.BRP = (uint8_t) round((((APB_CLK_FREQ * __tq) / 2) - 1) / 1000000) - 1;

	/* Set sampling
	 * 1 -> triple; the bus is sampled three times; recommended for low/medium speed buses     (class A and B) where
	 * filtering spikes on the bus line is beneficial 0 -> single; the bus is sampled once; recommended for high speed
	 * buses (SAE class C)*/
	MODULE_CAN->BTR1.B.SAM = 0x1;

	// enable all interrupts
	MODULE_CAN->IER.U = 0xef;

	 // Set acceptance filter	
	MODULE_CAN->MOD.B.AFM = __filter.FM;	
    MODULE_CAN->MBX_CTRL.ACC.CODE[0] = __filter.ACR0;
    MODULE_CAN->MBX_CTRL.ACC.CODE[1] = __filter.ACR1;
    MODULE_CAN->MBX_CTRL.ACC.CODE[2] = __filter.ACR2;
    MODULE_CAN->MBX_CTRL.ACC.CODE[3] = __filter.ACR3;
    MODULE_CAN->MBX_CTRL.ACC.MASK[0] = __filter.AMR0;
    MODULE_CAN->MBX_CTRL.ACC.MASK[1] = __filter.AMR1;
    MODULE_CAN->MBX_CTRL.ACC.MASK[2] = __filter.AMR2;
    MODULE_CAN->MBX_CTRL.ACC.MASK[3] = __filter.AMR3;

	// set to normal mode
	MODULE_CAN->OCR.B.OCMODE = __CAN_OC_NOM;

	// clear error counters
	MODULE_CAN->TXERR.U = 0;
	MODULE_CAN->RXERR.U = 0;
	(void) MODULE_CAN->ECC;

	// clear interrupt flags
	(void) MODULE_CAN->IR.U;

	// install CAN ISR
	esp_intr_alloc(ETS_CAN_INTR_SOURCE, 0, CAN_isr, NULL, NULL);

	// allocate the tx complete semaphore
	sem_tx_complete = xSemaphoreCreateBinary();

	// Showtime. Release Reset Mode.
	MODULE_CAN->MOD.B.RM = 0;

	return 0;
}

int CAN_write_frame(const CAN_frame_t *p_frame) {
	if (sem_tx_complete == NULL) {
		return -1;
	}

	// Write the frame to the controller
	CAN_write_frame_phy(p_frame);

	// wait for the frame tx to complete
	xSemaphoreTake(sem_tx_complete, portMAX_DELAY);

	return 0;
}

int CAN_stop() {
	// enter reset mode
	MODULE_CAN->MOD.B.RM = 1;

	return 0;
}

int CAN_config_filter(const CAN_filter_t* p_filter) {
	
	__filter.FM = p_filter->FM;	
    __filter.ACR0 = p_filter->ACR0;
    __filter.ACR1 = p_filter->ACR1;
    __filter.ACR2 = p_filter->ACR2;
    __filter.ACR3 = p_filter->ACR3;
    __filter.AMR0 = p_filter->AMR0;
    __filter.AMR1 = p_filter->AMR1;
    __filter.AMR2 = p_filter->AMR2;
    __filter.AMR3 = p_filter->AMR3;
	
	return 0;
}