#include<sys/socket.h>
#include<netinet/in.h>
#include<sys/types.h>
#include<stdio.h>
#include<string.h>
#include<arpa/inet.h>
#include<stdlib.h>
#include<unistd.h>
#include<inttypes.h>
#include"server.h"

#define BUF_LENGHT 1024

uint16_t get_request(int socketfd, uint8_t *op, uint16_t *st, uint16_t *n, uint8_t **val){
	int i,ti;
	unsigned int len = 0;
	uint8_t *apdu = (uint8_t *) malloc(1);

    ti = receive_modbus_request(socketfd, &apdu);

	*op = apdu[0];
	if(*op == 0x03){
		*st = apdu[1];
		*st = *st << 8;
		*st = *st ^ apdu[2];

		*n = apdu[3];
		*n = *n << 8;
		*n = *n ^ apdu[4];

		len = 5;
	}
	else if(*op == 0x10){
		*st = apdu[1];
		*st = *st << 8;
		*st = *st ^ apdu[2];

		*n = apdu[3];
		*n = *n << 8;
		*n = *n ^ apdu[4];

		int B = apdu[5];
		len = 6 + B;

		printf("\nst: %x, n: %x\n\n", *st,*n);

		*val = (uint8_t *) malloc(B);
		for(i = 0; i < B; i++){
			val[i] = (uint8_t *) malloc(16);
			val[i] = &apdu[i+6];
		}
	}
	else{
		printf("Invalid function code\n");
		return -1;
	}
	printf("op: %x, len: %x\n",*op,len);
	printf("\nClient %d request:\n",ti);
	for(i = 0; i < len; i++)
		printf("apdu[%d]: %x \n",i, apdu[i]);

	return ti;
}

int send_response(int socketfd, uint8_t ti, uint8_t op, uint16_t st, uint16_t n, uint8_t *val){
	uint8_t *apdu, len;

	if(op == 0x10){
		len = 5;
		apdu = (uint8_t *) malloc(len);
		apdu[0] = op;
		apdu[1] = st >> 8;
		apdu[2] = st;
		apdu[3] = n >> 8;
		apdu[4] = n;
		send_modbus_response(socketfd,apdu,len, ti);
	}
	if(op == 0x03){
		int B = n, i;
		len = 2 + B;
		apdu = (uint8_t *) malloc(len);
		apdu[0] = op;
		apdu[1] = B;

		for(i = 2; i < len; i++)
			apdu[i] = val[i-2];

		for(i = 0; i < len; i++)
			printf("apdu[%d]: %d\n",i,apdu[i]);

		send_modbus_response(socketfd,apdu,len,ti);
	}
	return 0;
}