#include <stdlib.h>
#include <stdio.h>
#include "ioctl.h"
#include "sample_grabber.h"

#define CALIBRATION_DATA "/etc/penmount.dat"

extern int g_userAbort;
extern int g_curState;
extern int g_prevState;
extern int g_touchEnabled;
extern int g_IOCTLInProgress;

int calibPointsNum = 4;
int pmode;
int Raw_X[4];
int Raw_Y[4];
int Cal_X[2];
int Cal_Y[2];

int SaveCalibPoints(char *fname, int *X, int *Y, int num)
{

        FILE *f;
        int i;
        f = fopen(fname, "w");
        if(!f) return -1;
        fprintf(f, "%d ", num);

        for(i =0; i<25;i++)
        {
                if(i<num)
                {
                fprintf(f, "%d %d ", X[i], Y[i]);
                }
        }
        fclose(f);
        return 0;
}


void SetCalibrationData() {
	int temp;
	FILE *f;
	
	if( (Raw_X[3]<Raw_X[0] && Raw_X[0]<Raw_X[2] && Raw_X[2]<Raw_X[1])&& (Raw_Y[0]<Raw_Y[1] && Raw_Y[1]<Raw_Y[3] && Raw_Y[3]<Raw_Y[2]) )
	{
	pmode=1;
	}
	else if( (Raw_X[3]<Raw_X[0] && Raw_X[0]<Raw_X[2] && Raw_X[2]<Raw_X[1])&& (Raw_Y[2]<Raw_Y[3] && Raw_Y[3]<Raw_Y[1] && Raw_Y[1]<Raw_Y[0]) )
	{
	pmode=2;
	}
	else if( (Raw_X[1]<Raw_X[2] && Raw_X[2]<Raw_X[0] && Raw_X[0]<Raw_X[3])&& (Raw_Y[0]<Raw_Y[1] && Raw_Y[1]<Raw_Y[3]&& Raw_Y[3]<Raw_Y[2]) )
	{
	pmode=3;
	}
	else if( (Raw_X[1]<Raw_X[2] && Raw_X[2]<Raw_X[0] && Raw_X[0]<Raw_X[3])&& (Raw_Y[2]<Raw_Y[3] && Raw_Y[3]<Raw_Y[1] && Raw_Y[1]<Raw_Y[0]) )
	{
	pmode=4;
	}
	else if( (Raw_X[0]<Raw_X[1] && Raw_X[1]<Raw_X[3] && Raw_X[3]<Raw_X[2])&& (Raw_Y[3]<Raw_Y[0] && Raw_Y[0]<Raw_Y[2] && Raw_Y[2]<Raw_Y[1]) )
	{
	pmode=5;
	}
	else if( (Raw_X[2]<Raw_X[3] && Raw_X[3]<Raw_X[1] && Raw_X[1]<Raw_X[0])&& (Raw_Y[3]<Raw_Y[0] && Raw_Y[0]<Raw_Y[2] && Raw_Y[2]<Raw_Y[1]) )
	{
	pmode=6;
	}
	else if( (Raw_X[0]<Raw_X[1] && Raw_X[1]<Raw_X[3] && Raw_X[3]<Raw_X[2])&& (Raw_Y[1]<Raw_Y[2] && Raw_Y[2]<Raw_Y[0] && Raw_Y[0]<Raw_Y[3]) )
	{
	pmode=7;
	}
	else if( (Raw_X[2]<Raw_X[3] && Raw_X[3]<Raw_X[1] && Raw_X[1]<Raw_X[0])&& (Raw_Y[1] <Raw_Y[2] && Raw_Y[2]<Raw_Y[0] && Raw_Y[0]<Raw_Y[3]) )
	{
	pmode=8;
	}
	else
	{
	pmode=8;
	}

	if(pmode >= 5)
	{
		Cal_Y[0]=Raw_X[0];
		Cal_Y[1]=Raw_X[2];
		Cal_X[0]=Raw_Y[3];
		Cal_X[1]=Raw_Y[1];
	}
	else
	{
		Cal_Y[0]=Raw_Y[0];
		Cal_Y[1]=Raw_Y[2];
		Cal_X[0]=Raw_X[3];
		Cal_X[1]=Raw_X[1];
	}

	if(Cal_Y[0] > Cal_Y[1]) 
	{
		temp = Cal_Y[1];
		Cal_Y[1] = Cal_Y[0];
		Cal_Y[0] = temp;
	}
	if(Cal_X[0] > Cal_X[1]) 
	{
		temp = Cal_X[1];
		Cal_X[1] = Cal_X[0];
		Cal_X[0] = temp;
	}

	f = fopen(CALIBRATION_DATA, "w");
	if(f != NULL) 
	{
		fprintf(f, "%d %d %d %d %d", 
			pmode, Cal_X[0], Cal_X[1], Cal_Y[0], Cal_Y[1]);
		fclose(f); 
	}
}

