#include "cmsis_os.h"
#include "commonMsg.h"
#include "utils.h"

#define MAX_ERR_NO	4
#define VAL_PARSE	15

extern osMutexId errorListMutexHandle;

uint16_t ErrorNo[MAX_ERR_NO] = { 0 };
uint8_t CR = 255, last_CR = 255;


uint8_t checksum()
{
	uint8_t sum = 0;

	for( int i=0; i<MAX_ERR_NO; i++ )
	{
		sum += ErrorNo[i];
	}
	sum ^= 0xFF;

	return sum;
}

int8_t isAnyError()
{
	int c = 0;
	for( int i=0; i<MAX_ERR_NO; i++, c++ )
	{
		if( ErrorNo[i] == 0 )
			break;
	}
	return c;
}

int8_t addErrorToList( uint16_t code )
{
	int c = -1;

    osStatus status = osMutexWait(errorListMutexHandle, 5000);
    if (status == osOK)
    {
		c = isAnyError();
		if( c == MAX_ERR_NO-1 )
		{
			deleteErrorById( c );
		}
		ErrorNo[c] = code;
		CR = checksum();
    	osMutexRelease(errorListMutexHandle);
    }
	return c;
}

void clearErrorList()
{
    osStatus status = osMutexWait(errorListMutexHandle, 5000);
    if (status == osOK)
    {
		memset(ErrorNo,0,sizeof( ErrorNo ));
		CR = checksum();
    	osMutexRelease(errorListMutexHandle);
    }
}

int8_t findErrorByNo( uint16_t code )
{
	for( int i=0; i<MAX_ERR_NO; i++ )
	{
		if( ErrorNo[i] == code )
		{
			return i;
		}
	}
	return -1;
}

uint16_t findErrorById( int8_t id )
{
	uint16_t ret = -1;

	if( id >= 0 && id < MAX_ERR_NO )
	{
		ret = ErrorNo[id];
	}
	return ret;
}

void deleteErrorById( int8_t id )
{
    osStatus status = osMutexWait(errorListMutexHandle, 5000);
    if (status == osOK)
    {
		if( id > 0 )
		{
			ErrorNo[id] = 0;
			int i = id+1;
			while( i < MAX_ERR_NO )
			{
				ErrorNo[i] = ErrorNo[i-1];
				i++;
			}
			CR = checksum();
		}
    	osMutexRelease(errorListMutexHandle);
    }
}

void deleteErrorByNo( uint16_t code )
{
	int8_t id = findErrorByNo( code );
	deleteErrorById( id );
}

// C substring function definition
void substring(char s[], char sub[], int startIndex, int endIndex )
{
   int c = 0;

   endIndex -= startIndex;
   while ( c < endIndex ) {
      sub[c] = s[startIndex + c];
      c++;
   }
   sub[c] = '\0';
}

int toInt( char in[], int startIndex, int endIndex )
{
	char buff[VAL_PARSE] = {0};

	substring(in,buff,startIndex,endIndex);
	return atoi(buff);
}

int toIntLen( char in[], int length )
{
	char buff[VAL_PARSE] = {0};

	substring(in,buff,0,length);
	return atoi(buff);
}

long toLongLen( char in[], int length )
{
	char buff[VAL_PARSE] = {0};

	substring(in,buff,0,length);
	return atol(buff);
}

float toFloat( char in[], int startIndex, int endIndex )
{
	char buff[VAL_PARSE] = {0};

	substring(in,buff,startIndex,endIndex);
	return atof(buff);
}

bool toBool( char in[], int startIndex )
{
	int r = toInt( in, startIndex, startIndex+1 );
	return r > 0;
}
