// Write a value in a given register
int sh_reg_write(int argc, char ** argv) {
	uint16_t reg;
	uint16_t val;
	int ok;

	unsigned int * p;

	cprintf("write\n\r");

	if (argc < 3) {
		cprintf("error: missing argument\r\n");
		cprintf("correct usage:\r\n");
		cprintf("\t%s REGISTER VALUE\r\n", argv[0]);

		return -1;
	}

	reg = read_uint16(argv[1], &ok);
	val = read_uint16(argv[2], &ok);

	cprintf("r=%d v=%d\r\n", reg, val);

	p = (unsigned int *)reg;
	*p = val;

	return 0;
}

void 
Squeezer_file_header_t::read_from_file(FILE * in)
{
    file_type_mark[0] = read_uint8(in);
    file_type_mark[1] = read_uint8(in);
    file_type_mark[2] = read_uint8(in);
    file_type_mark[3] = read_uint8(in);

    floating_point_check = read_double(in);

    date_year = read_uint16(in);
    date_month = read_uint8(in);
    date_day = read_uint8(in);

    time_hour = read_uint8(in);
    time_minute = read_uint8(in);
    time_second = read_uint8(in);

    radiometer.horn = read_uint8(in);
    radiometer.arm = read_uint8(in);
    od = read_uint16(in);
    first_obt = read_double(in);
    last_obt = read_double(in);
    first_scet_in_ms = read_double(in);
    last_scet_in_ms = read_double(in);
    number_of_chunks = read_uint32(in);
}

static void parse_destination_down_message(int s, const uint8_t* data_items, uint16_t len)
{
	const uint8_t* data_item = data_items;

	printf("Received Destination Down message from modem:\n");

	/* The message has been validated so just scan for the relevant data_items */
	while (data_item < data_items + len)
	{
		/* Octets 0 and 1 are the data item type */
		enum dlep_data_item item_id = read_uint16(data_item);

		/* Octets 2 and 3 are the data item length */
		uint16_t item_len = read_uint16(data_item + 2);

		/* Increment data_item to point to the data */
		data_item += 4;

		switch (item_id)
		{
		case DLEP_MAC_ADDRESS_DATA_ITEM:
			printf("  MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n",data_item[0],data_item[1],data_item[2],data_item[3],data_item[4],data_item[5]);
			send_destination_down_resp(s,data_item,DLEP_SC_SUCCESS);
			break;

		default:
			/* Others will be caught by the check function */
			break;
		}

		/* Increment data_item to point to the next data item */
		data_item += item_len;
	}
}

int iff_parse_rsmp(IFFChunk * ChunkInfo, const uint8_t * Buffer){
    IFFResourceMap *Map;
    bytestream b;
    unsigned i;

    if(ChunkInfo->Size < 20)
        return 0;
    set_bytestream(&b, Buffer, ChunkInfo->Size);
    ChunkInfo->FormattedData = calloc(1, sizeof(IFFResourceMap));
    if(ChunkInfo->FormattedData == NULL)
        return 0;

    Map = ChunkInfo->FormattedData;
    Map->Reserved = read_uint32(&b);
    Map->Version = read_uint32(&b);
    memcpy(Map->MagicNumber, b.Buffer, 4);
    skipbytes(&b, 4);
    Map->IFFSize = read_uint32(&b);
    Map->TypeCount = read_uint32(&b);
    if(Map->Reserved != 0 || Map->Version > 1)
        return 0;

    Map->ResourceTypes = calloc(Map->TypeCount, sizeof(IFFResourceType));
    if(Map->ResourceTypes == NULL)
        return 0;

    for(i=0; i<Map->TypeCount; i++){
        IFFResourceType * Type = &Map->ResourceTypes[i];
        unsigned j;
        if(b.Size < 8) return 0;

        memcpy(Type->Type, b.Buffer, 4);
        skipbytes(&b, 4);
        Type->ResourceCount = read_uint32(&b);
        Type->Resources = calloc(Type->ResourceCount, sizeof(IFFResource));
        if(Type->Resources == NULL)
            return 0;

        for(j=0; j<Type->ResourceCount; j++){
            IFFResource * Resource = &Type->Resources[j];
            if(b.Size < ((Map->Version == 0) ? 9 : 11)) return 0;
            Resource->Offset = read_uint32(&b);
            Resource->ChunkID = (Map->Version == 0) ? read_uint16(&b) : read_uint32(&b);
            Resource->Flags = read_uint16(&b);

            if(Map->Version == 0){
                if(!read_c_string(&b, &Resource->Label))
                    return 0;
            }else{
                if(!read_pascal_string(&b, &Resource->Label))
                    return 0;
            }
        }
    }
    return 1;
}

