#pragma once

#include <stdint.h>

#include <util/twi.h>

namespace i2c {

#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega328P__)

template <uint32_t Clock>
class Hardware {
  public:
	static void init()
	{
		constexpr auto clock = calcClock();
		static_assert(checkClock(clock), "Unable to set requested I2C frequency");

		TWSR = clock.prescaler;
		TWBR = clock.bitrate;
	}

	template <uint8_t Addr>
	static bool start(bool read)
	{
		TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
		while (!(TWCR & (1 << TWINT)))
			;

		uint8_t status = TW_STATUS & 0xF8;
		if (status != TW_START && status != TW_REP_START)
			return false;

		TWDR = (Addr << 1) | read;
		TWCR = (1 << TWINT) | (1 << TWEN);

		while (!(TWCR & (1 << TWINT)))
			;

		status = TW_STATUS & 0xF8;
		if (status != TW_MT_SLA_ACK && status != TW_MR_SLA_ACK)
			return false;

		return true;
	}

	static bool write(uint8_t data)
	{
		TWDR = data;
		TWCR = (1 << TWINT) | (1 << TWEN);

		while (!(TWCR & (1 << TWINT)))
			;

		uint8_t status = TW_STATUS & 0xF8;
		if (status != TW_MT_DATA_ACK)
			return false;

		return true;
	}

	template <bool LastByte = false>
	static uint8_t read()
	{
		TWCR = (1 << TWINT) | (1 << TWEN) | (!LastByte << TWEA);
		while (!(TWCR & (1 << TWINT)))
			;

		return TWDR;
	}

	static void stop()
	{
		TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);

		while (TWCR & (1 << TWSTO))
			;
	}

  private:
	struct ClockCfg {
		uint8_t prescaler;
		uint8_t bitrate;
	};

	static constexpr auto calcScl(ClockCfg clk)
	{
		return F_CPU / (16 + 2 * clk.bitrate * clk.prescaler);
	}

	static constexpr auto checkClock(ClockCfg clk)
	{
		constexpr auto constexprAbs = [](auto val) {
			if (val < 0)
				return -val;
			return val;
		};

		constexpr auto CLOCK_EPSILON = 10;
		auto scl = calcScl(clk);
		if (constexprAbs(static_cast<int>(scl) - static_cast<int>(Clock)) < CLOCK_EPSILON)
			return true;
		return false;
	}

	static constexpr auto calcClock()
	{
		constexpr auto calcBitRate = [](uint8_t prescaler) { return (F_CPU / Clock - 16) / (2 * prescaler); };

		auto clk = ClockCfg{1, 0};

		for (uint8_t prescaler = 1; prescaler <= 64; prescaler *= 4) {
			clk.prescaler = prescaler;
			clk.bitrate = calcBitRate(prescaler);

			if (checkClock(clk))
				break;
		}

		return clk;
	}
};

#endif

} // namespace i2c