#ifndef _HEX_STRINGS_H_
#define _HEX_STRINGS_H_

unsigned char * AsciiToChar ( unsigned char * cAsciiString, unsigned char ucLen )
{
    unsigned char * p_ucBuf;
    unsigned char i;
    p_ucBuf = ( unsigned char * ) calloc ( ucLen / 2 + 1, sizeof ( unsigned char ) );

    //Convert HEX string to data
    for ( i = 0; i < ucLen; i += 2 )
    {
        p_ucBuf[ i / 2 ] = ( cAsciiString[ i ] > '9' ) ? ( cAsciiString[ i ] - 'A' + 10 ) : ( cAsciiString[ i ] - '0' );
        p_ucBuf[ i / 2 ] <<= 4;
        p_ucBuf[ i / 2 ] |= ( cAsciiString[ i + 1 ] > '9' ) ? ( cAsciiString[ i + 1 ] - 'A' + 10 ) : ( cAsciiString[ i + 1 ] - '0' );
    }
    
    p_ucBuf [ ucLen / 2 ] = 0;
    return p_ucBuf;
}

unsigned char * CharToAscii ( unsigned char * cHexData, unsigned char ucLen )
{
    unsigned char * p_ucBuf;
    unsigned char  i;
    p_ucBuf = ( unsigned char * ) calloc ( ucLen * 2 + 1, sizeof ( unsigned char ) );

    //Convert data to HEX string
    for ( i = 0; i < ucLen; i ++ )
    {
        p_ucBuf[ i * 2 + 1 ] = ( ( ( cHexData[ i ] ) & 0x0F ) > 9 ) ? ( ( cHexData[ i ] & 0x0F ) + 'A' - 10 ) : ( ( cHexData[ i ] & 0x0F ) + '0' );
        p_ucBuf[ i * 2 ] = ( ( cHexData [ i ] >> 4 ) > 9 ) ? ( ( cHexData[ i ] >> 4 ) + 'A' - 10 ) : ( ( cHexData[ i ] >> 4 ) + '0' );
    }
    
    p_ucBuf [ ucLen * 2 ] = 0;
    return p_ucBuf;
}

unsigned char * OctToSeptSMS ( unsigned char * p_ucData, unsigned char ucLen )
{
    //Convert octets to septets
    unsigned char * p_ucBuf;
    unsigned char i;
    p_ucBuf = ( unsigned char * ) calloc ( ucLen + ucLen / 7 + 1, sizeof ( unsigned char ) );

    unsigned char k, s;
    
    p_ucBuf[ 0 ] = p_ucData[ 0 ] & ~( 1 << 7 );

    for ( i = 1, k  = 7, s = 0; i < ucLen; i++ )
    {
        p_ucBuf[ i + s ] = ( p_ucData[ i - 1 ] >> k ) | ( p_ucData[ i ] << ( 8 - k ) );
        k--;
        if ( k == 1 )
        {
            s++;
            p_ucBuf[ i + s ] = ( p_ucData[ i - 1  ] >> k );
            k = 7;
        }
    }

    p_ucBuf [ ucLen + ucLen / 7 ] = 0;
    return p_ucBuf;
}

unsigned char * SeptToOctSMS ( unsigned char * p_ucData, unsigned char ucLen )
{
    //Convert octets to septets
    unsigned char * p_ucBuf;
    unsigned char i;
    p_ucBuf = ( unsigned char * ) calloc ( ucLen - ucLen / 8 + 1, sizeof ( unsigned char ) );

    unsigned char k, s;

    for ( i = 0, k = 0, s = 0; i < ucLen - 1; i++ )
    {
        p_ucBuf[ i - s ] = ( p_ucData[ i ] >> k ) | ( p_ucData[ i + 1 ] << ( 6 - k ) );
        k++;
        if ( k == 6 )
        {
            s++;
            i++;
            k = 0;
        }
    }

    p_ucBuf[ i - s ] = p_ucData[ i ] >> k;
    p_ucBuf [ ucLen - ucLen / 8 ] = 0;
    return p_ucBuf;
}

#endif //_HEX_STRINGS_H_

p_ucBuf = AsciiToChar ( (unsigned char *)argv[2], strlen ( argv[2] ) );
p_ucStr = OctToSeptSMS ( (unsigned char *)p_ucBuf, strlen ( (char *) p_ucBuf ) );
free ( p_ucBuf );
p_ucBuf = p_ucStr;
p_ucStr = CharToAscii ( (unsigned char *)p_ucBuf, strlen ( (char *) p_ucBuf ) );
free ( p_ucBuf );
cout << p_ucStr << endl << flush;

               char tst1( char hi_char )
               {
                   if( hi_char > '9' )
   \   CPI     R16, 58
   \   BRCS    ??tst1_0
                 {   hi_char |= 0x20;    
                       hi_char -= 'A' + 10;
   \   ORI     R16, 0x20
   \   SUBI    R16, 75
   \   RET
                   }
                   else
                       hi_char -= '0';
   \   ??tst1_0:
   \   SUBI    R16, 48
                   return( hi_char );
   \   RET
               }
                



                   char tst2( char hi_char )
                  {
                 hi_char -= '0';
   \   SUBI    R16, 48
                if (hi_char > 9)
   \   CPI     R16, 10
   \   BRCS    ??tst2_0
                    {
                    hi_char &= ~0x20;    
                    hi_char -= 'A' - '0' - 0x0A;
   \   ANDI    R16, 0xDF
   \   SUBI    R16, 7
                   }
                return( hi_char );
   \   ??tst2_0:
   \   RET
              }