#include "zh_avr_i2c.h"

#define ZH_ERROR_CHECK(cond, err, ...) \
	if (!(cond))                       \
	{                                  \
		return err;                    \
	}

#define I2C_OK 0x01
#define I2C_NACK 0x02
#define I2C_COLLISION 0x04
#define I2C_BUS_FAIL 0x08

#define I2C_START ((1 << TWINT) | (1 << TWEN) | (1 << TWIE))
#define I2C_MASTER_READ 1
#define I2C_MASTER_WRITE 0

typedef enum
{
	MASTER_WRITE,
	MASTER_READ,
	MASTER_WRITE_REG,
	MASTER_READ_REG,
	MASTER_PROBE
} _work_mode_t;

avr_err_t _zh_avr_i2c_master_start(TickType_t xTicksToWait);

static EventGroupHandle_t _event_group_handle = NULL;
static uint8_t _target_i2c_address = 0;
volatile static uint8_t _work_mode = 0;

volatile static uint8_t *_master_tx_data = NULL;
volatile static uint8_t _master_tx_data_size = 0;
volatile static uint8_t _master_tx_data_counter = 0;

avr_err_t zh_avr_i2c_master_init(const bool pullup)
{
	_event_group_handle = xEventGroupCreate();
	ZH_ERROR_CHECK(_event_group_handle != NULL, AVR_ERR_NO_MEM);
	cli();
	DDRC &= ~(1 << PORTC5 | 1 << PORTC4);
	if (pullup == true)
	{
		PORTC |= 1 << PORTC5 | 1 << PORTC4;
	}
	TWBR = ((F_CPU / 100000) - 16) / 2;
	TWSR = 0xF8;
	sei();
	return AVR_OK;
}

avr_err_t zh_avr_i2c_master_probe(const uint8_t addr, TickType_t xTicksToWait)
{
	_work_mode = MASTER_PROBE;
	_target_i2c_address = addr;
	return _zh_avr_i2c_master_start(xTicksToWait);
}

avr_err_t zh_avr_i2c_master_transmit(const uint8_t addr, uint8_t *data, uint8_t size, TickType_t xTicksToWait)
{
	ZH_ERROR_CHECK(data != NULL || size > 0, AVR_ERR_INVALID_ARG);
	_work_mode = MASTER_WRITE;
	_target_i2c_address = addr;
	_master_tx_data = data;
	_master_tx_data_size = size;
	_master_tx_data_counter = 0;
	return _zh_avr_i2c_master_start(xTicksToWait);
}

avr_err_t zh_avr_i2c_master_receive(const uint8_t addr, uint8_t *data, uint8_t size, TickType_t xTicksToWait)
{
	ZH_ERROR_CHECK(data != NULL || size > 0, AVR_ERR_INVALID_ARG);
	_work_mode = MASTER_READ;
	_target_i2c_address = addr;
	_master_tx_data = data;
	_master_tx_data_size = size;
	_master_tx_data_counter = 0;
	return _zh_avr_i2c_master_start(xTicksToWait);
}

avr_err_t zh_avr_i2c_master_transmit_receive(const uint8_t addr, uint8_t *write_data, uint8_t write_size, uint8_t *read_data, uint8_t read_size, size_t delay)
{
	return AVR_OK;
}

avr_err_t _zh_avr_i2c_master_start(TickType_t xTicksToWait)
{
	TWCR = I2C_START | (1 << TWSTA);
	EventBits_t bits = xEventGroupWaitBits(_event_group_handle, I2C_OK | I2C_NACK | I2C_COLLISION | I2C_BUS_FAIL, pdTRUE, pdFALSE, xTicksToWait);
	if ((bits & I2C_OK) != 0)
	{
		return AVR_OK;
	}
	else if ((bits & I2C_NACK) != 0)
	{
		return AVR_ERR_INVALID_RESPONSE;
	}
	else
	{
		return AVR_FAIL;
	}
}

