#pragma once

#include "../clock.hpp"

#include "../util/type.hpp"

#include <math.h>
#include <stdint.h>

#define FORCE_INLINE __attribute__((always_inline))

namespace uart {

enum class Mode {
	ASYNCHRONOUS,
	SYNCHRONOUS_MASTER,
	SYNCHRONOUS_SLAVE,
	SPI,
};

enum class Driven {
	INTERRUPT,
	BLOCKING,
};

namespace detail {

using reg_ptr_t = volatile uint8_t *;

template <uintptr_t Address>
static inline reg_ptr_t getRegPtr()
{
	return reinterpret_cast<reg_ptr_t>(Address);
}

template <typename data_t, uint8_t Size>
struct RingBuffer {
	uint8_t head;
	uint8_t tail;
	data_t buf[Size];
};

template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Driven driven,
          Mode mode>
class Hardware {
  public:
	static void init() FORCE_INLINE
	{
		constexpr auto AbsDoubleError = fabs(calcBaudError<true>());
		constexpr auto AbsNormalError = fabs(calcBaudError<false>());
		static_assert(AbsDoubleError <= 3.0 || AbsNormalError <= 3.0, "Baud rate error over 3%, probably unusable");

		constexpr auto UseDoubleSpeed = (AbsDoubleError < AbsNormalError);
		constexpr auto BaudVal = calcBaudVal<UseDoubleSpeed>();

		*getRegPtr<Registers::BAUD_REG_H_ADDR>() = static_cast<uint8_t>(BaudVal >> 8);
		*getRegPtr<Registers::BAUD_REG_L_ADDR>() = static_cast<uint8_t>(BaudVal);

		constexpr auto DataBitsValues = calcDataBits();
		constexpr auto ParityVal = calcParity();
		constexpr auto StopBitsVal = calcStopBits();
		constexpr auto ModeVal = calcMode();
		constexpr auto EnableRx = calcRxState<true>();
		constexpr auto EnableTx = calcTxState<true>();
		constexpr auto InterruptVal = calcInterrupt();

		constexpr uint8_t ControlRegB = DataBitsValues.regBVal | EnableRx | EnableTx | InterruptVal;
		constexpr uint8_t ControlRegC = DataBitsValues.regCVal | ParityVal | StopBitsVal | ModeVal;

		if constexpr (UseDoubleSpeed)
			*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() |= (1 << CtrlFlagsA::SPEED_2X);
		else
			*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() &= ~(1 << CtrlFlagsA::SPEED_2X);

		*getRegPtr<Registers::CTRL_STAT_REG_B_ADDR>() = ControlRegB;
		*getRegPtr<Registers::CTRL_STAT_REG_C_ADDR>() = ControlRegC;
	}

	static bool rxByteBlocking(typename cfg::data_t &byte) FORCE_INLINE
	{
		if (*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() & (1 << CtrlFlagsA::RECEIVE_COMPLETE)) {
			byte = *getRegPtr<Registers::IO_REG_ADDR>();
			return true;
		}

		return false;
	}

	static typename cfg::data_t rxByteInterrupt() FORCE_INLINE
	{
		return *getRegPtr<Registers::IO_REG_ADDR>();
	}

	static bool txEmpty() FORCE_INLINE
	{
		return *getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() & (1 << CtrlFlagsA::DATA_REG_EMPTY);
	}

	static bool txComplete() FORCE_INLINE
	{
		return *getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() & (1 << CtrlFlagsA::TRANSMIT_COMPLETE);
	}

	static void clearTxComplete() FORCE_INLINE
	{
		*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() |= (1 << CtrlFlagsA::TRANSMIT_COMPLETE);
	}

	static void txByteBlocking(const typename cfg::data_t &byte) FORCE_INLINE
	{
		while (!txEmpty())
			;

		*getRegPtr<Registers::IO_REG_ADDR>() = byte;
	}

	static void txByteInterrupt(volatile const typename cfg::data_t &byte) FORCE_INLINE
	{
		*getRegPtr<Registers::IO_REG_ADDR>() = byte;
	}

	static bool peekBlocking() FORCE_INLINE
	{
		if (*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() & (1 << CtrlFlagsA::RECEIVE_COMPLETE)) {
			return true;
		}

		return false;
	}

	static void enableDataRegEmptyInt() FORCE_INLINE
	{
		*getRegPtr<Registers::CTRL_STAT_REG_B_ADDR>() |= (1 << CtrlFlagsB::DATA_REG_EMPTY_INT_ENABLE);
	}

	static void disableDataRegEmptyInt() FORCE_INLINE
	{
		*getRegPtr<Registers::CTRL_STAT_REG_B_ADDR>() &= ~(1 << CtrlFlagsB::DATA_REG_EMPTY_INT_ENABLE);
	}

