/** \file
 *  \brief This file implements a simple serial debug console and command interpreter.
 */

/** \defgroup CONSOLE Console
 *
 *  \brief Serial debug console command interpreter.
 *
 *  \todo Describe the command interpreter.
 *
 * @{
 * @}
 */

/* Include Files */
#include "KTag.h"

/* Local Definitions and Constants */
    
//! Text representations of numeric digits, used by COMM_Console_Print_UInt32().
static const char8 DIGITS[] = "0123456789ABCDEF";

#if (CONFIG__FEATURE_COMM_CONSOLE == CONFIG__FEATURE_ENABLED)
    
//! Maximum number of characters (save one) able to be printed by COMM_Console_Print_String().
#define MAX_CONSOLE_STRING_LENGTH   81

//! States in the COMM_Console_Task() state machine.
typedef enum
{
	COMM_STATE_INITIALIZING = 0,
    COMM_STATE_DISPLAY_POWERUP_INFO,
	COMM_STATE_IDLE,
    COMM_STATE_COMMAND_TOO_LONG,
    COMM_STATE_IDENTIFY_COMMAND,
    COMM_STATE_EXECUTE_COMMAND,
    COMM_STATE_UNKNOWN_COMMAND
} COMM_Console_State_T;

/* Public Variables */
char8 Command_Buffer[COMM_CONSOLE_COMMAND_MAX_LENGTH];
uint_fast16_t Command_Buffer_Index = 0;
TaskHandle_t COMM_Console_Task_Handle;

/* Private Variables */

//! Current state of the COMM_Console_Task() state machine.
static COMM_Console_State_T Current_State = COMM_STATE_INITIALIZING;

//! Next state of the COMM_Console_Task() state machine.
static COMM_Console_State_T Next_State = COMM_STATE_INITIALIZING;

//! Index into the #COMM_Console_Command_Table for the command currently being handled.
/*!
 *  If #Current_Command is set to UINT_FAST16_MAX, the command being handled is unknown, or no command is being handled.
 */
static uint_fast16_t Current_Command = 0;

/* Private Function Prototypes */

static void ConsoleISR(void);
static bool ConsoleCommandMatches(const char8 * const command_name);
static void ReverseString(char8 * value, uint32_t length);

/* Inline Functions */

//! Swaps the characters in x and y.
static inline void Swap_Char8(char8 * x, char8 * y)
{
    uint8_t temp = *x;
    *x = *y;
    *y = temp;
 }

static inline void Reset_Command_Buffer()
{
    taskENTER_CRITICAL();
    for (uint_fast16_t i = 0; i < COMM_CONSOLE_COMMAND_MAX_LENGTH; i++)
    {
        Command_Buffer[i] = COMM_CONSOLE_STRING_TERMINATOR;
    }
    Command_Buffer_Index = 0;
    taskEXIT_CRITICAL();   
}

/* Public Functions */

//! Initializes the console.
/*!
 *  \ingroup CONSOLE
 */
void COMM_Console_Init(void)
{
    // Enable the pullup on the Rx pin to keep the noise down.
    Cy_GPIO_SetDrivemode(UART_Console_rx_PORT, UART_Console_rx_NUM, CY_GPIO_DM_PULLUP);
    UART_Console_Start();
    
    /// Unmask only the RX FIFO not empty interrupt bit.
    UART_Console_HW->INTR_RX_MASK = SCB_INTR_RX_MASK_NOT_EMPTY_Msk;
    Cy_SysInt_Init(&Int_UART_Console_cfg, ConsoleISR);
    NVIC_ClearPendingIRQ(Int_UART_Console_cfg.intrSrc);
    NVIC_EnableIRQ(Int_UART_Console_cfg.intrSrc);
}
            
