#include <cstdint>
#include <cstddef>
#include <cassert>
#include <array>
#include <limits>
#include <climits>
#include <stm32f10x.h>
#include <stm32f10x_can.h>
#include <stm32f10x_gpio.h>
#include <misc.h>

template <typename T>
__attribute__ ((always_inline)) constexpr T cmask (const int count, int bit = 0) {
	return (count == sizeof (T) * CHAR_BIT) ? std::numeric_limits<T>::max ()
			: (((static_cast<T> ( 1 ) << count) - static_cast<T> (1)) << bit);
}

/*
 *
 * CAN1 (1 MBaud)
 * 	RX = PA11
 * 	TX = PA12
 *
 *
 * CAN2 (500 kBaud)
 *  RX = PB12
 *  TX = PB13
 *
 */

void initCAN ();

CAN_TxMailBox_TypeDef* getFreeMailbox (CAN_TypeDef* C) {
	uint32_t TSR = C->TSR;
	uint32_t code = (TSR >> 24) & 3;
	if (TSR & (1 << (26 + code)))	return &C->sTxMailBox [code];
	else							return nullptr;
}

class ID {
	public:
		using Raw = uint32_t;

		__attribute__ ((always_inline))  constexpr ID () : m_ID (0xFFFFFFFF) {}
		__attribute__ ((always_inline))  constexpr ID (bool IDE, uint32_t I) : m_ID (I << (IDE ? 3 : 21) | (IDE << 2)) {}
		__attribute__ ((always_inline))  constexpr ID (const ID& src) = default;

		__attribute__ ((always_inline))  constexpr bool enabled () const { return m_ID != 0xFFFFFFFF; }

		__attribute__ ((always_inline))  ID operator = (const ID& src) { m_ID = src.m_ID; return *this; }
		__attribute__ ((always_inline))  bool operator == (const ID& other) const { return m_ID == other.m_ID; }

		__attribute__ ((always_inline))  constexpr uint32_t hwFormat () const { return m_ID; }

		static __attribute__ ((always_inline))  constexpr ID fromHwFormat (uint32_t i) { return {i}; }
		static constexpr __attribute__ ((always_inline))  Raw encode (bool IDE, uint32_t I) { return ID { IDE, I}.hwFormat(); }
	private:
		__attribute__ ((always_inline))  constexpr ID (uint32_t hw) : m_ID (hw) {}

		uint32_t m_ID;
};

class Forward {
	public:
		__attribute__ ((always_inline)) constexpr Forward (ID rx, ID tx) : m_rx (rx), m_tx (tx), m_buffer {}, m_fill { false }, m_dlc {} {}

		__attribute__ ((always_inline)) constexpr ID rx () const { return m_rx; }
		__attribute__ ((always_inline)) constexpr ID tx () const { return m_tx; }

		ID m_rx, m_tx;
		uint64_t m_buffer;
		bool m_fill;
		uint8_t m_dlc;
};


std::array<Forward, 1> fw1to2 {{
	// Leite Nachricht mit Standard-ID 42 von CAN1 nach CAN2 mit Standard-ID 55 durch
	{ {false, 42}, {false, 55} },
}};

std::array<Forward, 1> fw2to1 {{
	// Leite Nachricht mit Extended-ID 0xFFFF von CAN1 nach CAN2 mit Standard-ID 3 durch
	{ { true, 0xFFFF }, { false, 3 }},
}};

uint8_t updateNetFilters (uint8_t iFilter, const Forward* msg, uint8_t size) {
	for (uint_fast8_t iNetMsg = 0; iNetMsg < size / 2; ++iNetMsg) {
		ID id1 = msg [2 * iNetMsg].rx (), id2 = msg [2 * iNetMsg + 1].rx ();

		CAN1->sFilterRegister [iFilter].FR1 = id1.hwFormat ();
		CAN1->sFilterRegister [iFilter].FR2 = id2.hwFormat ();

		++iFilter;
	}

	if (size % 2) {
		CAN1->sFilterRegister [iFilter].FR1 = msg [size - 1].rx ().hwFormat ();
		CAN1->sFilterRegister [iFilter].FR2 = 0xFFFFFFFF;

		++iFilter;
	}
	return iFilter;
}

void updateFilters () {
	// Set FINIT bit
	CAN1->FMR = 1;

	const uint32_t mask = cmask<uint32_t> ((fw1to2.size () + fw2to1.size () + 1) / 2);
	CAN1->FM1R = mask;
	CAN1->FS1R = mask;
	CAN1->FA1R = 0;
	CAN1->FFA1R = 0;

	uint_fast8_t iFilter = 0;

	iFilter = updateNetFilters (iFilter, fw1to2.data (), fw1to2.size ());
	iFilter = updateNetFilters (iFilter, fw2to1.data (), fw2to1.size ());

	CAN1->FA1R = mask;

	// Reset FINIT bit, assign filter banks to CAN1/CAN2
	CAN1->FMR = (static_cast <uint32_t> ((fw1to2.size () + 1) / 2) << 8);
}

int main () {
	__disable_irq ();
	RCC->APB1ENR |= RCC_APB1ENR_CAN1EN | RCC_APB1ENR_CAN2EN;
	RCC->APB2ENR |= RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN;

	initCAN ();

	__enable_irq ();

	while (1) {
		asm volatile ("wfi");
	}
}

