#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <sys/time.h> 
#include <pthread.h>
#include "msw.h"
#include "sio.h"
#include "i8120W.h"

unsigned char debug_falg;
unsigned long debug_cnt;
pthread_mutex_t lock=PTHREAD_MUTEX_INITIALIZER; 


int CAN_Init(unsigned char slot)
{
	int ret=0;
	ConfigStruct CanConfig;


	CanConfig.BaudRate=8; //baudrate, 4: 125K, 8: 1M
	CanConfig.AccCode[0]=0;
	CanConfig.AccCode[1]=0;
	CanConfig.AccCode[2]=0;
	CanConfig.AccCode[3]=0;
	CanConfig.AccMask[0]=0xFF;
	CanConfig.AccMask[1]=0xFF;
	CanConfig.AccMask[2]=0xFF;
	CanConfig.AccMask[3]=0xFF;
	
	//初始化並註冊中斷
  	ret=I8120_Init(slot);
  	if (ret){
  		printf("Init SlotNo %d error, error code %d\r\n",slot,ret);
		return -1;
  	}   
	usleep(1);
	
	//設定CAN參數
    ret=I8120_Config(slot,&CanConfig);
	if (ret){
		printf("Configure SloNo %d fail, error code %d\r\n",slot,ret);
		return -1;
	}
	usleep(1);
	
	return 0;
}

void* CAN_recv_callback(void* slot)
{
	unsigned char SlotNo=*(unsigned char*)slot;
	int ret=1,i=0,j=0;
	int cnt=0;
	unsigned long start=0,end=0,avg=0;
	PacketStruct CANMsg;
	unsigned short RxCnt;
	unsigned long test_cnt=0;
	unsigned char value=0;
	
	while (1)
	{
		//pthread_mutex_lock(&lock);
		ret=I8120_RxMsgCount(SlotNo, &RxCnt);
		if (RxCnt > 0)
		{
			for (j = 0; j < RxCnt; j++)
			{
				CANMsg.Id = 0;
				if (cnt == 1)
				{
					start=GetTimeTicks_ms();
				}
				//Thread.Sleep(1);
				ret = I8120_ReceiveMsg(SlotNo, &CANMsg);
				if (ret != 0)
				{
					if(ret==22)
					{
						if(CANMsg.Data[0]==value&&CANMsg.Data[1]==value&&CANMsg.Data[2]==value&&CANMsg.Data[3]==value
						&&CANMsg.Data[4]==value&&CANMsg.Data[5]==value&&CANMsg.Data[6]==value&&CANMsg.Data[7]==value)
						{
							value++;
							cnt++;
							if (cnt == 100000)
							{
								value=0;
								cnt = 0;
								end=GetTimeTicks_ms();
								printf("[Slot %d]Recv%ld: %ld ms\r\n",SlotNo,test_cnt,(end-start));
								avg+=(end-start);
								test_cnt++;
							}
						}
						else
						{
							value=CANMsg.Data[0]+1;
							printf("\n[Slot %d] want: %x, recv: %x , ",SlotNo,value,CANMsg.Data[0]);
						}
					}
					else
						printf("\n[Slot %d] recv: error code %d ",SlotNo,ret);
				}
				else
				{
					if(CANMsg.Data[0]==value&&CANMsg.Data[1]==value&&CANMsg.Data[2]==value&&CANMsg.Data[3]==value
					&&CANMsg.Data[4]==value&&CANMsg.Data[5]==value&&CANMsg.Data[6]==value&&CANMsg.Data[7]==value)
					{
						value++;
						cnt++;
						if (cnt == 100000)
						{
							value=0;
							cnt = 0;
							end=GetTimeTicks_ms();
							printf("[Slot %d]Recv%ld: %ld ms\r\n",SlotNo,test_cnt,(end-start));
							avg+=(end-start);
							test_cnt++;
						}
					}
					else
					{
						value=CANMsg.Data[0]+1;
						printf("\n[Slot %d] want: %x, recv: %x , ",SlotNo,value,CANMsg.Data[0]);
					}
				}

			}
		}
		//if(ret!=22&&ret!=0)
		//	printf("\n[Slot %d] recv: error code %d ",SlotNo,ret);
		//pthread_mutex_unlock(&lock);
		//usleep(1);
		/*else
		{
			usleep(1);
		}*/
		if (test_cnt == 10)
		{
			avg /= 10;
			printf("[Slot %d]Average : %lu ms",SlotNo,avg);
			break;
		}
	}
}