//! Parses and handle console commands in the background.
/*!
 *  \ingroup CONSOLE
 *
 *  The [UML State Machine Diagram](http://www.uml-diagrams.org/state-machine-diagrams.html) below
 *  shows how the console messages processed by this code.  Note that all of the *Character Rx'd*
 *  transitions occur in the #UART_Console_SPI_UART_ISR_EntryCallback() itself on the PSoC4, in
 *  #ConsoleRxISR() on the PSoC5, and in #ConsoleISR() on the PSoC6, to improve overall performance.
 *
 *  \startuml{COMM_Console_Task.png} "Console Task"
 *
 *      skinparam headerFontSize 18
 *      skinparam state {
 *         BackgroundColor #eeeeee
 *         BackgroundColor<<Error>> #ffaaaa
 *         FontName Impact
 *         FontSize 18
 *      }
 *      skinparam note {
 *         FontName "Comic Sans MS"
 *         FontStyle italic
 *      }
 *
 *      state "Initializing" as STATE_INITIALIZING
 *      [*] --> STATE_INITIALIZING
 *      note left of STATE_INITIALIZING : Wait for the rest of the system to come online. 
 *      state "Display Powerup Info" as STATE_DISPLAY_POWERUP_INFO
 *      STATE_DISPLAY_POWERUP_INFO : do/ print OS version
 *      STATE_DISPLAY_POWERUP_INFO : do/ print configuration (debug or release)
 *      STATE_INITIALIZING --> STATE_DISPLAY_POWERUP_INFO : after(100ms)
 *      state "Idle" as STATE_IDLE
 *      STATE_IDLE : do/ RTOS_Sleep()
 *      STATE_DISPLAY_POWERUP_INFO --> STATE_IDLE
 *      state STATE_IS_EOM <<choice>>
 *      note top of STATE_IS_EOM : This happens in\nUART_Console_SPI_UART_ISR_ExitCallback() on PSoC4,\nConsoleRxISR() on PSoC5,\nand ConsoleISR() on PSoC6.
 *      STATE_IDLE --> STATE_IS_EOM : character rx'd
 *      state STATE_IS_COMMAND_BUFFER_FULL <<choice>>
 *      note top of STATE_IS_COMMAND_BUFFER_FULL : This happens in\nUART_Console_SPI_UART_ISR_ExitCallback() on PSoC4,\nConsoleRxISR() on PSoC5,\nand ConsoleISR() on PSoC6.
 *      STATE_IS_EOM --> STATE_IS_COMMAND_BUFFER_FULL : [else]
 *      state "Identify Command" as STATE_IDENTIFY_COMMAND
 *      STATE_IDENTIFY_COMMAND : do/ look for command in the COMM_Console_Command_Table[]
 *      STATE_IS_EOM --> STATE_IDENTIFY_COMMAND : [rx'd character is EOM]
 *      STATE_IDLE --> STATE_IDENTIFY_COMMAND : COMM_Console_Execute_Internal_Command()
 *      state "Command Too Long" as STATE_COMMAND_TOO_LONG
 *      STATE_COMMAND_TOO_LONG : do/ print error message
 *      STATE_COMMAND_TOO_LONG : do/ reset command buffer
 *      STATE_IS_COMMAND_BUFFER_FULL --> STATE_COMMAND_TOO_LONG : [command buffer is full]
 *      STATE_IS_COMMAND_BUFFER_FULL --> STATE_IDLE : [else]/\nAppend received character to command buffer
 *      STATE_COMMAND_TOO_LONG --> STATE_IDLE
 *      state "Execute Command" as STATE_EXECUTE_COMMAND
 *      STATE_EXECUTE_COMMAND : do/ execute console command
 *      STATE_EXECUTE_COMMAND : exit/ reset command buffer
 *      STATE_EXECUTE_COMMAND --> STATE_IDLE
 *      STATE_IDENTIFY_COMMAND --> STATE_EXECUTE_COMMAND : [command matched]
 *      state "Unknown Command" as STATE_UNKNOWN_COMMAND
 *      STATE_UNKNOWN_COMMAND : do/ print error message
 *      STATE_UNKNOWN_COMMAND : do/ reset command buffer
 *      STATE_IDENTIFY_COMMAND --> STATE_UNKNOWN_COMMAND : [else]
 *      STATE_UNKNOWN_COMMAND --> STATE_IDLE
 *
 *      left footer Key: UML 2.5\nLast modified 2020-12-14
 *  \enduml
 *
 *  \return               None (infinite loop)
 */