  private:
	struct DataBitsVal {
		uint8_t regCVal = 0;
		uint8_t regBVal = 0;
	};

	template <bool DoubleSpeed = true>
	static constexpr auto calcBaudVal()
	{
		if constexpr (DoubleSpeed) {
			constexpr auto BaudVal = static_cast<uint16_t>(round(F_CPU / (8.0 * cfg::BAUD_RATE) - 1));
			return BaudVal;
		}

		constexpr auto BaudVal = static_cast<uint16_t>(round(F_CPU / (16.0 * cfg::BAUD_RATE) - 1));
		return BaudVal;
	}

	template <uint16_t BaudVal, bool DoubleSpeed = true>
	static constexpr auto calcBaudRate()
	{
		if constexpr (DoubleSpeed) {
			constexpr auto BaudRate = static_cast<uint32_t>(round(F_CPU / (8.0 * (BaudVal + 1))));
			return BaudRate;
		}

		constexpr auto BaudRate = static_cast<uint32_t>(round(F_CPU / (16.0 * (BaudVal + 1))));
		return BaudRate;
	}

	template <bool DoubleSpeed = true>
	static constexpr auto calcBaudError()
	{
		constexpr auto BaudVal = calcBaudVal<DoubleSpeed>();
		constexpr auto ClosestBaudRate = calcBaudRate<BaudVal, DoubleSpeed>();
		constexpr auto BaudError = (static_cast<double>(ClosestBaudRate) / cfg::BAUD_RATE - 1) * 100;
		return BaudError;
	}

	static constexpr auto calcDataBits()
	{
		DataBitsVal dataBitsVal;

		switch (cfg::DATA_BITS) {
		case DataBits::FIVE:
			dataBitsVal.regCVal = 0;
			break;
		case DataBits::SIX:
			dataBitsVal.regCVal = (1 << CtrlFlagsC::CHAR_SIZE_0);
			break;
		case DataBits::SEVEN:
			dataBitsVal.regCVal = (1 << CtrlFlagsC::CHAR_SIZE_1);
			break;
		case DataBits::EIGHT:
			dataBitsVal.regCVal = (1 << CtrlFlagsC::CHAR_SIZE_1) | (1 << CtrlFlagsC::CHAR_SIZE_0);
			break;
		case DataBits::NINE:
			dataBitsVal.regCVal = (1 << CtrlFlagsC::CHAR_SIZE_1) | (1 << CtrlFlagsC::CHAR_SIZE_0);
			dataBitsVal.regBVal = (1 << CtrlFlagsB::CHAR_SIZE_2);
			break;
		}

		return dataBitsVal;
	}

	static constexpr auto calcParity()
	{
		uint8_t parityVal = 0;

		if (cfg::PARITY == Parity::EVEN)
			parityVal = (1 << CtrlFlagsC::PARITY_MODE_1);
		else if (cfg::PARITY == Parity::ODD)
			parityVal = (1 << CtrlFlagsC::PARITY_MODE_1) | (1 << CtrlFlagsC::PARITY_MODE_0);

		return parityVal;
	}

	static constexpr auto calcStopBits()
	{
		uint8_t stopBitsVal = 0;

		if (cfg::STOP_BITS == StopBits::TWO)
			stopBitsVal = (1 << CtrlFlagsC::STOP_BIT_SEL);

		return stopBitsVal;
	}

	static constexpr auto calcMode()
	{
		static_assert(mode != Mode::SPI, "SPI mode can not be used with uart");

		uint8_t modeVal = 0;

		if (mode == Mode::SYNCHRONOUS_MASTER || mode == Mode::SYNCHRONOUS_SLAVE) {
			modeVal = (1 << CtrlFlagsC::MODE_SEL_0);
		}

		return modeVal;
	}

	template <bool Enable>
	static constexpr auto calcRxState()
	{
		uint8_t enableVal = 0;

		if (Enable)
			enableVal = (1 << CtrlFlagsB::RX_ENABLE);

		return enableVal;
	}

	template <bool Enable>
	static constexpr auto calcTxState()
	{
		uint8_t enableVal = 0;

		if (Enable)
			enableVal = (1 << CtrlFlagsB::TX_ENABLE);

		return enableVal;
	}

	static constexpr auto calcInterrupt()
	{
		uint8_t interruptVal = 0;

		if (driven == Driven::INTERRUPT)
			interruptVal = (1 << CtrlFlagsB::RX_INT_ENABLE);

		return interruptVal;
	}
};

template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Mode mode>
class BlockingHardware {
  public:
	using data_t = typename cfg::data_t;
	static constexpr auto DATA_BITS = cfg::DATA_BITS;

