#include <sta/devices/stm32/bus/i2c.hpp>

#include <sta/debug/debug.hpp>
#include <sta/debug/assert.hpp>
#include <sta/debug/debug.hpp>

#include <cstring>

#include <i2c.h>

#ifdef STA_STM32_I2C_ENABLED

namespace sta
{
	STM32I2C::STM32I2C(I2C_HandleTypeDef * handle, Mutex * mutex)
			: I2C{mutex}, handle_{handle}
	{
		STA_ASSERT(handle != nullptr);
	}

	void STM32I2C::transfer(uint8_t value)
	{
		HAL_StatusTypeDef res;

		if (master_)
		{
			res = HAL_I2C_Master_Transmit(handle_, address_, &value, 1, timeout_);
		} else {
			res = HAL_I2C_Slave_Transmit(handle_, &value, 1, timeout_);
		}

		STA_ASSERT(res == HAL_OK);
	}

	void STM32I2C::transfer16(uint16_t value)
	{
		HAL_StatusTypeDef res;

		if (blocking_)
		{
			if (master_)
			{
				res = HAL_I2C_Master_Transmit(handle_, address_, reinterpret_cast<uint8_t *>(&value), 2, timeout_);
			} else {
				res = HAL_I2C_Slave_Transmit(handle_, reinterpret_cast<uint8_t *>(&value), 2, timeout_);
			}
		} else {
			if (master_)
			{
				res = HAL_I2C_Slave_Transmit_IT(handle_, reinterpret_cast<uint8_t *>(&value), 2);
			} else {
				res = HAL_I2C_Slave_Transmit_IT(handle_, reinterpret_cast<uint8_t *>(&value), 2);
			}
		}

		STA_ASSERT(res == HAL_OK);
	}

	void STM32I2C::transfer(const uint8_t * buffer, size_t size)
	{
		HAL_StatusTypeDef res;

		/*
		 *  It's undecided if we want to change the parameter for this function. Since the transmission
		 *  doesn't take a const buffer as an argument, we are using this fix by creating a temporary buffer.
		 */
		uint8_t * temp_buffer = new uint8_t[size];
		memcpy(temp_buffer, buffer, size);

		if (blocking_)
		{
			if (master_)
			{
				res = HAL_I2C_Master_Transmit(handle_, address_, temp_buffer, size, timeout_);
			} else {
				res = HAL_I2C_Slave_Transmit(handle_, temp_buffer, size, timeout_);
			}
		} else {
			if (master_) {
				res = HAL_I2C_Master_Transmit_IT(handle_, address_, temp_buffer, size);
			} else {
				res = HAL_I2C_Slave_Transmit_IT(handle_, temp_buffer, size);
			}
		}

		delete [] temp_buffer;

		STA_ASSERT(res == HAL_OK);
	}

	void STM32I2C::receive(uint8_t * buffer, size_t size)
	{
		HAL_StatusTypeDef res;

		if (blocking_) {
			if (master_) {
				res = HAL_I2C_Master_Receive(handle_, address_, buffer, size, timeout_);
			} else {
				res = HAL_I2C_Slave_Receive(handle_, buffer, size, timeout_);
			}
		} else {
			if (master_) {
				res = HAL_I2C_Master_Receive_IT(handle_, address_, buffer, size);
			} else {
				res = HAL_I2C_Slave_Receive_IT(handle_, buffer, size);
			}
		}

		STA_ASSERT(res == HAL_OK);
	}

	void STM32I2C::fill(uint8_t value, size_t count)
	{
		// Initialize a buffer of size count and fill it with the value.
		uint8_t *buffer = new uint8_t[count];
		memset(buffer, value, count);

		// Transfer the buffer via the bus.
		transfer(buffer, count);

		delete [] buffer;
	}

	bool STM32I2C::hasAddress(int address)
	{
		return HAL_I2C_IsDeviceReady(handle_, (uint16_t)(address << 1), 3, 5) == HAL_OK;
	}

	void STM32I2C::addressScan()
	{
		for(uint8_t i = 1; i < 128; i++)
		{
			if (hasAddress(i))
			{
				STA_DEBUG_PRINTF("Address %d exists", i);
			}
		}
	}

	STM32I2CDevice::STM32I2CDevice(STM32I2C * intf, int address, bool master, bool blocking)
		/* The address is bit-shifted by one to work properly for the STM32 HAL. */
		: I2CDevice(intf, address << 1, master, blocking)
	{
		intf->acquire();
		STA_ASSERT(intf->hasAddress(address));
		intf->release();
	}

} // namespace sta

#endif // STA_STM32_I2C_ENABLED