void COMM_Console_Task(void * pvParameters)
{    
    static TickType_t xTicksToWait = pdMS_TO_TICKS(10);
    static uint32_t * NotificationValue;
    
    while(true)
    {
        (void) xTaskNotifyWait(0, 0, (uint32_t *)&NotificationValue, xTicksToWait);
        
        // Change to the next state atomically.
        taskENTER_CRITICAL();
        Current_State = Next_State;
        taskEXIT_CRITICAL();
        
    	switch (Current_State)
    	{
            default:
            case COMM_STATE_INITIALIZING:
                Next_State = COMM_STATE_DISPLAY_POWERUP_INFO;
                vTaskDelay(pdMS_TO_TICKS(10));
                xTicksToWait = 1;
                break;
            
            case COMM_STATE_DISPLAY_POWERUP_INFO:
                COMM_Console_Print_String("[COMM] ");
                COMM_RTOS_HandleConsoleVersion(NULL, 0);
                vTaskDelay(pdMS_TO_TICKS(10));
                COMM_Console_Print_String("[COMM] Console ready (awaiting commands).\n");
                Next_State = COMM_STATE_IDLE;
                xTicksToWait = 1;
                break;
                
	        case COMM_STATE_IDLE:
                xTicksToWait = pdMS_TO_TICKS(100);
                break;
                
            case COMM_STATE_COMMAND_TOO_LONG:
                COMM_Console_Print_String("[COMM] ERROR: Command \"");
                COMM_Console_Print_String(Command_Buffer);
                COMM_Console_Print_String("\" too long!\n");
                Reset_Command_Buffer();
                Next_State = COMM_STATE_IDLE;
                xTicksToWait = 1;
                break;
                
            case COMM_STATE_IDENTIFY_COMMAND:
                Current_Command = UINT_FAST16_MAX;
                
                for (uint_fast16_t i = 0; i < COMM_N_CONSOLE_COMMANDS; i++)
                {
                    if (ConsoleCommandMatches(COMM_Console_Command_Table[i].Command_Name) == true)
                    {
                        Current_Command = i;
                        Next_State = COMM_STATE_EXECUTE_COMMAND;
                        xTicksToWait = 1;
                        break;
                    }
                }
                
                if (Current_Command == UINT_FAST16_MAX)
                {
                    // No matching command was found.
                    Next_State = COMM_STATE_UNKNOWN_COMMAND;
                    xTicksToWait = 1;
                }
                break;
                
            case COMM_STATE_EXECUTE_COMMAND:
                if (COMM_Console_Command_Table[Current_Command].Execute_Command != NULL)
                {
                    COMM_Console_Command_Result_T result = COMM_Console_Command_Table[Current_Command].Execute_Command(Command_Buffer, Command_Buffer_Index);
                    
                    if (result == COMM_CONSOLE_CMD_RESULT_PARAMETER_ERROR)
                    {
                        COMM_Console_Print_String("ERROR: Parameter error!\n");
                    }
                }
                Reset_Command_Buffer();
                Next_State = COMM_STATE_IDLE;
                xTicksToWait = 1;
                break;
                
            case COMM_STATE_UNKNOWN_COMMAND:
                COMM_Console_Print_String("ERROR: Command \"");
                COMM_Console_Print_String(Command_Buffer);
                COMM_Console_Print_String("\" not recognized!  Try '?' for help.\n");
                Reset_Command_Buffer();
                Next_State = COMM_STATE_IDLE;
                xTicksToWait = 1;
                break;
        }
    }
}

SystemKResult_T HW_Execute_Console_Command(const uint8_t * const command)
{
    COMM_Console_Execute_Internal_Command(command);
    
    return SYSTEMK_RESULT_SUCCESS;
}