ISR(TWI_vect)
{
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	switch (TWSR & 0xF8)
	{
	case 0x00: // Bus error.
		printf("00\n");
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_BUS_FAIL, &xHigherPriorityTaskWoken);
		break;
	case 0x08: // A START condition has been transmitted.
		printf("08\n");
		switch (_work_mode)
		{
		case MASTER_WRITE:
		case MASTER_WRITE_REG:
		case MASTER_READ_REG:
			TWDR = (_target_i2c_address << 1) | I2C_MASTER_WRITE;
			break;
		case MASTER_READ:
		case MASTER_PROBE:
			TWDR = (_target_i2c_address << 1) | I2C_MASTER_READ;
			break;
		default:
			break;
		}
		TWCR = I2C_START;
		break;
	case 0x10: // A repeated START condition has been transmitted.
		printf("10\n");
		// TWCR = I2C_START | (1 << TWSTO);
		break;
	case 0x18: // SLA+W has been transmitted. ACK has been received.
		printf("18\n");
		TWDR = *(_master_tx_data++);
		_master_tx_data_counter++;
		TWCR = I2C_START;
		break;
	case 0x20: // SLA+W has been transmitted. NOT ACK has been received.
		printf("20\n");
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_NACK, &xHigherPriorityTaskWoken);
		break;
	case 0x28: // Data byte has been transmitted. ACK has been received.
		printf("28\n");
		if (_master_tx_data_counter < _master_tx_data_size)
		{
			TWDR = *(_master_tx_data++);
			_master_tx_data_counter++;
			TWCR = I2C_START;
		}
		else
		{
			TWCR = I2C_START | (1 << TWSTO);
			xEventGroupSetBitsFromISR(_event_group_handle, I2C_OK, &xHigherPriorityTaskWoken);
		}
		break;
	case 0x30: // Data byte has been transmitted. NOT ACK has been received.
		printf("30\n");
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_NACK, &xHigherPriorityTaskWoken);
		break;
	case 0x38: // Arbitration lost in SLA+W or data bytes. Arbitration lost in SLA+R or NOT ACK bit.
		printf("38\n");
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_COLLISION, &xHigherPriorityTaskWoken);
		break;
	case 0x40: // SLA+R has been transmitted. ACK has been received.
		printf("40\n");
		switch (_work_mode)
		{
		case MASTER_WRITE:
			break;
		case MASTER_READ:
			if (_master_tx_data_size == 1)
			{
				TWCR = I2C_START;
			}
			else
			{
				TWCR = I2C_START | (1 << TWEA);
			}
			break;
		case MASTER_WRITE_REG:
			break;
		case MASTER_READ_REG:
			break;
		case MASTER_PROBE:
			TWCR = I2C_START | (1 << TWSTO);
			xEventGroupSetBitsFromISR(_event_group_handle, I2C_OK, &xHigherPriorityTaskWoken);
			break;
		default:
			break;
		}
		break;
	case 0x48: // SLA+R has been transmitted. NOT ACK has been received.
		printf("48\n");
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_NACK, &xHigherPriorityTaskWoken);
		break;
	case 0x50: // Data byte has been received. ACK has been returned.
		printf("50\n");
		*(_master_tx_data++) = TWDR;
		if (--_master_tx_data_size == 1)
		{
			TWCR = I2C_START;
		}
		else
		{
			TWCR = I2C_START | (1 << TWEA);
		}
		break;
	case 0x58: // Data byte has been received. NOT ACK has been returned.
		printf("58\n");
		*(_master_tx_data) = TWDR;
		TWCR = I2C_START | (1 << TWSTO);
		xEventGroupSetBitsFromISR(_event_group_handle, I2C_OK, &xHigherPriorityTaskWoken);
		break;
		// printf("%d\n", (TWSR & 0xF8));
		// case 0x00: // Bus Fail (автобус сломался)
		// {

		// 	i2c_Do |= i2c_ERR_BF;

		// 	TWCR = 0 << TWSTA |
		// 		   1 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Go!

		// 	MACRO_i2c_WhatDo_ErrorOut break;
		// }

		// case 0x08: // Старт был, а затем мы:
		// {
		// 	printf("08\n");
		// 	TWCR = (1 << TWINT) | (0 << TWEA) | (0 << TWSTA) | (0 << TWSTO) | (0 << TWWC) | (0 << TWEN) | (1 << TWIE);
		// 	// i2c_index = 0; // Обнуляем индекс буфера.

		// 	// if ((i2c_Do & i2c_type_msk) == i2c_sarp) // В зависимости от режима
		// 	// {
		// 	// 	i2c_SlaveAddress |= 0x01; // Шлем Addr+R
		// 	// }
		// 	// else // Или
		// 	// {
		// 	// 	i2c_SlaveAddress &= 0xFE; // Шлем Addr+W
		// 	// }

		// 	// TWDR = i2c_SlaveAddress; // Адрес слейва
		// 	// TWCR = 0 << TWSTA |
		// 	// 	   0 << TWSTO |
		// 	// 	   1 << TWINT |
		// 	// 	   i2c_i_am_slave << TWEA |
		// 	// 	   1 << TWEN |
		// 	// 	   1 << TWIE; // Go!
		// 	break;
		// }

		// case 0x10: // Повторный старт был, а затем мы
		// {
		// 	if ((i2c_Do & i2c_type_msk) == i2c_sawsarp) // В зависимости от режима
		// 	{
		// 		i2c_SlaveAddress |= 0x01; // Шлем Addr+R
		// 	}
		// 	else
		// 	{
		// 		i2c_SlaveAddress &= 0xFE; // Шлем Addr+W
		// 	}

		// 	// To Do: Добавить сюда обработку ошибок

		// 	TWDR = i2c_SlaveAddress; // Адрес слейва
		// 	TWCR = 0 << TWSTA |
		// 		   0 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Go!
		// 	break;
		// }

		// case 0x18: // Был послан SLA+W получили ACK, а затем:
		// {
		// 	if ((i2c_Do & i2c_type_msk) == i2c_sawp) // В зависимости от режима
		// 	{
		// 		TWDR = i2c_Buffer[i2c_index]; // Шлем байт данных
		// 		i2c_index++;				  // Увеличиваем указатель буфера

		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   i2c_i_am_slave << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Go!
		// 	}

		// 	if ((i2c_Do & i2c_type_msk) == i2c_sawsarp)
		// 	{
		// 		i2c_PageAddrIndex = 0; // Обнулили указатель буфера адреса страницы.

		// 		TWDR = i2c_PageAddress[i2c_PageAddrIndex]; // Или шлем адрес странцы (по сути тоже байт данных)
		// 		i2c_PageAddrIndex++;
		// 		// Увеличиваем указатель буфера страницы
		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   i2c_i_am_slave << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Go!
		// 	}
		// }
		// break;

		// case 0x20: // Был послан SLA+W получили NACK - слейв либо занят, либо его нет дома.
		// {
		// 	i2c_Do |= i2c_ERR_NA;
		// 	// Код ошибки
		// 	TWCR = 0 << TWSTA |
		// 		   1 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Шлем шине Stop

		// 	MACRO_i2c_WhatDo_ErrorOut // Обрабатываем событие ошибки;
		// 		break;
		// }

		// case 0x28:									 // Байт данных послали, получили ACK!  (если sawp - это был байт данных. если sawsarp - байт адреса страницы)
		// {											 // А дальше:
		// 	if ((i2c_Do & i2c_type_msk) == i2c_sawp) // В зависимости от режима
		// 	{
		// 		if (i2c_index == i2c_ByteCount) // Если был байт данных последний
		// 		{
		// 			TWCR = 0 << TWSTA |
		// 				   1 << TWSTO |
		// 				   1 << TWINT |
		// 				   i2c_i_am_slave << TWEA |
		// 				   1 << TWEN |
		// 				   1 << TWIE; // Шлем Stop

		// 			MACRO_i2c_WhatDo_MasterOut // И выходим в обработку стопа
		// 		}
		// 		else
		// 		{
		// 			TWDR = i2c_Buffer[i2c_index]; // Либо шлем еще один байт
		// 			i2c_index++;
		// 			TWCR = 0 << TWSTA |
		// 				   0 << TWSTO |
		// 				   1 << TWINT |
		// 				   i2c_i_am_slave << TWEA |
		// 				   1 << TWEN |
		// 				   1 << TWIE; // Go!
		// 		}
		// 	}

		// 	if ((i2c_Do & i2c_type_msk) == i2c_sawsarp) // В другом режиме мы
		// 	{
		// 		if (i2c_PageAddrIndex == i2c_PageAddrCount) // Если последний байт адреса страницы
		// 		{
		// 			TWCR = 1 << TWSTA |
		// 				   0 << TWSTO |
		// 				   1 << TWINT |
		// 				   i2c_i_am_slave << TWEA |
		// 				   1 << TWEN |
		// 				   1 << TWIE; // Запускаем Повторный старт!
		// 		}
		// 		else
		// 		{											   // Иначе
		// 			TWDR = i2c_PageAddress[i2c_PageAddrIndex]; // шлем еще один адрес страницы
		// 			i2c_PageAddrIndex++;					   // Увеличиваем индекс счетчика адреса страниц
		// 			TWCR = 0 << TWSTA |
		// 				   0 << TWSTO |
		// 				   1 << TWINT |
		// 				   i2c_i_am_slave << TWEA |
		// 				   1 << TWEN |
		// 				   1 << TWIE; // Go!
		// 		}
		// 	}
		// }
		// break;

		// case 0x30: // Байт ушел, но получили NACK причин две. 1я передача оборвана слейвом и так надо. 2я слейв сглючил.
		// {
		// 	i2c_Do |= i2c_ERR_NK; // Запишем статус ошибки. Хотя это не факт, что ошибка.

		// 	TWCR = 0 << TWSTA |
		// 		   1 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Шлем Stop

		// 	MACRO_i2c_WhatDo_MasterOut // Отрабатываем событие выхода

		// 		break;
		// }

		// case 0x38: //  Коллизия на шине. Нашелся кто то поглавней
		// {
		// 	i2c_Do |= i2c_ERR_LP; // Ставим ошибку потери приоритета

		// 	// Настраиваем индексы заново.
		// 	i2c_index = 0;
		// 	i2c_PageAddrIndex = 0;

		// 	TWCR = 1 << TWSTA |
		// 		   0 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Как только шина будет свободна
		// 	break;			  // попробуем передать снова.
		// }

		// case 0x40: // Послали SLA+R получили АСК. А теперь будем получать байты
		// {
		// 	if (i2c_index + 1 == i2c_ByteCount) // Если буфер кончится на этом байте, то
		// 	{
		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   0 << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Требуем байт, а в ответ потом пошлем NACK(Disconnect)
		// 	} // Что даст понять слейву, что мол хватит гнать. И он отпустит шину
		// 	else
		// 	{
		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   1 << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Или просто примем байт и скажем потом ACK
		// 	}

		// 	break;
		// }

		// case 0x48: // Послали SLA+R, но получили NACK. Видать slave занят или его нет дома.
		// {
		// 	printf("48\n");
		// 	i2c_Do |= i2c_ERR_NA;
		// 	// Код ошибки No Answer
		// 	TWCR = 0 << TWSTA |
		// 		   1 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Шлем Stop

		// 	MACRO_i2c_WhatDo_ErrorOut // Отрабатываем выходную ситуацию ошибки
		// 		break;
		// }

		// case 0x50: // Приняли байт.
		// {
		// 	i2c_Buffer[i2c_index] = TWDR; // Забрали его из буфера
		// 	i2c_index++;

		// 	// To Do: Добавить проверку переполнения буфера. А то мало ли что юзер затребует

		// 	if (i2c_index + 1 == i2c_ByteCount) // Если остался еще один байт из тех, что мы хотели считать
		// 	{
		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   0 << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Затребываем его и потом пошлем NACK (Disconnect)
		// 	}
		// 	else
		// 	{
		// 		TWCR = 0 << TWSTA |
		// 			   0 << TWSTO |
		// 			   1 << TWINT |
		// 			   1 << TWEA |
		// 			   1 << TWEN |
		// 			   1 << TWIE; // Если нет, то затребываем следующий байт, а в ответ скажем АСК
		// 	}
		// 	break;
		// }

		// case 0x58: // Вот мы взяли последний байт, сказали NACK слейв обиделся и отпал.
		// {
		// 	i2c_Buffer[i2c_index] = TWDR; // Взяли байт в буфер
		// 	TWCR = 0 << TWSTA |
		// 		   1 << TWSTO |
		// 		   1 << TWINT |
		// 		   i2c_i_am_slave << TWEA |
		// 		   1 << TWEN |
		// 		   1 << TWIE; // Передали Stop

		// 	MACRO_i2c_WhatDo_MasterOut // Отработали точку выхода

		// 		break;
		// }

		// IIC  Slave ============================================================================

	// case 0x68: // RCV SLA+W Low Priority				// Словили свой адрес во время передачи мастером
	// case 0x78: // RCV SLA+W Low Priority (Broadcast)				// Или это был широковещательный пакет. Не важно
	// {
	// 	i2c_Do |= i2c_ERR_LP | i2c_Interrupted; // Ставим флаг ошибки Low Priority, а также флаг того, что мастера прервали

	// 	// Restore Trans after.
	// 	i2c_index = 0; // Подготовили прерваную передачу заново
	// 	i2c_PageAddrIndex = 0;
	// } // И пошли дальше. Внимание!!! break тут нет, а значит идем в "case 60"

	// case 0x60: // RCV SLA+W  Incoming?					// Или просто получили свой адрес
	// case 0x70: // RCV SLA+W  Incoming? (Broascast)		// Или широковещательный пакет
	// {

	// 	i2c_Do |= i2c_Busy; // Занимаем шину. Чтобы другие не совались
	// 	i2c_SlaveIndex = 0; // Указатель на начало буфера слейва, Неважно какой буфер. Не ошибемся

	// 	if (i2c_MasterBytesRX == 1) // Если нам суждено принять всего один байт, то готовимся принять  его
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   0 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Принять и сказать пошли все н... NACK!
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // А если душа шире чем один байт, то сожрем и потребуем еще ACK!
	// 	}
	// 	break;
	// }

	// case 0x80: // RCV Data Byte						// И вот мы приняли этот байт. Наш или широковещательный. Не важно
	// case 0x90: // RCV Data Byte (Broadcast)
	// {
	// 	i2c_InBuff[i2c_SlaveIndex] = TWDR; // Сжираем его в буфер.

	// 	i2c_SlaveIndex++; // Сдвигаем указатель

	// 	if (i2c_SlaveIndex == i2c_MasterBytesRX - 1) // Свободно место всего под один байт?
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   0 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Приянть его и сказать NACK!
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Места еще дофига? Принять и ACK!
	// 	}
	// 	break;
	// }

	// case 0x88: // RCV Last Byte							// Приянли последний байт
	// case 0x98: // RCV Last Byte (Broadcast)
	// {
	// 	i2c_InBuff[i2c_SlaveIndex] = TWDR; // Сожрали его в буфер

	// 	if (i2c_Do & i2c_Interrupted) // Если у нас был прерываный сеанс от имени мастера
	// 	{
	// 		TWCR = 1 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Влепим в шину свой Start поскорей и сделаем еще одну попытку
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Если не было такого факта, то просто отвалимся и будем ждать
	// 	}

	// 	MACRO_i2c_WhatDo_SlaveOut // И лениво отработаем наш выходной экшн для слейва
	// 		break;
	// }

	// case 0xA0: // Ой, мы получили Повторный старт. Но чо нам с ним делать?
	// {

	// 	// Можно, конечно, сделать вспомогательный автомат, чтобы обрабатывать еще и адреса внутренних страниц, подобно еепромке.
	// 	// Но я не стал заморачиваться. В этом случае делается это тут.

	// 	TWCR = 0 << TWSTA |
	// 		   0 << TWSTO |
	// 		   1 << TWINT |
	// 		   1 << TWEA |
	// 		   1 << TWEN |
	// 		   1 << TWIE; // просто разадресуемся, проигнорировав этот посыл
	// 	break;
	// }

	// case 0xB0: // Поймали свой адрес на чтение во время передачи Мастером
	// {
	// 	i2c_Do |= i2c_ERR_LP | i2c_Interrupted; // Ну чо, коды ошибки и флаг прерваной передачи.

	// 	// Восстанавливаем индексы
	// 	i2c_index = 0;
	// 	i2c_PageAddrIndex = 0;

	// } // Break нет! Идем дальше

	// case 0xA8: // Либо просто словили свой адрес на чтение
	// {
	// 	i2c_SlaveIndex = 0; // Индексы слейвовых массивов на 0

	// 	TWDR = i2c_OutBuff[i2c_SlaveIndex]; // Чтож, отдадим байт из тех что есть.

	// 	if (i2c_MasterBytesTX == 1)
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   0 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Если он последний, мы еще на NACK в ответ надеемся
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // А если нет, то  ACK ждем
	// 	}

	// 	break;
	// }

	// case 0xB8: // Послали байт, получили ACK
	// {

	// 	i2c_SlaveIndex++;					// Значит продолжаем дискотеку. Берем следующий байт
	// 	TWDR = i2c_OutBuff[i2c_SlaveIndex]; // Даем его мастеру

	// 	if (i2c_SlaveIndex == i2c_MasterBytesTX - 1) // Если он последний был, то
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   0 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Шлем его и ждем NACK
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   0 << TWEN |
	// 			   1 << TWIE; // Если нет, то шлем и ждем ACK
	// 	}

	// 	break;
	// }

	// case 0xC0: // Мы выслали последний байт, больше у нас нет, получили NACK
	// case 0xC8: // или ACK. В данном случае нам пох. Т.к. больше байтов у нас нет.
	// {
	// 	if (i2c_Do & i2c_Interrupted) // Если там была прерваная передача мастера
	// 	{							  // То мы ему ее вернем
	// 		i2c_Do &= i2c_NoInterrupted;
	// 		// Снимем флаг прерваности
	// 		TWCR = 1 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Сгенерим старт сразу же как получим шину.
	// 	}
	// 	else
	// 	{
	// 		TWCR = 0 << TWSTA |
	// 			   0 << TWSTO |
	// 			   1 << TWINT |
	// 			   1 << TWEA |
	// 			   1 << TWEN |
	// 			   1 << TWIE; // Если мы там одни, то просто отдадим шину
	// 	}

	// 	MACRO_i2c_WhatDo_SlaveOut // И отработаем выход слейва. Впрочем, он тут
	// 							  // Не особо то нужен. Разве что как сигнал, что мастер
	// 		break;				  // Нас почтил своим визитом.
	// }
	default:
		break;
	}
	if (xHigherPriorityTaskWoken == pdTRUE)
	{
		portYIELD();
	};
}