void* CAN_send_callback(void* slot)
{
	unsigned char SlotNo=*(unsigned char*)slot;
	int i,j;
	unsigned char value=0;
	PacketStruct CANMsg;
	unsigned long start=0,end=0,avg=0;
	int cnt=0,ret=0;
 	unsigned char status=0;
	
	
	CANMsg.UpperTimeStamp=0;
	CANMsg.LowerTimeStamp=0;  //TimeStamps=(MsgUpperTimeStamps<<32)+MsgLowerTimeStamps , unit:100us
	CANMsg.Mode=0;   //mode 0 -> 11 bits ID    1 -> 29 bits ID
	CANMsg.Id=value;
	CANMsg.Rtr=0;
	CANMsg.Len=8;
	for(i=0;i<8;i++)
		CANMsg.Data[i]=value;
	
	
	for(i=0;i<10;i++)
	{
		cnt=0;
		start=GetTimeTicks_ms();
		I8120_Status(SlotNo,&status);
		CANMsg.Id=SlotNo;
		while(1)
		{
			if(cnt<100000)
			{
				//送出資料到I-8120W的buffer
				//pthread_mutex_lock(&lock);
				ret=I8120_SendMsg(SlotNo,&CANMsg);
				//pthread_mutex_unlock(&lock);
				if (ret){
					if(ret!=22)
						printf("\n[Slot %d] Send CAN message fail, error code %d, %d",SlotNo,ret,cnt);
				}
				else
				{
					cnt++;
					value++;
					//CANMsg.Id=value;
					for(j=0;j<8;j++)
						CANMsg.Data[j]=value;
				}	
			}
			
			if(cnt==100000)
			{
				value=0;
				//CANMsg.Id=value;
				for(j=0;j<8;j++)
					CANMsg.Data[j]=value;
				break;
			}
			
			usleep(1); //golden,2021
		}
		end=GetTimeTicks_ms();
		printf("[Slot %d]Send%d: %ld ms\n",SlotNo,i,(end-start));
		avg+=(end-start);
	}
	avg/=10;
	printf("[Slot %d]Send average: %ld\n\n",SlotNo,avg);
	
	
}
#define RCVBUF_SIZE 50
void* SerialPort_callback()
{
	int fd_m;			        /* file descriptor */
	char cmd[10];				             	/* point to command string */
	char rbuf[RCVBUF_SIZE];			   	/* receive buffer */
	__tod_t texp;				           	/* to receive the expended time */
	unsigned char val=0;
	int rcv_val=0;
	WORD m_wT;
	int i, cmd_cnt=0;

	fd_m = sio_open("/dev/ttyS1", B9600, DATA_BITS_8, NO_PARITY,ONE_STOP_BIT);
	if (fd_m == ERR_PORT_OPEN) {
		printf("open port_m failed!\n");
		return;
	}
	sio_set_noncan(fd_m);
	sio_set_timer(fd_m, 1, 0);
	while(1)
	{
		memset(cmd,0,10);
		sprintf(cmd,"%d",val);
		//printf("send:%s\n",cmd);
		cmd_cnt=0;
		for(i=0;i<10;i++)
		{
			if(cmd[i]!=0)
				cmd_cnt++;
		}
		write(fd_m,cmd,cmd_cnt);
		memset(rbuf,0,RCVBUF_SIZE);
		read(fd_m,rbuf,RCVBUF_SIZE);
		rcv_val=atoi(rbuf);
		//printf("recv:%d\n",rcv_val);
		if(rcv_val==val)
		{
			val++;
		}
		else
		{
			printf("Serial Port error: send=%d, rcv=%d\n",val,rcv_val);
		}
		
	}

	sio_close(fd_m);
}


int main(void)
{
	int ret=0,i=0,j=0, wRetVal=0;
	pthread_t CAN1_send,CAN1_recv,CAN2_send,CAN2_recv,Serial;
	unsigned char SLOT[2]={1,2};
	 	  
  wRetVal = Open_Slot(1);
    if (wRetVal > 0) {
		  printf("Open Slot failed!\n");
		  return (-1);
	  }
	  
	wRetVal = Open_Slot(2);
    if (wRetVal > 0) {
		  printf("Open Slot failed!\n");
		  return (-1);
	
  }
      
	ret=CAN_Init(SLOT[0]);
	if (ret){
  		printf("Init SlotNo 1 error\n");
		return 0;
  	} 
	ret=CAN_Init(SLOT[1]);
	if (ret){
  		printf("Init SlotNo 2 error\n");
		return 0;
  	}
  	
	//pthread_create(&Serial,NULL,SerialPort_callback,NULL); //這邊可以註解掉不測試，這是COM port自送自收的thread
	pthread_create(&CAN1_recv, NULL, CAN_recv_callback, (void*) &SLOT[0]);
	pthread_create(&CAN2_recv, NULL, CAN_recv_callback, (void*) &SLOT[1]);
	usleep(10);
	pthread_create(&CAN1_send, NULL, CAN_send_callback, (void*) &SLOT[0]);
	pthread_create(&CAN2_send, NULL, CAN_send_callback, (void*) &SLOT[1]);
	
	
	pthread_join(CAN1_send, NULL );
	pthread_join(CAN2_send, NULL );
	pthread_join(CAN1_recv, NULL );
	pthread_join(CAN2_recv, NULL );
	//pthread_join(Serial,NULL);


	Close_Slot(1);
	Close_Slot(2);

	return 0;
}