//! Executes a (potentially cross-task) console command.
/*!
 *  This function is used to initiate a console command from a software source internal to this
 *  CPU.  This provides a way to use preexisting console commands on TX-only consoles.
 *
 *  \note If two calls to this function are made back-to-back (before the COMM_Console_Task() has an
 *  opportunity to run), only the second command will be executed, as it will have overwritten the
 *  first.  Allow time for the console commands to execute between calls to this function.
 *
 *  \param command  String containing the command to be executed.
 */
void COMM_Console_Execute_Internal_Command(const uint8_t * const command)
{
    bool finished = false;
    uint_fast16_t i = 0;
    
    taskENTER_CRITICAL();
	while ( (finished == false) &&
	        (i < COMM_CONSOLE_COMMAND_MAX_LENGTH)  &&
            (command[i] != COMM_CONSOLE_END_OF_MESSAGE ) &&
            (command[i] != COMM_CONSOLE_STRING_TERMINATOR )
	  )
    {
        Command_Buffer[i] = command[i];
        i++;
    }
    Command_Buffer_Index = i;
    
    // If there is still room, terminate the command.
    if (i < COMM_CONSOLE_COMMAND_MAX_LENGTH)
    {
        Command_Buffer[i] = COMM_CONSOLE_END_OF_MESSAGE;
    }
    taskEXIT_CRITICAL();
    
    Next_State = COMM_STATE_IDENTIFY_COMMAND;
    xTaskNotifyGive(COMM_Console_Task_Handle);
}

//! Prints a NULL-terminated string to the serial console.
void COMM_Console_Print_String(const char8 * const text)
{
	for (size_t i = 0; i < MAX_CONSOLE_STRING_LENGTH; i++)
	{
		// Check for the end of the string.  If there is no NULL terminator, up to
        // MAX_CONSOLE_STRING_LENGTH characters of randomness will be printed. 
		if (text[i] == COMM_CONSOLE_STRING_TERMINATOR)
		{
			break;
		}
		
		// Send out the string, one character at a time.
        COMM_Console_PutChar(text[i]);
	}
}

//! Prints a 32-bit unsigned integer to the serial console.
void COMM_Console_Print_UInt32(uint32_t value)
{
    // The largest string for a unit32_t is 10 characters (4294967296).
    char8 buffer[10+1];
    uint_fast8_t buffer_index = 0;
    
    while (value > 9)
    {
        uint8_t digit_index = value % 10;
        buffer[buffer_index] = DIGITS[digit_index];
        value = value / 10;
        buffer_index++;
    }	
    buffer[buffer_index] = DIGITS[value];
    buffer_index++;
    ReverseString(buffer, buffer_index);
    
    // NULL-terminate the string.
    buffer[buffer_index] = 0;
    
    COMM_Console_Print_String(buffer);
}

//! Prints a 32-bit signed integer to the serial console.
void COMM_Console_Print_SInt32(int32_t value)
{
    if (value < 0)
    {
        value *= -1;
        COMM_Console_PutChar('-');
    }

    COMM_Console_Print_UInt32(value);
}

//! Prints a 32-bit unsigned integer to the serial console using a hexadecimal representation.
void COMM_Console_Print_UInt32AsHex(uint32_t value)
{
    // The largest hexadecimal string for a unit32_t is 8 characters (FFFFFFFF).
    char8 buffer[8+1];
    uint_fast8_t buffer_index = 0;
    
    while (value > 15)
    {
        uint8_t digit_index = value % 16;
        buffer[buffer_index] = DIGITS[digit_index];
        value = value / 16;
        buffer_index++;
    }	
    buffer[buffer_index] = DIGITS[value];
    buffer_index++;
    ReverseString(buffer, buffer_index);
    
    // NULL-terminate the string.
    buffer[buffer_index] = 0;
    
    COMM_Console_PutChar('0');
    COMM_Console_PutChar('x');
    COMM_Console_Print_String(buffer);
}

