#include "ParsePack.h"

void Packer_init(Packer_t *pack)
{
    pack->buff[0] = head1;
    pack->buff[1] = head2;
}

size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint32_t len)
{
    size_t j;
	size_t i;
	uint8_t sum;

	pack->buff[2] = id;


//    unsigned char *pt = (unsigned char *)&len;

//    pack->buff[3] = pt[0];
//    pack->buff[4] = pt[1];
//    pack->buff[5] = pt[2];
//    pack->buff[6] = pt[3];


    pack->buff[3] = (len & 0x000000FF) >> 0;
    pack->buff[4] = (len & 0x0000FF00) >> 8;
    pack->buff[5] = (len & 0x00FF0000) >> 16;
    pack->buff[6] = (len & 0xFF000000) >> 24;


	sum = 0u;
    for (i=7,j=0u;j<len;++i,++j)
	{
		pack->buff[i] = pkg[j];
	    sum += pkg[j];
	}
	pack->buff[i++] = sum;
	return i;
}

void Parser_init(Parser_t *parser)
{
  parser->stage = 0;
}


int Parser_char(Parser_t *parser, uint8_t c)
{
	int rslt = -1;

	switch (parser->stage)
	{
	case 0:
        if (c == head1)
		{
			parser->stage++;
		}
		break;
	case 1:
        if (c == head2)
		{
			parser->stage++;
		}
		else
		{
			parser->stage = 0;
		}
		break;
	case 2:
		parser->id = c;
        parser->len_count = 0;
        parser->len = 0;
		parser->stage++;
		break;
	case 3:

        parser->len += ((uint32_t)c << (8 * parser->len_count));

        parser->len_count++;
        if(parser->len_count >= 4)
        {
            parser->stage++;
            parser->idx = 0;
            parser->sum = 0;
        }
		break;
	case 4:
		parser->buff[parser->idx++] = c;
		parser->sum += c;
		if (parser->idx == parser->len)
		{
			parser->stage++;
		}
		break;
	case 5:
		if (parser->sum == c)
		{
			rslt = parser->id;
		}
		parser->stage = 0;
		break;
	default:
		parser->stage = 0;
		break;
	}

	return rslt;
}