#include "modbus-tcp.h"

#include <tcp.h>

char tcp_buffer[1024];

#define MAX_REG 250

char registers[MAX_REG];

static err_t modbus_incomming_data(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err){
	int i;
	int len;
	char *pc;
    uint8_t bg_red = 0;
    uint8_t bg_green = 0;
    uint8_t bg_blue = 0;
    uint8_t txt_red = 0;
	uint8_t txt_green = 0;
	uint8_t txt_blue = 0;
	uint8_t nr_img = 0;
	char text[200];
	for(i = 0; i < 100;i++){
		text[i] = 95; // _
	}

	if (p != NULL){
		printf("data not null\n");
		//here im going to procces the modbusdata
		tcp_recved( pcb, p->tot_len );
		pc = (char*)p->payload;
		len =p->tot_len;

		//putting the bufer in the register array
		for( i=0; i<len; i++ ) {
			registers[i] = pc[i];//getting the error "void value not ignored as it ought to be" on this line
		}

		if(registers[7] == 0x10){// Check if it's a Modbus Write Multiple Registers request (0x10)
			printf("in writing multiple register mode\n");
		}
		//putting the values from the array in their variable
		bg_red = (uint8_t)(registers[14]);//01
		bg_green = (uint8_t)(registers[16]);//02
		bg_blue = (uint8_t)(registers[18]);//03
		txt_red = (uint8_t)(registers[20]);//04
		txt_green = (uint8_t)(registers[22]);//05
		txt_blue = (uint8_t)(registers[24]);//06
		nr_img = (uint8_t)(registers[26]);//07
						   //registers[428] //208

		printf("%d %d %d %d %d %d %d ",bg_red,bg_green,bg_blue,txt_red,txt_green,txt_blue,nr_img);

		int j = 0;
		for(i = 28;i < 428;i++){
			if(i % 2 == 0){
				text[j]=registers[i];
				j++;
			}
		}

		for(i = 0; i < 100;i++){
			printf("%c ",text[i]);
		}
		printf("\n");


	} else if (err == ERR_OK){
		tcp_close(pcb);//when everithing was ok close the tcpconnection
	}
	return ERR_OK;
}

static err_t modbus_accept(void *arg, struct tcp_pcb *pcb, err_t err )
{
    LWIP_UNUSED_ARG( arg );//Eliminates compiler warning about unused arguments
    LWIP_UNUSED_ARG( err );//Eliminates compiler warning about unused arguments
    tcp_setprio( pcb, TCP_PRIO_MIN );//Sets the priority of a connection.
    tcp_recv( pcb, modbus_incomming_data);//sets which function is being called when new data arives
    tcp_err( pcb, NULL );//sets the function thats called when an error ocours
    tcp_poll( pcb, NULL, 4 );//the aplication is being polled every 4 seconds
    tcp_write( pcb, "TEST\r\n", 8, 0 );//pcb , data , lenght in bytes , apiflags
    printf("connected\n");
    tcp_sent( pcb, NULL );//Specifies the callback function that should be called when data has successfully been received

    return ERR_OK;
}

void modbus_init( void )
{
    struct tcp_pcb *modbus_pcb;
    modbus_pcb = tcp_new();//creating a new tcp pcb
    tcp_bind(modbus_pcb, IP_ADDR_ANY, MODBUSPOORT);//bind the modbus_pcb to port 502

    modbus_pcb = tcp_listen( modbus_pcb );//listen
    tcp_accept(modbus_pcb, modbus_accept);//set callback function
}