//! Prints a 64-bit unsigned integer to the serial console.
void COMM_Console_Print_UInt64(uint64_t value)
{
    // The largest string for a unit64_t is 20 characters (18446744073709551615).
    char8 buffer[20+1];
    uint_fast8_t buffer_index = 0;
    
    while (value > 9)
    {
        uint8_t digit_index = value % 10;
        buffer[buffer_index] = DIGITS[digit_index];
        value = value / 10;
        buffer_index++;
    }	
    buffer[buffer_index] = DIGITS[value];
    buffer_index++;
    ReverseString(buffer, buffer_index);
    
    // NULL-terminate the string.
    buffer[buffer_index] = 0;
    
    COMM_Console_Print_String(buffer);
}

//! Prints a 64-bit unsigned integer to the serial console using a hexadecimal representation.
void COMM_Console_Print_UInt64AsHex(uint64_t value)
{
    // The largest hexadecimal string for a unit64_t is 16 characters (FFFFFFFFFFFFFFFF).
    char8 buffer[16+1];
    uint_fast8_t buffer_index = 0;
    
    while (value > 15)
    {
        uint8_t digit_index = value % 16;
        buffer[buffer_index] = DIGITS[digit_index];
        value = value / 16;
        buffer_index++;
    }	
    buffer[buffer_index] = DIGITS[value];
    buffer_index++;
    ReverseString(buffer, buffer_index);
    
    // NULL-terminate the string.
    buffer[buffer_index] = 0;
    
    COMM_Console_PutChar('0');
    COMM_Console_PutChar('x');
    COMM_Console_Print_String(buffer);
}

//! Prints a floating-point number to the serial console.
/*!
 *  With thanks to Rick Regan and his [Quick and Dirty Floating-Point to Decimal Conversion](https://www.exploringbinary.com/quick-and-dirty-floating-point-to-decimal-conversion/).
 */
void COMM_Console_Print_Float(float value)
{
    #define MAX_INTEGRAL_DIGITS 12
    #define MAX_FRACTIONAL_DIGITS 6
    #define BUFFER_SIZE (MAX_INTEGRAL_DIGITS + MAX_FRACTIONAL_DIGITS + 2)
   
    char8 buffer[BUFFER_SIZE];
    char8 integral_buffer_reversed[MAX_INTEGRAL_DIGITS];
    uint16_t buffer_index = 0;
    double integral_value;
    double fractional_value;
    bool overflow = false;
    
    if (value < 0.0)
    {
        COMM_Console_Print_String("-");
        value *= -1.0;
    }

    // Break the given value into fractional and integral parts.
    fractional_value = modf(value, &integral_value); 

    if (integral_value > 0)
    {
        // Convert the integral part.
        while ((integral_value > 0) && (buffer_index < MAX_INTEGRAL_DIGITS))
        {
            integral_buffer_reversed[buffer_index++] = '0' + (int)fmod(integral_value, 10);
            integral_value = floor(integral_value / 10);
        }
        
        // If there is still an integral part remaining, and overflow has occurred.
        if (integral_value > 0)
        {
            overflow = true;
        }
        
        // Reverse and append the integral part.
        for (uint16_t i = 0; i < buffer_index; i++)
        {
           buffer[i] = integral_buffer_reversed[buffer_index-i-1];
        }
    }
    else
    {
        // Append a leading zero.
        buffer[buffer_index++] = '0';
    }
    
    // Append the decimal point.
    buffer[buffer_index++] = '.';

    // Convert the fractional part, even if it is zero, and leave room for the NULL terminator.
    while (buffer_index < (BUFFER_SIZE - 1))
    {
        fractional_value *= 10;
        buffer[buffer_index++] = '0' + (int)fractional_value;
        fractional_value = modf(fractional_value, &integral_value);
    }

    // Append the NULL terminator.
    buffer[buffer_index] = 0;
    
    if (overflow == true)
    {
        COMM_Console_Print_String("OVERFLOW");
    }
    else
    {
        COMM_Console_Print_String(buffer);
    }
}

#endif // (CONFIG__FEATURE_COMM_CONSOLE == CONFIG__FEATURE_ENABLED)

