#define X5K_MCU_CMD_GET_TEMP	"#t\n"

#define X5K_MCU_TEMPSIZE 12

#include <proto/exec.h>
#include <proto/dos.h>
#include <devices/serial.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

struct MsgPort* port = NULL;
struct IOExtSer* req = NULL;
uint8 dev = 1;


struct TempData
{ /* all values in degrees Celsius */
	int32 PCB;
	int32 CPU;
	int32 PCI;
};

void cleanup_stuff(void);
int32 ConvertTemp(struct TempData *td, char* t);
int32 HexToInt(char **c, int len);

int32 HexToInt(char **c, int len)
{
	int32 r = 0;
	int32 t = 0;
	while(len)
	{
		if( (**c >='0') & (**c <= '9') )
			t = **c - 48;

		if( (**c >='A') & (**c <= 'F') )
			t = **c - 55;

		if( (**c >='a') & (**c <= 'f') )
			t = **c - 87;

		r = (r << 4) + t;
		(*c)++;
		len--;
	}
	return r;
	}

int32 ConvertTemp(struct TempData *td, char* t)
{
	int32 ret = 0;
	
	if(	(t[0] != '$') | (t[1] != 't') )
	{
		return 1;
	}
	t = t + 3;
	td->PCB = HexToInt(&t, 2);
	t++;
	td->CPU = HexToInt(&t, 2);
	t++;
	td->PCI = HexToInt(&t, 2);

	return ret;
	}


void cleanup_stuff(void)
{
	if(dev)
	{
		IExec->CloseDevice((struct IORequest*)req);
	}
	if(req)
	{
		IExec->FreeSysObject(ASOT_IOREQUEST, req);
	}
	if(port)
	{
		IExec->FreeSysObject(ASOT_PORT, port);
	}
}



int main()
{

/*	
	char test_temp[X5K_MCU_TEMPSIZE] = "$t+20+38+4B\n";
*/	
	struct TempData temp = {0,0,0};
	
	int ret = RETURN_OK;

#define BUFSIZE 60
	char buf[BUFSIZE];
	uint8 err = 0;

while(1)
{
	port = IExec->AllocSysObject(ASOT_PORT, NULL);
	if(!port)
	{
		printf("No port\n");
		cleanup_stuff();
	}
	req = IExec->AllocSysObjectTags(ASOT_IOREQUEST, ASOIOR_Size, sizeof(struct IOExtSer),
													ASOIOR_ReplyPort, port,
													TAG_DONE);
	if(!req)
	{
		printf("No Request\n");
		cleanup_stuff();
	}
		
	err = IExec->OpenDevice("serial.device", 1, (struct IORequest*)req, 0);
	if(err)
	{
		dev = 0;
		printf("OpenDevice failed: %d\n", err);
		cleanup_stuff();
	}
	
	// Set parameters to address MCU 
	req->IOSer.io_Command = SDCMD_SETPARAMS;
	req->io_Baud = 38400; // 38400 Baud
	req->io_ReadLen = 8; // 8 bit data
	req->io_WriteLen = 8; // 8 bit data
	req->io_StopBits = 1; // 1 Stop bit
	req->io_SerFlags &= ~SERF_PARTY_ON; // No Parity
	req->io_SerFlags |= SERF_XDISABLED; // No Flow Control
	err = IExec->DoIO((struct IORequest*)req);
	if(err != IOERR_SUCCESS)
	{
		printf("Setting parameters to serial failed. Code: %d\n", err);
			cleanup_stuff();
	}
		
	// Query and convert values
	
	// Temperatures
	req->IOSer.io_Length = 3;
	req->IOSer.io_Data = (APTR)X5K_MCU_CMD_GET_TEMP;
	req->IOSer.io_Command = CMD_WRITE;
	err = IExec->DoIO((struct IORequest*)req);
	
	req->IOSer.io_Length = X5K_MCU_TEMPSIZE;
	req->IOSer.io_Data = (APTR)buf;
	req->IOSer.io_Command = CMD_READ;
	err = IExec->DoIO((struct IORequest*)req);
	
	ConvertTemp(&temp, buf);


	int length;

	length = snprintf( NULL, 0, "%d", temp.CPU );
	char* temp_CPU = malloc( length + 1 );
	snprintf( temp_CPU, length + 1, "%d", temp.CPU );

	length = snprintf( NULL, 0, "%d", temp.PCB );
	char* temp_PCB = malloc( length + 1 );
	snprintf( temp_PCB, length + 1, "%d", temp.PCB );

	length = snprintf( NULL, 0, "%d", temp.PCI );
	char* temp_PCI = malloc( length + 1 );
	snprintf( temp_PCI, length + 1, "%d", temp.PCI );


	IDOS->SetVar("temp_cpu", temp_CPU, -1, GVF_GLOBAL_ONLY);
	IDOS->SetVar("temp_pcb", temp_PCB, -1, GVF_GLOBAL_ONLY);
	IDOS->SetVar("temp_pcie", temp_PCI, -1, GVF_GLOBAL_ONLY);
	
	free(temp_CPU);
	free(temp_PCB);
	free(temp_PCI);
	
	cleanup_stuff();

	sleep(5);
	
//	printf ("yeah\n");
}

	exit(0);

}