/*
 * Command.c
 *
 * Created: Thu Oct 13 2022 16:37:17
 *  Author Chris
 */
// ============================================================================================
// Includes
#include "Command.h"

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/irq.h"


// ============================================================================================
// Defines
#define COMMAND_BUFFER_SIZE			16
#define INVALID						-1


// ============================================================================================
// Variables
static Command_u	_Command_Buffer[COMMAND_BUFFER_SIZE];


// ============================================================================================
// Function Declarations
void Command_Send_Multicore_Command(uint8_t command, uint8_t parameter, uint16_t value);
void Command_Add_Multicore_Command_To_Buffer(uint command);


/*******************************************************************
	Interrupt Service Routines
*******************************************************************/
void ISR_MULTICORE_READ()
{
    uint32_t Multicore_Data;

	// Just record the latest entry
    while(multicore_fifo_rvalid())
	{
        Multicore_Data = multicore_fifo_pop_blocking();
		Command_Add_Multicore_Command_To_Buffer(Multicore_Data);
	}	

    multicore_fifo_clear_irq();
}


/*******************************************************************
	Functions
*******************************************************************/
void Command_Init()
{
	for(uint i=0;i<COMMAND_BUFFER_SIZE;i++)
	{
		_Command_Buffer[i].Data = MULTICORE_COMMAND_EMPTY;
	}

	irq_set_exclusive_handler(SIO_FIFO_IRQ_NUM(0), ISR_MULTICORE_READ);
    irq_set_enabled(SIO_FIFO_IRQ_NUM(0), true);
}

void Command_Issue_Get_Request(uint8_t command, uint8_t parameter)
{
	Command_Send_Multicore_Command(command, parameter, 0);
}

void Command_Issue_Set_Request(uint8_t command, uint8_t parameter, uint16_t value)
{
	Command_Send_Multicore_Command(command, parameter, value);
}

int Command_Get_Request_Response_By_Command_Only(uint8_t command, int timeout_us)
{
	int Return_Value = INVALID;
	
	uint64_t Timeout_Time_us = to_us_since_boot(get_absolute_time()) + timeout_us;

	do
	{
		for(uint i=0;i<COMMAND_BUFFER_SIZE;i++)
		{
			if(_Command_Buffer[i].Fields.Command == command)
			{
				Return_Value = _Command_Buffer[i].Fields.Value;
				_Command_Buffer[i].Data = MULTICORE_COMMAND_EMPTY;
			}
		}
	} while (Return_Value == INVALID && Timeout_Time_us > to_us_since_boot(get_absolute_time()));

	return Return_Value;
}

int Command_Get_Request_Response_By_Command_And_Parameter(uint8_t command, uint8_t parameter, int timeout_us)
{
	int Return_Value = INVALID;
	
	uint64_t Timeout_Time_us = to_us_since_boot(get_absolute_time()) + timeout_us;

	do
	{
		for(uint i=0;i<COMMAND_BUFFER_SIZE;i++)
		{
			if(_Command_Buffer[i].Fields.Command == command && _Command_Buffer[i].Fields.Parameter == parameter)
			{
				Return_Value = _Command_Buffer[i].Fields.Value;
				_Command_Buffer[i].Data = MULTICORE_COMMAND_EMPTY;
			}
		}
	} while (Return_Value == INVALID && Timeout_Time_us > to_us_since_boot(get_absolute_time()));
	
	return Return_Value;
}

uint Command_Get_Request_Response_Buffer_Fill_Count()
{
	uint Fill_Count = 0;

	for(uint i=0;i<COMMAND_BUFFER_SIZE;i++)
	{
		if(_Command_Buffer[i].Fields.Command != MULTICORE_COMMAND_EMPTY)
		{
			Fill_Count++;
		}
	}

	return Fill_Count;
}


/*******************************************************************
	Internal Functions
*******************************************************************/
void Command_Send_Multicore_Command(uint8_t command, uint8_t parameter, uint16_t value)
{
	Command_u Command;
	Command.Fields.Command 		= command;
	Command.Fields.Parameter 	= parameter;
	Command.Fields.Value		= value;

	multicore_fifo_push_blocking(Command.Data);
}

void Command_Add_Multicore_Command_To_Buffer(uint command)
{
	for(uint i=0;i<COMMAND_BUFFER_SIZE;i++)
	{
		if(_Command_Buffer[i].Fields.Command == MULTICORE_COMMAND_EMPTY)
		{
			_Command_Buffer[i].Data = command;
			return;
		}
	}
}