//! Converts a byte to a two-character hexadecimal representation.
/*!
 *  \param  buffer  Buffer into which to place the resulting sting.  It needs to be at least three
 *                  characters wide.
 *  \param  byte    The byte to be converted.
 */
void COMM_Console_ByteToHex(char8 * buffer, uint8_t byte)
{
    if (byte < 16)
    {
        buffer[0] = '0';
        buffer[1] = DIGITS[byte];
        buffer[2] = 0;
    }
    else
    {
        buffer[0] = DIGITS[byte / 16];
        buffer[1] = DIGITS[byte % 16];
        buffer[2] = 0;
    }
}

#if (CONFIG__FEATURE_COMM_CONSOLE == CONFIG__FEATURE_ENABLED)

/* Private Functions */

static void ConsoleISR(void)
{
    portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    
    // Check for the "Rx FIFO not empty" interrput.
    if ((UART_Console_HW->INTR_RX_MASKED & SCB_INTR_RX_MASKED_NOT_EMPTY_Msk ) != 0)
    {
        // Clear the "Rx FIFO not empty" interrput.
        UART_Console_HW->INTR_RX = UART_Console_HW->INTR_RX & SCB_INTR_RX_NOT_EMPTY_Msk;
        
        // Get the character.
        uint32_t value = UART_Console_Get();
        
        // Check if there is actually data.  Sometimes the flag is set when there is no data (why?).
        if (value != CY_SCB_UART_RX_NO_DATA)
        {
		    char8 rx_data = (char8) value;
        
            // Determine what to do with it.
            if (Command_Buffer_Index < COMM_CONSOLE_COMMAND_MAX_LENGTH)
            {
                if (rx_data == COMM_CONSOLE_END_OF_MESSAGE)
                {
                    Command_Buffer[Command_Buffer_Index] = COMM_CONSOLE_STRING_TERMINATOR;
                    Next_State = COMM_STATE_IDENTIFY_COMMAND;
                    vTaskNotifyGiveFromISR(COMM_Console_Task_Handle, &xHigherPriorityTaskWoken);
                }
                else
                {
                    Command_Buffer[Command_Buffer_Index] = rx_data;
                    Command_Buffer_Index++;
                }
            }
            else
            {
                Next_State = COMM_STATE_COMMAND_TOO_LONG;
                vTaskNotifyGiveFromISR(COMM_Console_Task_Handle, &xHigherPriorityTaskWoken);
            }
        }
    }
    
    NVIC_ClearPendingIRQ(Int_UART_Console_cfg.intrSrc);
    
    // If the state needs to change, a context switch might be required.
	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
}

static bool ConsoleCommandMatches(const char8 * const command_name)
{
	uint32_t i = 0;
	bool is_match = false;

	if (Command_Buffer[i] == command_name[i])
	{
		is_match = true;
		i++;
	}

	while ( (is_match == true) &&
		    (i < COMM_CONSOLE_COMMAND_MAX_LENGTH)  &&
		    (Command_Buffer[i] != COMM_CONSOLE_PARAMETER_DELIMITER) &&
            (Command_Buffer[i] != COMM_CONSOLE_END_OF_MESSAGE ) &&
            (Command_Buffer[i] != COMM_CONSOLE_STRING_TERMINATOR )
		  )
	{
		if ( Command_Buffer[i] != command_name[i] )
		{
			is_match = false;
		}
		i++;
	}

	return is_match;
}

//! Reverses a string in place.
/*!
 *  \param  value   Pointer to the string to be reversed.
 *  \param  length  Length of the string, including the NULL terminator.
 */
static void ReverseString(char8 * value, uint32_t length) 
{
    if (length > 1)
    {
        uint_fast32_t start = 0; 
        uint_fast32_t end = length - 1; 
        while (start < end) 
        { 
            Swap_Char8(value + start, value + end); 
            start++; 
            end--; 
        } 
    }
} 

#endif // (CONFIG__FEATURE_COMM_CONSOLE == CONFIG__FEATURE_ENABLED)