static int
rdata_ilnp64_to_string(buffer_type *output, rdata_atom_type rdata,
	rr_type* ATTR_UNUSED(rr))
{
	uint8_t* data = rdata_atom_data(rdata);
	uint16_t a1 = read_uint16(data);
	uint16_t a2 = read_uint16(data+2);
	uint16_t a3 = read_uint16(data+4);
	uint16_t a4 = read_uint16(data+6);

	buffer_printf(output, "%.4x:%.4x:%.4x:%.4x", a1, a2, a3, a4);
	return 1;
}

// Read and display the value of a given register
int sh_reg_read(int argc, char ** argv) {
	uint16_t reg;
	uint16_t val;
	int ok;

	unsigned int * p;

	if (argc < 2) {
		cprintf("error: missing argument\r\n");
		cprintf("correct usage:\r\n");
		cprintf("\t%s REGISTER\r\n", argv[0]);

		return -1;
	}

	cprintf("read\n\r");

	reg = read_uint16(argv[1], &ok);

	p = (unsigned int *)reg;
	val = *p;

	cprintf("r=%d v=%d\r\n", reg, val);

	return 0;
}

int sh_speed(int argc, char ** argv) {
	uint16_t timeout = 0;
	int left;
	int right;
	int ok;


	if (argc < 3) {
		cprintf("correct usage:\r\n");
		cprintf("\t%c [o] LEFT RIGHT <TIMEOUT>\r\n", argv[0]);
		return -1;
	}

	if (argc > 3) {
		timeout = read_uint16(argv[3], &ok);
		timer_start_cb(timer_id, timeout, 1);
	}

	left = read_int16(argv[1], &ok);
	right = read_int16(argv[2], &ok);
	cprintf("%d %d\r\n",left, right);

//	speed_start(speed_l);
//	speed_start(speed_r);

	speed_setPoint(speed_l, left);
	speed_setPoint(speed_r, right);

	return 0;
}

static uint16_t SPICE_GNUC_UNUSED consume_uint16(uint8_t **ptr)
{
    uint16_t val;
    val = read_uint16(*ptr);
    *ptr += 2;
    return val;
}

    /**
     * Reads a variable length integer.
     */
    std::uint64_t read_var_int()
    {
        std::uint64_t ret = 0;

        std::uint8_t size = read_uint8();

        if (size < 253)
        {
            ret = size;
        }
        else if (size == 253)
        {
            ret = read_uint16();
        }
        else if (size == 254)
        {
            ret = read_uint32();
        }
        else if (size == 255)
        {
            ret = read_uint64();
        }
        else
        {
            assert(0);
        }

        return ret;
    }

static int send_rtp(const uint8_t *pkt, size_t len, void *arg){
	struct sync_state* pss = (struct sync_state*)arg;
    
	pthread_mutex_lock(&pss->mutex);
	pss->event = true;
	pss->pt = read_uint8(&pkt[1]);
    uint16_t seq = read_uint16(&pkt[2]);
    uint32_t timestamp = read_uint32(&pkt[4]);
    pss->ssrc = read_uint32(&pkt[8]);
    pss->seq_diff = seq - pss->prev_seq;
    if(pss->seq_diff < 0) {
        pss->seq_diff += 0xffff;
    }
    pss->prev_seq = seq;
    pss->timestamp_diff = timestamp - pss->prev_timestamp;
    if(pss->timestamp_diff < 0) {
        pss->timestamp_diff += 0xfffffff;
    }
    pss->prev_timestamp = timestamp;
    memcpy(pss->packet_buf, pkt, len);
    pss->nbytes = len;
    
	pthread_cond_signal(&pss->cond);
	pthread_mutex_unlock(&pss->mutex);

    return 0;
}

static gboolean
read_old_length (const guchar **at,
                 const guchar *end,
                 guchar ctb,
                 gsize *pkt_len)
{
	gsize llen = ctb & 0x03;
	guint16 v16;
	guint32 v32;
	guint8 c;

	if (llen == 0) {
		if (!read_byte (at, end, &c))
			return FALSE;
		*pkt_len = c;
	} else if (llen == 1) {
		if (!read_uint16 (at, end, &v16))
			return FALSE;
		*pkt_len = v16;
	} else if (llen == 2) {
		if (!read_uint32 (at, end, &v32))
			return FALSE;
		*pkt_len = v32;
	} else {
		*pkt_len = end - *at;
	}

	return TRUE;
}