/*
 * PA11 - CAN1_RXD
 * PA12 - CAN1_TXD
 *
 * PB12 - CAN2_RXD
 * PB13 - CAN2_TXD
 */
void initCAN () {
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
	GPIO_Init(GPIOA, &GPIO_InitStructure);


	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
	GPIO_Init(GPIOB, &GPIO_InitStructure);




	CAN_InitTypeDef CAN_InitStructure;
	/* CAN register init */
	CAN_DeInit (CAN1);
	CAN_DeInit (CAN2);

	updateFilters ();

	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;

	NVIC_InitStructure.NVIC_IRQChannel = CAN1_RX0_IRQn;
	NVIC_Init (&NVIC_InitStructure);

	NVIC_InitStructure.NVIC_IRQChannel = CAN1_TX_IRQn;
	NVIC_Init (&NVIC_InitStructure);

	NVIC_InitStructure.NVIC_IRQChannel = CAN2_RX0_IRQn;
	NVIC_Init (&NVIC_InitStructure);

	NVIC_InitStructure.NVIC_IRQChannel = CAN2_TX_IRQn;
	NVIC_Init (&NVIC_InitStructure);


	CAN_StructInit (&CAN_InitStructure);

	/* CAN cell init */
	CAN_InitStructure.CAN_TTCM = DISABLE;
	CAN_InitStructure.CAN_ABOM = ENABLE;
	CAN_InitStructure.CAN_AWUM = DISABLE;
	CAN_InitStructure.CAN_NART = DISABLE;
	CAN_InitStructure.CAN_RFLM = DISABLE;
	CAN_InitStructure.CAN_TXFP = DISABLE;
	CAN_InitStructure.CAN_Mode = CAN_Mode_Normal;
	CAN_InitStructure.CAN_SJW = CAN_SJW_2tq;

	/* CAN Baudrate = 125kbps (CAN clocked at 36 MHz) */
	CAN_InitStructure.CAN_BS1 = CAN_BS1_9tq;
	CAN_InitStructure.CAN_BS2 = CAN_BS2_8tq;
	CAN_InitStructure.CAN_Prescaler = 2;

	// Configure Filters...
	updateFilters ();

	if (CAN_Init (CAN1, &CAN_InitStructure) != CAN_InitStatus_Success)
		assert (false);


	CAN_InitStructure.CAN_Prescaler = 4;
	if (CAN_Init (CAN2, &CAN_InitStructure) != CAN_InitStatus_Success)
		assert (false);


	CAN_ITConfig(CAN1, CAN_IT_FMP0, ENABLE);
	CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE);

	CAN_ITConfig(CAN2, CAN_IT_FMP0, ENABLE);
	CAN_ITConfig(CAN2, CAN_IT_TME, ENABLE);
}

void onReceive (CAN_TypeDef* C, Forward* msg, size_t count) {
	CAN_FIFOMailBox_TypeDef* const mb = &C->sFIFOMailBox [0];

	while (C->RF0R & 3) {
		uint32_t RDTR = mb->RDTR;
		uint8_t FMI = (RDTR >> 8) & 0xFF;
		uint8_t DLC = RDTR & 0xF;

		// Kein Fehler wenn 2. Eintrag des letzten Filters...
		assert ((FMI < count) || (count % 2 == 1 && FMI <= count));
		// ... aber ignorieren
		if (FMI < count) {
			assert (ID::fromHwFormat (mb->RIR & ~2) == msg [FMI].rx ());

			msg [FMI].m_buffer = mb->RDR;
			msg [FMI].m_fill = true;
			msg [FMI].m_dlc = DLC;
		}
		C->RF0R = CAN_RF0R_RFOM0;
	}
}

void onTransmit (CAN_TypeDef* C, Forward* msg, size_t count) {
	C->TSR = CAN_TSR_RQCP2 | CAN_TSR_RQCP1 | CAN_TSR_RQCP0;
	CAN_TxMailBox_TypeDef* nextMailbox = getFreeMailbox (C);
	if (nextMailbox == nullptr) return;

	for (size_t i = 0; i < count; ++i) {
		if (msg [i].m_fill) {
			nextMailbox->TDR = msg [i].m_buffer;
			nextMailbox->TDTR = msg [i].m_dlc;
			nextMailbox->TIR = msg [i].tx ().hwFormat () | 1;
			msg [i].m_fill = false;
			nextMailbox = getFreeMailbox (C);
			if (nextMailbox == nullptr)
				return;
		}
	}
}

extern "C" void CAN1_TX_IRQHandler () {
	onTransmit (CAN1, fw2to1.data (), fw2to1.size ());
}

extern "C" void CAN1_RX0_IRQHandler () {
	onReceive (CAN1, fw1to2.data (), fw1to2.size ());
	onTransmit (CAN2, fw1to2.data (), fw1to2.size ());
}

extern "C" void CAN2_TX_IRQHandler () {
	onTransmit (CAN2, fw1to2.data (), fw1to2.size ());
}

extern "C" void CAN2_RX0_IRQHandler () {
	onReceive (CAN2, fw2to1.data (), fw2to1.size ());
	onTransmit (CAN1, fw2to1.data (), fw2to1.size ());
}