/*
 * main_cpp.cpp
 *
 *  Created on: 11  2016 .
 *      Author: Sergey
 */


#include "stm32f0xx_hal.h"
#include "usbd_cdc_if.h"
#include "commands.h"
#include <algorithm>

extern I2C_HandleTypeDef hi2c1;

#define EEPROM_ADDR 0x50

uint8_t tx_buffer[16];

struct UsbRxBuffer {
	uint8_t data[64];
	uint8_t size;
	uint8_t position;
	uint8_t read_complete;
};

extern uint32_t vcp_initialized;

struct IRequest {
	virtual ~IRequest() = default;
	virtual bool Read(uint8_t* buffer, uint32_t length) = 0;
};

struct IResponse {
	virtual ~IResponse() = default;
	virtual bool Write(const uint8_t* buffer, uint32_t length) = 0;
};

struct Command {
	virtual ~Command() = default;
	virtual bool Execute(IRequest* request, IResponse* response) = 0;
};

struct CmdEEPROMRead : public Command{
	bool Execute(IRequest* request, IResponse* response) override {
		CmdEEPROMReadBufferArgs args;
		//read args
		if(!request->Read((uint8_t*)&args, sizeof(args))){
			return false;
		}

		//read and send memory
		while(args.size > 0){
			auto block_size = std::min<uint32_t>(args.size, sizeof(tx_buffer));
			auto result = HAL_I2C_Mem_Read(&hi2c1, (EEPROM_ADDR << 1) | ((args.startAddress >> 7) & 0xE), args.startAddress & 0xff, I2C_MEMADD_SIZE_8BIT, &tx_buffer[0], block_size, 1000);
			args.startAddress += block_size;
			args.size -= block_size;
			if(!response->Write(&tx_buffer[0], block_size)){
				return false;
			}
		}
		return true;
	}
};

struct CmdEEPROMWrite : public Command{
	bool Execute(IRequest* request, IResponse* response) override {
		CmdEEPROMWriteBufferArgs args;
		//read args
		if(!request->Read((uint8_t*)&args, sizeof(args))){
			return false;
		}

		//receive and write memory blocks
		while(args.size > 0){
			auto block_size = std::min<uint32_t>(args.size, sizeof(tx_buffer));
			//read block
			if(!request->Read(&tx_buffer[0], sizeof(tx_buffer))){
				return false;
			}

			auto result = HAL_I2C_Mem_Write(&hi2c1, (EEPROM_ADDR << 1) | ((args.startAddress >> 7) & 0xE), args.startAddress & 0xff, I2C_MEMADD_SIZE_8BIT, &tx_buffer[0], block_size, 1000);
			HAL_Delay(20);
			args.startAddress += block_size;
			args.size -= block_size;
		}
		return true;
	}
};

struct UnknownCommandHandler : public Command {
	bool Execute(IRequest* request, IResponse* response) override {
		return true;
	}
};

CmdEEPROMRead cmd_eeprom_read;
CmdEEPROMRead cmd_eeprom_write;
UnknownCommandHandler cmd_unknown;

Command* GetCommandHandler(CommandId id){
	switch(id){
	case CommandId::EEPROMReadBuffer:
		return &cmd_eeprom_read;
	case CommandId::EEPROMWriteBuffer:
			return &cmd_eeprom_write;
	default:
		return &cmd_unknown;
	}
}

struct CommandProcessor : public IRequest, public IResponse{
	CommandProcessor():_connected(false), _bits_received(0), _bits_transmitted(0), _last_byte_received(false),_tx_bits_buffered(0),
			_rx_bits_buffer(0), _tx_bits_buffer(0){

	}
	void OnDisconnect(){
		_connected = false;
	}