static gboolean
read_mpi (const guchar **at,
          const guchar *end,
          guint16 *bits,
          guchar **value)
{
	gsize bytes;
	guint16 b;
	g_assert (at);
	if (!bits)
		bits = &b;
	if (!read_uint16 (at, end, bits))
		return FALSE;
	bytes = (*bits + 7) / 8;
	if (bytes == 0)
		return FALSE;
	if (value)
		*value = g_malloc (bytes);
	if (!read_bytes (at, end, value ? *value : NULL, bytes)) {
		if (value)
			g_free (*value);
		return FALSE;
	}
	return TRUE;
}

void read_uint16_n(const char *id, uint16_t* buf, int buf_len)
{
  int i;
  for(i = 0; i < buf_len; i++)
    {
      buf[i] = read_uint16((char*) id);
    }
}

static int
rdata_short_to_string(buffer_type *output, rdata_atom_type rdata,
	rr_type* ATTR_UNUSED(rr))
{
	uint16_t data = read_uint16(rdata_atom_data(rdata));
	buffer_printf(output, "%lu", (unsigned long) data);
	return 1;
}

static int
rdata_rrtype_to_string(buffer_type *output, rdata_atom_type rdata,
	rr_type* ATTR_UNUSED(rr))
{
	uint16_t type = read_uint16(rdata_atom_data(rdata));
	buffer_printf(output, "%s", rrtype_to_string(type));
	return 1;
}

//---------------------------------------------------------------------------------------------
void BitMessage::read_netaddr(os::NetAddress* addr) const
{
	for (int i = 0; i < 4; i++) 
	{
		addr->m_address[i] = read_uint8();
	}
	
	addr->m_port = read_uint16();  
}

static jl_fptr_t jl_deserialize_fptr(ios_t *s)
{
    int fptr = read_uint16(s);
    if (fptr == 0)
        return NULL;
    void **pbp = ptrhash_bp(&id_to_fptr, (void*)(ptrint_t)fptr);
    if (*pbp == HT_NOTFOUND)
        jl_error("unknown function pointer ID");
    return *(jl_fptr_t*)pbp;
}

char*
read_len_string(FILE *stream)
{
    uint16_t    len;

    if (!read_uint16(stream, &len)) {
        return NULL;
    }
    return read_string(stream, len);
}

int main(int argc, char* argv[])
{
        FILE* ofile1;
        FILE* ofile2;
        char ostr[17];
	uint16_t N[ORDER];

#ifdef SW
	init_pipe_handler();
	PTHREAD_DECL(gcd_daemon);
	PTHREAD_CREATE(gcd_daemon);
#endif

        ofile1 = fopen("gcd_inputs.txt","w");
        ofile2 = fopen("gcd_result.txt","w");

	int err = 0;
   
	uint16_t S = 1;
	if(argc > 1)
		S = atoi(argv[1]);
	fprintf(stderr,"Scale-factor = %d.\n",S);


	srand48(19);

	uint16_t i;
	for(i = 0; i < ORDER; i++)
	{
		N[i] = ((uint16_t) (0x0fff * drand48())) * S;
		fprintf(stderr,"N[%d] = %d\n", i, N[i]);
		write_uint16("in_data",N[i]);
		to_string(ostr,N[i]);
		fprintf(ofile1,"%s\n", ostr);
	}

	uint16_t g = read_uint16("out_data");
	fprintf(stderr,"GCD = %d.\n",g);
	to_string(ostr,g);
	fprintf(ofile2,"%s\n",ostr);

	uint32_t et = read_uint32("elapsed_time");
	fprintf(stderr,"Elapsed time = %u.\n", et);

	fclose(ofile1);
	fclose(ofile2);

#ifdef SW
	PTHREAD_CANCEL(gcd_daemon);
	close_pipe_handler();
#endif

	return(0);
}

void unpack_question(char *buf, dns_question_t* question)
{
    char *qdata = buf;
    question->label = malloc(sizeof(dns_labels_t));
    dns_labels_t *elem = question->label;
    dns_labels_t *last = NULL;
    size_t label_len;
    while ((label_len = *qdata++) != 0) {

        elem->label = malloc(label_len);
        memcpy(elem->label, qdata, label_len);
        qdata += label_len;

        last = elem;
        elem->next = malloc(sizeof(dns_labels_t));
        elem = elem->next;
    }
    free(last->next);
    last->next = NULL;
    qdata = read_uint16(qdata, &question->qtype, true);
    qdata = read_uint16(qdata, &question->qclass, true);
}