/*
 * UtilCore.cpp
 *
 *  Created on: 14 April 2019
 *      Author: wentzelc
 */

// Standard C/C++ Libraries
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

// redA Libraries
#include "UtilCore.h"

//---------------------------------------------------------------------------

// Variable used to temp values with
static char    ReturnStr[1000];

//---------------------------------------------------------------------------

char * BytesToSafeStr( const char * Bytes, const int Len, const bool NoCrLf, const char SpecChar, char * OutBuf )
{
    char    * TempBuf;
    char    * BufPos;

    // Select/create  output buffer
    if (!OutBuf) {
        TempBuf = ReturnStr;
    }
    else {
//        if (!*OutBuf)
//            *OutBuf = (char*)malloc( Len+1 );
//        TempBuf = *OutBuf;
        TempBuf = OutBuf;
    }
    BufPos = TempBuf;

    // Remove special char
    for (int i=0; i<Len; i++) {
        if (((Bytes[i] < 32) || (Bytes[i] > 126)) ||
            (NoCrLf && ((Bytes[i] == '\r') || (Bytes[i] == '\n')))) {
            *BufPos = SpecChar;
        }
        else {
            *BufPos = Bytes[i];
        }
        BufPos++;
    }
    BufPos = 0;
    return TempBuf;
}
//---------------------------------------------------------------------------

char * BytesToHexStr( const char * Bytes, const int Len, const char * Separator, char * OutBuf )
{
    char    * TempBuf;
    char    * BufPos;
    bool    First       = true;
    char    SepLen      = (Separator)? strlen(Separator) : 0;

    // Select/create  output buffer
    if (!OutBuf) {
        TempBuf = ReturnStr;
    }
    else {
//        if (!*OutBuf)
//            *OutBuf = (char*)malloc( Len*(2+SepLen)+1 );
//        TempBuf = *OutBuf;
        TempBuf = OutBuf;
    }
    BufPos = TempBuf;

    // Print Hex values of individual bytes
    for (int i=0; i<Len; i++) {
        sprintf( BufPos, "%s%02X", ((First)? "" : Separator), (unsigned char)Bytes[i] );
        BufPos += (First)? 2 : 2+SepLen;
        First = false;
    }
    BufPos = 0;
    return TempBuf;
}
//---------------------------------------------------------------------------

char * BytesToBinStr( const char * Bytes, const int Len, const char * Separator, char * OutBuf )
{
    char    * TempBuf;
    char    * BufPos;
    bool    First       = true;
    char    SepLen      = (Separator)? strlen(Separator) : 0;

    // Select/create  output buffer
    if (!OutBuf) {
        TempBuf = ReturnStr;
    }
    else {
//        if (!*OutBuf)
//            *OutBuf = (char*)malloc( Len*(8+SepLen)+1 );
//        TempBuf = *OutBuf;
        TempBuf = OutBuf;
    }
    BufPos = TempBuf;

    // Print each byte as 8-bit binary
    for (int i=0; i<Len; i++) {
        sprintf( BufPos, "%s%c%c%c%c%c%c%c%c", ((First)? "" : Separator),
                 (Bytes[i] & 0x80)?'1':'0', (Bytes[i] & 0x40)?'1':'0', (Bytes[i] & 0x20)?'1':'0', (Bytes[i] & 0x10)?'1':'0',
                 (Bytes[i] & 0x08)?'1':'0', (Bytes[i] & 0x04)?'1':'0', (Bytes[i] & 0x02)?'1':'0', (Bytes[i] & 0x01)?'1':'0' );
        BufPos += (First)? 8 : 8+SepLen;
        First = false;
    }
    BufPos = 0;
    return TempBuf;
}
//---------------------------------------------------------------------------

char * HexStrToBytes( const char * Str, const int Len, const char * Separator, char * OutBuf )
{
    char        * TempBuf;
    u_int8_t    * BufPos;
    char        * StrPos    = (char*)Str;
    char        SepLen      = (Separator)? strlen(Separator) : 0;

    // Select/create  output buffer
    if (!OutBuf) {
        TempBuf = ReturnStr;
    }
    else {
//        if (!*OutBuf)
//            *OutBuf = (char*)malloc( Len/(2+SepLen)+1 );
//        TempBuf = *OutBuf;
        TempBuf = OutBuf;
    }
    BufPos = (u_int8_t*)TempBuf;

    // Print Hex values of individual bytes
    while ((StrPos - Str < Len) && isxdigit(StrPos[0]) && isxdigit(StrPos[1])) {
        *BufPos = (HexToInt(StrPos[0]) << 4) + HexToInt(StrPos[1]);
        BufPos++;
        StrPos += 2;
        if (Separator) {
            if (memcmp( StrPos, Separator, SepLen ))
                break;
            else
                StrPos += SepLen;
        }
    }
    BufPos = 0;
    return TempBuf;
}
//---------------------------------------------------------------------------

char *  StrSearch( const char * Haystack, const char * Needle, const int HaystackLen, const int NeedleLen )
{
    if ((Haystack == NULL) || (Needle == NULL))
        return NULL;

    char *  hPos    = (char*)Haystack;
    char *  nPos    = (char*)Needle;
    int     hLen    = (!HaystackLen)? strlen(Haystack) : HaystackLen;
    int     nLen    = (!NeedleLen)?   strlen(Needle)   : NeedleLen;
    char *  Mark    = NULL;

    while ((hPos = (char*)memchr( hPos, *nPos, (hLen-(hPos-Haystack)) ))) {                 // Check if first char match
        Mark = hPos;
        hPos++; nPos++;
        while ((hPos - Haystack < hLen) && (nPos - Needle < nLen) && (*hPos == *nPos)) {    // Check remain char match
            hPos++; nPos++;
        }
        if (nPos - Needle == nLen) return Mark;     // Found it!
        hPos = Mark+1;                              // Continue search
        nPos = (char*)Needle;
    }
    return NULL;
}
//---------------------------------------------------------------------------