	void OnCdcInit(){
		rx_buffer.position = 0;
		rx_buffer.size = 0;
		rx_buffer.read_complete = 0;
		_connected = false;
		USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &rx_buffer.data[0]);
	}
	void OnCdcReceive(uint8_t* Buf, uint32_t *Len){
		rx_buffer.position = 0;
		rx_buffer.size = *Len;
		rx_buffer.read_complete = 1;
	}
	void Run(){
		while(true){
			//do connection
			_connected = true;

			//execute commands
			while(true){
				CommandId cmd;
				//read header
				uint32_t rx_size = 0;
				if(!Read((uint8_t*)&cmd, sizeof(cmd), rx_size) || (rx_size != sizeof(cmd))){
					break;
				}

				//execute command
				auto cmd_handler = GetCommandHandler(cmd);
				if(!cmd_handler->Execute(this, this)){
					break;
				}

				//finalize read
				FinalizeRead();

				//finalize write
				FinalizeWrite();

			}
		}
	}

	void FinalizeRead(){
		while(!_last_byte_received){
			uint8_t buffer;
			uint32_t received;
			if(!Read(&buffer, sizeof(buffer), received)){
				break;
			}
		}
		_last_byte_received = false;
		_rx_bits_buffer = 0;
		_bits_received = 0;
	}

	void FinalizeWrite(){
		//flush bits
		if((_bits_transmitted) % 8 != 0){
			CDC_Write((uint8_t*)&_tx_bits_buffer, 1);
		}
		//write finalization byte
		uint8_t finalize_byte = 0xff;
		CDC_Write(&finalize_byte, sizeof(finalize_byte));
		_tx_bits_buffer = 0;
		_bits_received = 0;
	}

	bool Read(uint8_t* buffer, uint32_t length, uint32_t& received) override {
		received = 0;
		if(_last_byte_received){
			return true;
		}

		while(length > 0){
			uint8_t rx;
			if(!CDC_Read(&rx, sizeof(rx))){
				return false;
			}

			auto buffered_bits = _bits_received % 8;

			if(rx & 0x80){//if finalization byte received
				_last_byte_received = true;
				if(buffered_bits){//flush buffered bits
					buffer[0] = _bits_buffer & 0xff;
					received++;
					return true;
				}
			}

			//put bits to buffer
			_bits_buffer |= (rx & 0x7f) << buffered_bits;

			if(buffered_bits != 0){
				buffer[0] = _bits_buffer & 0xff;
				--length;
				received++;
				_bits_buffer >>= 8;
			}
		}
	}

	bool Write(const uint8_t* buffer, uint32_t length) override{
		while(length > 0){
			_tx_bits_buffer |= (uint16_t)(buffer[0]) << _tx_bits_buffered;
			buffer++;
			length--;
			_tx_bits_buffered += 8;

			while(_tx_bits_buffered >= 7){
				uint8_t tx_buffer = _tx_bits_buffer & 0x7f;
				if(!CDC_Write((uint8_t*)&tx_buffer, 1)){
					return false;
				}
				_bits_transmitted -= 7;
				_tx_bits_buffer >>= 7;
			}
		}
		return true;
	}
private:

	bool CDC_Write(uint8_t* buffer, uint16_t size){
		while(vcp_initialized == 0){}

		while(USBD_OK != CDC_Transmit_FS(buffer, size)){
			if(!_connected){
				return false;
			}
		}
		return true;
	}

	bool CDC_Read(uint8_t* buffer, uint16_t size){
		uint32_t i;
		uint32_t block_size;
		if(size == 0){
			return true;
		}

		while(vcp_initialized == 0){}

		while(size > 0){

			while(rx_buffer.read_complete == 0){
				if(!_connected){
					return false;
				}
			}

			if(rx_buffer.position >= rx_buffer.size){
				rx_buffer.read_complete = 0;
				USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &rx_buffer.data[0]);
				USBD_CDC_ReceivePacket(&hUsbDeviceFS);

				while(rx_buffer.read_complete == 0){
					if(!_connected){
						return false;
					}
				}
			}

			block_size = size < (rx_buffer.size - rx_buffer.position) ? size : (rx_buffer.size - rx_buffer.position);

			//copy data
			for(i = 0; i < block_size; ++i){
				buffer[i] = rx_buffer.data[rx_buffer.position++];
			}
			buffer += block_size;
			size -= block_size;
		}
		return _connected;
	}

	bool _connected;
	UsbRxBuffer rx_buffer;
	uint32_t _bits_received;
	uint32_t _bits_transmitted;
	uint8_t _tx_bits_buffered;
	uint16_t _rx_bits_buffer;
	uint16_t _tx_bits_buffer;
	bool _last_byte_received;
};

CommandProcessor commandProcessor;


extern "C" void OnCdcDisconnect(){
	HAL_GPIO_TogglePin(LD5_GPIO_Port, LD5_Pin);
	commandProcessor.OnDisconnect();
}

extern "C" void OnCdcInit(){
	commandProcessor.OnCdcInit();
}

extern "C" void OnCdcReceive(uint8_t* Buf, uint32_t *Len){
	commandProcessor.OnCdcReceive(Buf, Len);
}

extern "C" void main_cpp(){
	while(true){


		switch(cmd){
		case CommandId::EEPROMReadBuffer:{

			}
			break;
		case CommandId::EEPROMWriteBuffer:{


			}
			break;
		}

		HAL_GPIO_TogglePin(LD4_GPIO_Port, LD4_Pin);
	}
}