	static void init() FORCE_INLINE
	{
		HardwareImpl::init();
	}

	static void txByte(const data_t &byte) FORCE_INLINE
	{
		HardwareImpl::txByteBlocking(byte);
	}

	static bool rxByte(data_t &byte) FORCE_INLINE
	{
		return HardwareImpl::rxByteBlocking(byte);
	}

	static bool peek(data_t &) FORCE_INLINE
	{
		static_assert(util::always_false_v<data_t>, "Peek with data is not supported in blocking mode");
		return false;
	}

	static bool peek() FORCE_INLINE
	{
		return HardwareImpl::peekBlocking();
	}

	static void flushTx() FORCE_INLINE
	{
		while (!HardwareImpl::txEmpty())
			;
		while (!HardwareImpl::txComplete())
			;
		HardwareImpl::clearTxComplete();
	}

  private:
	using HardwareImpl = Hardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, Driven::BLOCKING, mode>;
};

template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Mode mode>
class InterruptHardware {
  public:
	using data_t = typename cfg::data_t;
	static constexpr auto DATA_BITS = cfg::DATA_BITS;

	static void txByte(const data_t &byte) FORCE_INLINE
	{
		uint8_t tmpHead = (sm_txBuf.head + 1) % TX_BUFFER_SIZE;
		while (tmpHead == sm_txBuf.tail)
			;

		sm_txBuf.buf[tmpHead] = byte;
		sm_txBuf.head = tmpHead;

		HardwareImpl::enableDataRegEmptyInt();
	}

	static bool rxByte(data_t &byte) FORCE_INLINE
	{
		if (sm_rxBuf.head == sm_rxBuf.tail)
			return false;

		uint8_t tmpTail = (sm_rxBuf.tail + 1) % RX_BUFFER_SIZE;
		byte = sm_rxBuf.buf[tmpTail];
		sm_rxBuf.tail = tmpTail;

		return true;
	}

	static bool peek(data_t &byte) FORCE_INLINE
	{
		if (sm_rxBuf.head == sm_rxBuf.tail)
			return false;

		uint8_t tmpTail = (sm_rxBuf.tail + 1) % RX_BUFFER_SIZE;
		byte = sm_rxBuf.buf[tmpTail];

		return true;
	}

	static bool peek() FORCE_INLINE
	{
		return (sm_rxBuf.head != sm_rxBuf.tail);
	}

	static void flushTx() FORCE_INLINE
	{
		while (sm_txBuf.head != sm_txBuf.tail)
			;
		while (!HardwareImpl::txEmpty())
			;
		while (!HardwareImpl::txComplete())
			;
		HardwareImpl::clearTxComplete();
	}

  protected:
	static void rxIntHandler() FORCE_INLINE
	{
		const auto data = HardwareImpl::rxByteInterrupt();

		const uint8_t tmpHead = (sm_rxBuf.head + 1) % RX_BUFFER_SIZE;

		if (tmpHead != sm_rxBuf.tail) {
			sm_rxBuf.head = tmpHead;
			sm_rxBuf.buf[tmpHead] = data;
		} else {
			// TODO: Handle overflow
		}
	}

	static void dataRegEmptyIntHandler() FORCE_INLINE
	{
		if (sm_txBuf.head != sm_txBuf.tail) {
			const uint8_t tmpTail = (sm_txBuf.tail + 1) % TX_BUFFER_SIZE;
			sm_txBuf.tail = tmpTail;
			HardwareImpl::txByteInterrupt(sm_txBuf.buf[tmpTail]);
		} else
			HardwareImpl::disableDataRegEmptyInt();
	}

  private:
	using HardwareImpl = Hardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, Driven::INTERRUPT, mode>;

	static constexpr auto TX_BUFFER_SIZE = 16;
	static constexpr auto RX_BUFFER_SIZE = 16;

	static volatile RingBuffer<data_t, TX_BUFFER_SIZE> sm_txBuf;
	static volatile RingBuffer<data_t, RX_BUFFER_SIZE> sm_rxBuf;
};

template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Mode mode>
volatile RingBuffer<typename InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::data_t,
                    InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::TX_BUFFER_SIZE>
    InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::sm_txBuf = {0, 0, {0}};

template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Mode mode>
volatile RingBuffer<typename InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::data_t,
                    InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::RX_BUFFER_SIZE>
    InterruptHardware<Registers, CtrlFlagsA, CtrlFlagsB, CtrlFlagsC, cfg, mode>::sm_rxBuf = {0, 0, {0}};

} // namespace detail

} // namespace uart

#undef FORCE_INLINE