void *IOWorkerThread(void* arg)
{
	unsigned char cmdbuf[IOCTL_COMMAND_BUFFER_LEN] = {0,0,0,0,0,0,0,0,0,0};
	// 5->6
	unsigned char databuf[IOCTL_DATA_BUFFER_LEN] = {0,0,0,0,0};
	int protocol_err_num = 0;

	FILE *f;
	int	i;
	
	while(g_curState < calibPointsNum)
	{
		if(g_userAbort) goto ABORT;
		if(g_touchEnabled)
		{
		//TODO: what if keep gettign garbage?
		protocol_err_num = 0;
		g_IOCTLInProgress = 1;
		while(DeviceIOControl(cmdbuf, IOCTL_COMMAND_BUFFER_LEN, databuf, IOCTL_DATA_BUFFER_LEN))
		{
			g_IOCTLInProgress = 0;

			
			if(databuf[0] == 0xFF && databuf[1] < 0x08 && databuf[3] < 0x08 ) {
				g_touchEnabled = 0;
				g_IOCTLInProgress = 0;
				Raw_X[g_curState] = databuf[1]*128 + databuf[2];
				Raw_Y[g_curState] = databuf[3]*128 + databuf[4];
				databuf[0] = databuf[1] = databuf[2] = databuf[3] = databuf[4] = 0;
				g_prevState = g_curState;
				g_curState++;
				break;
			} else if(databuf[0] == 0x70 && databuf[1] < 0x10 && databuf[3] < 0x10 ) {
				g_touchEnabled = 0;
				g_IOCTLInProgress = 0;
				Raw_X[g_curState] = databuf[1]*256 + databuf[2];
				Raw_Y[g_curState] = databuf[3]*256 + databuf[4];
				databuf[0] = databuf[1] = databuf[2] = databuf[3] = databuf[4] = 0;
				g_prevState = g_curState;
				g_curState++;
				break;
			} else {
				protocol_err_num++;
				if(protocol_err_num > 10)
					goto ABORT;					
			}				
			databuf[0] = databuf[1] = databuf[2] =
				databuf[3] = databuf[4] = 0;
		} // end of while(DeviceIoControl(cmdbuf, databuf))
		
		//g_IOCTLInProgress = 0;
		//Raw_X[g_curState] = databuf[1]*128 + databuf[2];
		//Raw_Y[g_curState] = databuf[3]*128 + databuf[4];
		//Raw_X[g_curState] = databuf[1]*256 + databuf[2];
		//Raw_Y[g_curState] = databuf[3]*256 + databuf[4];
		//databuf[0] = databuf[1] = databuf[2] =
		//	databuf[3] = databuf[4] = 0;
		//g_prevState = g_curState;
		//g_curState++;			
		} // end of if(g_touchEnabled)
		if(g_curState == calibPointsNum)
			break;
		usleep(100000);
	} // end of while(g_curState < calibPointsNum)	
	
	while(g_touchEnabled == 0) usleep(500000);
	
	if(!g_userAbort) {
		SetCalibrationData();
		cmdbuf[0] = IOCTL_SET_CALIBRATION;
		cmdbuf[1] = pmode;
		/*
		cmdbuf[2] = Cal_X[0]/256;
		cmdbuf[3] = Cal_X[0]%256;
		cmdbuf[4] = Cal_X[1]/256;
		cmdbuf[5] = Cal_X[1]%256;
		cmdbuf[6] = Cal_Y[0]/256;
		cmdbuf[7] = Cal_Y[0]%256;
		cmdbuf[8] = Cal_Y[1]/256;
		cmdbuf[9] = Cal_Y[1]%256;
		*/
		cmdbuf[2] = (Cal_X[0] >> 8) & 0x0F;
		cmdbuf[3] = (Cal_X[0] & 0xFF);
		cmdbuf[4] = (Cal_X[1] >> 8) & 0x0F;
		cmdbuf[5] = (Cal_X[1] & 0xFF);
		cmdbuf[6] = (Cal_Y[0] >> 8) & 0x0F;
		cmdbuf[7] = (Cal_Y[0] & 0xFF);
		cmdbuf[8] = (Cal_Y[1] >> 8) & 0x0F;
		cmdbuf[9] = (Cal_Y[1] & 0xFF);

		f = fopen("/var/log/std-calib.log", "a");
		
		fprintf(f, "Raw_X : ");
		for(i=0; i<4; i++)
			fprintf(f, "%d ", Raw_X[i]);
		fprintf(f, "\n");
		
		fprintf(f, "Raw_Y : ");
		for(i=0; i<4; i++)
			fprintf(f, "%d ", Raw_Y[i]);
		fprintf(f, "\n");
		
		fprintf(f, "Cal_XY : %d, %d, %d, %d\n",
					Cal_X[0], Cal_X[1], Cal_Y[0], Cal_Y[1]);
				
		fprintf(f, "cmdbuf : ");
		for(i=0; i<10; i++)
			fprintf(f, "%02x ", cmdbuf[i]);
		fprintf(f, "\n");
		fclose(f);
		
		
		DeviceIOControl(cmdbuf, IOCTL_COMMAND_BUFFER_LEN, databuf, IOCTL_DATA_BUFFER_LEN);
	}
	
	g_prevState = g_curState;
	g_curState++;
	usleep(500000);
	pthread_exit((void *)0);	
	
ABORT:
	pthread_exit((void *)0);
	
}