#include	"define.h"
#include 	"main.h"

//==========================
// System basic sample constant
//==========================
#define ARRAY_SIZE(A)		(sizeof(A) / sizeof(A[0]))
#define PASS								1
#define FAIL								-1
#define YES									1
#define NO									0
#define ON									1
#define OFF									0
#define BUFFER_SIZE							128

//==========================
// Timer interval
//==========================
#define TMR_IDX_HANDSHAKING			0
#define TMR_IDX_AUTH				1
#define TMR_IDX_2					2
#define TMR_IDX_3					3
#define TMR_IDX_4					4
#define TMR_IDX_5					5
#define TMR_IDX_6					6
#define TMR_IDX_7					7
#define TMR_IDX_8					8
#define TMR_IDX_9 					9

#define TIMEOUT_SPEC_HANDSHAKING	180000
#define TIMEOUT_SPEC_AUTH			30000

#define MtdBlockSize 				0x600000

//==========================
// Declare method
//==========================
void trim(char *s);
int mystrcmp(char *p1,char *p2);
void substr(char *dest, const char* src, unsigned int start, unsigned int cnt);
void split(char **arr, char *str, const char *del);

int InitRfidPort(void);
int GetAlarmValue(void);
int GetInfoValue(void);
int GetFaultValue(void);
int GetCardSerialNumber();
void setLedMotion(unsigned char gun_index,unsigned char led_mode);
void setRelay(unsigned char gun_index,unsigned char isOn);
void setSpeaker(unsigned char isOn, unsigned char speaker_mode);

//==========================
// Declare RFID module type
//==========================
#define MODULE_EWT		0

int rfidFd = -1;
char* rfidPortName = "/dev/ttyS2";
RFID rfid;

//==========================
// Declare share memory
//==========================
struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData			*ShmStatusCodeData;
struct PsuData					*ShmPsuData;
struct CHAdeMOData				*ShmCHAdeMOData;
struct CcsData					*ShmCcsData;
struct PrimaryMcuData			*ShmPrimaryMcuData;
struct FanModuleData			*ShmFanModuleData;
struct RelayModuleData			*ShmRelayModuleData;
struct OCPP16Data				*ShmOCPP16Data;
struct Charger					*ShmCharger;

struct timeb					startTime[10];
struct timeb					startChargingTime[AC_QUANTITY];
struct timeb					endChargingTime[AC_QUANTITY];


//=================================
// Common routine
//=================================
void trim(char *s)
{
    int i=0, j, k, l=0;

    while((s[i]==' ')||(s[i]=='\t')||(s[i]=='\n'))
        i++;

    j = strlen(s)-1;
    while((s[j]==' ')||(s[j]=='\t')||(s[j]=='\n'))
        j--;

    if(i==0 && j==strlen(s)-1) { }
    else if(i==0) s[j+1] = '\0';
    else {
        for(k=i; k<=j; k++) s[l++] = s[k];
        s[l] = '\0';
    }
}

int mystrcmp(char *p1,char *p2)
{
    while(*p1==*p2)
    {
        if(*p1=='\0' || *p2=='\0')
            break;
        p1++;
        p2++;
    }
    if(*p1=='\0' && *p2=='\0')
        return(PASS);
    else
        return(FAIL);
}

void substr(char *dest, const char* src, unsigned int start, unsigned int cnt)
{
	strncpy(dest, src + start, cnt);
	dest[cnt] = 0;
}

void split(char **arr, char *str, const char *del)
{
	char *s = strtok(str, del);

	while(s != NULL)
	{
		*arr++ = s;
		s = strtok(NULL, del);
	}
}

int StoreLogMsg(const char *fmt, ...)
{
	char Buf[4096+256];
	char buffer[4096];
	time_t CurrentTime;
	struct tm *tm;
	va_list args;

	va_start(args, fmt);
	int rc = vsnprintf(buffer, sizeof(buffer), fmt, args);
	va_end(args);

	memset(Buf,0,sizeof(Buf));
	CurrentTime = time(NULL);
	tm=localtime(&CurrentTime);
	sprintf(Buf,"echo \"[%04d.%02d.%02d %02d:%02d:%02d] - %s\" >> /Storage/SystemLog/[%04d.%02d]SystemLog",
			tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,
			buffer,
			tm->tm_year+1900,tm->tm_mon+1);
#ifdef SystemLogMessage
	system(Buf);
#endif

	printf("[%04d.%02d.%02d %02d:%02d:%02d] - %s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec, buffer);

	return rc;
}

int DiffTimeb(struct timeb ST)
{
	//return milli-second
	struct timeb ET;
	unsigned int StartTime,StopTime;

	ftime(&ET);
	StartTime=(unsigned int)ST.time;
	StopTime=(unsigned int)ET.time;
	return (StopTime-StartTime)*1000+ET.millitm-ST.millitm;
}

int DiffTimeb2(struct timeb ST, struct timeb ET)
{
	//return milli-second
	unsigned int StartTime,StopTime;

	StartTime=(unsigned int)ST.time;
	StopTime=(unsigned int)ET.time;
	//return (StopTime-StartTime)*1000+ET.millitm-ST.millitm;
	return (StopTime-StartTime);
}

//======================================================
// Check interface status
//======================================================
int isInterfaceUp(const char *interface)
{
	int result = FAIL;

	FILE *fp;
	char cmd[256];
	char buf[512];

	strcpy(cmd, "ifconfig");;
	fp = popen(cmd, "r");
	if(fp != NULL)
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, interface) > 0)
			{
				result = PASS;
			}
		}
	}
	pclose(fp);

	return result;
}

//======================================================
// Create all share memory
//======================================================
int CreatShareMemory()
{
	int result = PASS;
	int MeterSMId;


	//creat ShmSysConfigAndInfo
	if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo), IPC_CREAT | 0777)) < 0)
	{
		DEBUG_ERROR("shmget ShmSysConfigAndInfo NG\r\n");
		result = FAIL;
	}
	else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{

		DEBUG_ERROR("shmat ShmSysConfigAndInfo NG\r\n");

		result = FAIL;
   	}
   	memset(ShmSysConfigAndInfo,0,sizeof(struct SysConfigAndInfo));

   	//creat ShmStatusCodeData
   	if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData), IPC_CREAT | 0777)) < 0)
	{

   		DEBUG_ERROR("shmget ShmStatusCodeData NG\r\n");

   		result = FAIL;
	}
	else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{

		DEBUG_ERROR("shmat ShmStatusCodeData NG\r\n");

		result = FAIL;
   	}
   	memset(ShmStatusCodeData, 0, sizeof(struct StatusCodeData));

   	//creat ShmPsuData
   	if ((MeterSMId = shmget(ShmPsuKey, sizeof(struct PsuData), IPC_CREAT | 0777)) < 0)
	{

   		DEBUG_ERROR("shmget ShmPsuData NG\r\n");

   		result = FAIL;
	}
	else if ((ShmPsuData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{

		DEBUG_ERROR("shmat ShmPsuData NG\r\n");

		result = FAIL;
   	}
   	memset(ShmPsuData,0,sizeof(struct PsuData));

   	//creat ShmCHAdeMOData
   	if ((MeterSMId = shmget(ShmCHAdeMOCommKey, sizeof(struct CHAdeMOData), IPC_CREAT | 0777)) < 0)
	{

   		DEBUG_ERROR("shmget ShmCHAdeMOData NG1\r\n");

   		result = FAIL;
	}
	else if ((ShmCHAdeMOData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{

		DEBUG_ERROR("shmat ShmCHAdeMOData NG2\r\n");

		result = FAIL;
    }
    memset(ShmCHAdeMOData,0,sizeof(struct CHAdeMOData));

    //creat ShmCcsData
    if ((MeterSMId = shmget(ShmCcsCommKey, sizeof(struct CcsData), IPC_CREAT | 0777)) < 0)
   	{

   		DEBUG_ERROR("shmget ShmCcsData NG\r\n");

   		result = FAIL;
	}
   	else if ((ShmCcsData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
   	{

   		DEBUG_ERROR("shmat ShmCcsData NG\r\n");

   		result = FAIL;
    }
    memset(ShmCcsData,0,sizeof(struct CcsData));

   	//creat ShmPrimaryMcuData
    if ((MeterSMId = shmget(ShmPrimaryMcuKey, sizeof(struct PrimaryMcuData), IPC_CREAT | 0777)) < 0)
   	{

   		DEBUG_ERROR("shmget ShmPrimaryMcuData NG\r\n");

   		result = FAIL;
	}
    else if ((ShmPrimaryMcuData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {

    	DEBUG_ERROR("shmat ShmPrimaryMcuData NG\r\n");

    	result = FAIL;
   	}
   	memset(ShmPrimaryMcuData,0,sizeof(struct PrimaryMcuData));

   	 /*
   	 //creat ShmFanModuleData
   	if ((MeterSMId = shmget(ShmFanBdKey, sizeof(struct FanModuleData), IPC_CREAT | 0777)) < 0)
    {

   		DEBUG_ERROR("shmget ShmFanModuleData NG\r\n");

   		result = FAIL;
	}
    else if ((ShmFanModuleData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {

    	DEBUG_ERROR("shmat ShmFanModuleData NG\r\n");

    	result = FAIL;
   	 }
   	 memset(ShmFanModuleData,0,sizeof(struct FanModuleData));

   	 //creat ShmRelayModuleData
   	if ((MeterSMId = shmget(ShmRelayBdKey, sizeof(struct RelayModuleData), IPC_CREAT | 0777)) < 0)
    {

   		DEBUG_ERROR("shmget ShmRelayModuleData NG\r\n");

   		result = FAIL;
	}
    else if ((ShmRelayModuleData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {

    	DEBUG_ERROR("shmat ShmRelayModuleData NG\r\n");

    	result = FAIL;
   	}
   	memset(ShmRelayModuleData,0,sizeof(struct RelayModuleData));*/

   	//creat ShmOCPP16Data
   	if ((MeterSMId = shmget(ShmOcppModuleKey, sizeof(struct OCPP16Data), IPC_CREAT | 0777)) < 0)
    {

   		DEBUG_ERROR("shmget ShmOCPP16Data NG\r\n");

   		result = FAIL;
	}
    else if ((ShmOCPP16Data = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {

    	DEBUG_ERROR("shmat ShmOCPP16Data NG\r\n");

    	result = FAIL;
   	}
   	memset(ShmOCPP16Data,0,sizeof(struct OCPP16Data));

	//creat ShmCharger
   	if ((MeterSMId = shmget(ShmChargerKey, sizeof(struct Charger), IPC_CREAT | 0777)) < 0)
    {

   		DEBUG_ERROR("shmget ShmCharger NG\r\n");

   		result = FAIL;
	}
    else if ((ShmCharger = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {

    	DEBUG_ERROR("shmat ShmCharger NG\r\n");

    	result = FAIL;
   	}
   	memset(ShmCharger,0,sizeof(struct Charger));

    return result;
}
//======================================================
// Peripheral initial
//======================================================
void InitGPIO()
{
	/*****************0~3, 4 bank, bank x 32+ num*********************/
	/***************************************************************/
	/*************** INPUT PIN ***************************************/
	/***************************************************************/

	/***************************************************************/
	/*************** OUTPUT PIN ************************************/
	/***************************************************************/
	/*MCU request:GPIO3_20 => H:ON; L:OFF*/
	system("echo 116 > /sys/class/gpio/export");
	system("echo \"out\" > /sys/class/gpio/gpio116/direction");
	system("echo 0 > /sys/class/gpio/gpio116/value");

	/*Rfid:GPIO0_19 => Reset_PING H:ON; L:OFF*/
	system("echo 19 > /sys/class/gpio/export");
	system("echo \"out\" > /sys/class/gpio/gpio19/direction");
	system("echo 1 > /sys/class/gpio/gpio19/value");

	/*Speaker:GPIO2_1 => H:ON; L:OFF*/
	system("echo 65 > /sys/class/gpio/export");
	system("echo \"out\" > /sys/class/gpio/gpio65/direction");
	system("echo 0 > /sys/class/gpio/gpio65/value");

	DEBUG_INFO("Initial GPIO OK\r\n");
}

int LoadSysConfigAndInfo(struct SysConfigData *ptr)
{
	int fd,wrd;
	unsigned char *buf;
	unsigned int ChkSum,ChkSumOrg;

	if((buf=malloc(MtdBlockSize))==NULL)
	{

		DEBUG_ERROR("malloc buffer NG,rebooting..\r\n");

		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
		}
		sleep(5);
		system("reboot -f");
		sleep(5);
		system("reboot -f");
	}
	memset(buf, 0, MtdBlockSize);

	//================================================
	// Load configuration from mtdblock10
	//================================================
	fd = open("/dev/mtdblock10", O_RDWR);
	if (fd < 0)
	{
		free(buf);

		DEBUG_ERROR("open mtdblock10 NG,rebooting..\r\n");

		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
		}
		sleep(5);
		system("reboot -f");
		sleep(5);
		system("reboot -f");
	}
    wrd=read(fd, buf, MtdBlockSize);
	close(fd);
	if(wrd<MtdBlockSize)
	{
		free(buf);

		DEBUG_ERROR("read SysConfigData data NG,rebooting..\r\n");

		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
		}
		sleep(5);
		system("reboot -f");
		sleep(5);
		system("reboot -f");
	}
	ChkSum=0;
	for(wrd=0;wrd<MtdBlockSize-4;wrd++)
	{
		ChkSum+=buf[wrd];
	}
	memcpy(&ChkSumOrg,buf+(0x00600000-4),sizeof(ChkSumOrg));

	//================================================
	// Load configuration from mtdblock11
	//================================================
	if(ChkSum!=ChkSumOrg)
	{

		DEBUG_ERROR("Primary SysConfigData checksum NG, read backup\r\n");

		fd = open("/dev/mtdblock11", O_RDWR);
		if (fd < 0)
		{
			free(buf);

			DEBUG_ERROR("open mtdblock11 (backup) NG,rebooting..\r\n");

			if(ShmStatusCodeData!=NULL)
			{
				ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
			}
			sleep(5);
			system("reboot -f");
			sleep(5);
			system("reboot -f");
	    }
	    memset(buf, 0, MtdBlockSize);
   		wrd=read(fd, buf,MtdBlockSize);
    	close(fd);
		if(wrd<MtdBlockSize)
		{
			free(buf);

			DEBUG_ERROR("read backup SysConfigData data NG,rebooting..\r\n");

			if(ShmStatusCodeData!=NULL)
			{
				ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
			}
			sleep(5);
			system("reboot -f");
			sleep(5);
			system("reboot -f");
		}
		ChkSum=0;
		for(wrd=0;wrd<MtdBlockSize-4;wrd++)
		{
			ChkSum+=buf[wrd];
		}
		memcpy(&ChkSumOrg,buf+(0x00600000-4),sizeof(ChkSumOrg));

		//================================================
		// Load configuration from mtdblock12 (Factory default)
		//================================================
		if(ChkSum!=ChkSumOrg)
		{

			DEBUG_WARN("backup SysConfigData checksum NG, read Factory default\r\n");

			fd = open("/dev/mtdblock12", O_RDWR);
			if (fd < 0)
			{
				free(buf);

				DEBUG_ERROR("open mtdblock12 (Factory default) NG,rebooting..\r\n");

				if(ShmStatusCodeData!=NULL)
				{
					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
				}
				sleep(5);
				system("reboot -f");
				sleep(5);
				system("reboot -f");
	    	}
	    	memset(buf, 0, MtdBlockSize);
   			wrd=read(fd, buf,MtdBlockSize);
    		close(fd);
			if(wrd<MtdBlockSize)
			{
				free(buf);

				DEBUG_ERROR("read factory default  SysConfigData data NG,rebooting..\r\n");

				if(ShmStatusCodeData!=NULL)
				{
					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
				}
				sleep(5);
				system("reboot -f");
				sleep(5);
				system("reboot -f");
			}
			ChkSum=0;
			for(wrd=0;wrd<MtdBlockSize-4;wrd++)
			{
				ChkSum+=buf[wrd];
			}
			memcpy(&ChkSumOrg,buf+(0x00600000-4),sizeof(ChkSumOrg));
			if(ChkSum!=ChkSumOrg)
			{
				DEBUG_WARN("factory default  SysConfigData checksum NG, restore factory default\r\n");
				free(buf);
				system("cd /root;./Module_FactoryConfig -m");
				system("sync");
				sleep(5);
				system("reboot -f");
				sleep(5);
				system("reboot -f");

				return FAIL;
			}
		}
	}

	//load OK
	memcpy((struct SysConfigData *)ptr,buf,sizeof(struct SysConfigData));
	free(buf);

	DEBUG_INFO("Load SysConfigData OK\r\n");

	return PASS;

}

void InitEthernet()
{
	char tmpbuf[256];
	//unsigned int address;

	DEBUG_INFO("11111111111111111111");
	//Init Eth0 for internet
	if(isInterfaceUp("eth0")==PASS)
	{
		memset(tmpbuf,0,256);
		sprintf(tmpbuf,"/sbin/ifconfig eth0 %s netmask %s up",
		ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress,
		ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthSubmaskAddress);
		system(tmpbuf);
		memset(tmpbuf,0,256);
		sprintf(tmpbuf,"route add default gw %s eth0 ",
		ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthGatewayAddress);
		system(tmpbuf);
	}
	DEBUG_INFO("2222222222222222");
	if(isInterfaceUp("eth1")==PASS)
	{
		//Init Eth1 for administrator tool
		memset(tmpbuf,0,256);
		sprintf(tmpbuf,"/sbin/ifconfig eth1 %s netmask %s up",
		ShmSysConfigAndInfo->SysConfig.Eth1Interface.EthIpAddress,
		ShmSysConfigAndInfo->SysConfig.Eth1Interface.EthSubmaskAddress);
		system(tmpbuf);
	}
	DEBUG_INFO("3333333333333333333333");

    //Run DHCP client if enabled
	system("killall udhcpc");
	system("rm -rf /etc/resolv.conf");
	system("echo nameserver 8.8.8.8 > /etc/resolv.conf");		//Google DNS server
	system("echo nameserver 180.76.76.76 > /etc/resolv.conf");	//Baidu DNS server

	if(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthDhcpClient==0)
		system("/sbin/udhcpc -i eth0 -s /root/simple.script > /dev/null &");


	DEBUG_INFO("Initial Ethernet OK\r\n");

}

int SpawnTask()
{
	system ("pkill Module_4g");
	system ("pkill Module_Wifi");
	system ("pkill Module_EventLogging");
	system ("pkill OcppBackend");
	system ("pkill Module_AlarmDetect");
	system ("pkill Module_InternalComm");

	if(ShmSysConfigAndInfo->SysConfig.ModelName[10] == 'T')
	{
		system("/root/Module_4g &");
	}
	else if(ShmSysConfigAndInfo->SysConfig.ModelName[10] == 'W')
	{
		system("/root/Module_Wifi &");
	}

	system("/root/Module_EventLogging &");
	system("/root/OcppBackend &");
	system("/root/Module_AlarmDetect &");
	system("/root/Module_InternalComm &");

	return PASS;
}

int StoreUsrConfigData(struct SysConfigData *UsrData)
{
	int result = PASS;
	int fd,wrd;
	unsigned int i,Chk;
	unsigned char *ptr, *BufTmp;

	Chk=0;
	ptr=(unsigned char *)UsrData;
	if((BufTmp=malloc(MtdBlockSize))!=NULL)
	{
		memset(BufTmp,0,MtdBlockSize);
		memcpy(BufTmp,ptr,sizeof(struct SysConfigData));
		for(i=0;i<MtdBlockSize-4;i++)
			Chk+=*(BufTmp+i);
		memcpy(	BufTmp+MtdBlockSize-4,&Chk,4);
		fd = open("/dev/mtdblock10", O_RDWR);
		if (fd>0)
		{
			wrd=write(fd, BufTmp, MtdBlockSize);
			close(fd);
			if(wrd>=MtdBlockSize)
			{
				fd = open("/dev/mtdblock11", O_RDWR);
				if (fd>0)
				{
					wrd=write(fd, BufTmp, MtdBlockSize);
    					close(fd);
				   	if(wrd<MtdBlockSize)
					{
						DEBUG_ERROR("write /dev/mtdblock11(backup) NG\r\n");
				   		result = FAIL;
					}
				}
				else
				{
					DEBUG_ERROR("open /dev/mtdblock11(backup) NG\r\n");
					result = FAIL;
				}
			}
			else
			{
		    		DEBUG_ERROR("write /dev/mtdblock10 NG\r\n");
		    		result = FAIL;
			}
		}
		else
		{
			DEBUG_ERROR("open /dev/mtdblock10 NG\r\n");
			result = FAIL;
		}
	}
	else
	{
		DEBUG_ERROR("alloc BlockSize NG\r\n");
    		result = FAIL;
	}
	if(BufTmp!=NULL)
		free(BufTmp);

	return result;
}

int Initialization()
{
	LoadSysConfigAndInfo(&ShmSysConfigAndInfo->SysConfig);

	InitGPIO();

	InitEthernet();

	rfidFd = InitRfidPort();

	DEBUG_INFO("Initialization OK\r\n");

	return PASS;
}

//=====================================================
// Common routine
//=====================================================
char* getSystemModeName(unsigned char mode)
{
	char* result;

	switch(mode)
	{
		case MODE_BOOTING:
			result = "booting";
			break;
		case MODE_IDLE:
			result = "idle";
			break;
		case MODE_AUTHORIZING:
			result = "authorizing";
			break;
		case MODE_PREPARING:
			result = "preparing";
			break;
		case MODE_CHARGING:
			result = "charging";
			break;
		case MODE_TERMINATING:
			result = "terminating";
			break;
		case MODE_ALARM:
			result = "alarm";
			break;
		case MODE_FAULT:
			result = "fault";
			break;
		case MODE_MAINTAIN:
			result = "maintain";
			break;
		case MODE_RESERVATION:
			result = "reservation";
			break;
		case MODE_BOOKING:
			result = "booking";
			break;
	}

	return result;
}

void setChargerMode(unsigned char gun_index, unsigned char mode)
{
	// For OCPP
	ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PreviousSystemStatus = ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].SystemStatus;
	ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].SystemStatus = mode;

	DEBUG_INFO("Gun-%02d mode switch from %s to %s\r\n", gun_index, getSystemModeName(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PreviousSystemStatus), getSystemModeName(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].SystemStatus));

	// FOR CSU
	ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus = ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus;
	ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus = mode;

	DEBUG_INFO("Gun-%02d internal system mode switch from %s to %s\r\n", gun_index, getSystemModeName(ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus), getSystemModeName(ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus));
}

unsigned char isMode(unsigned char gun_index, unsigned char mode)
{
	//return ((ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].SystemStatus == mode)?YES:NO);
	return ((ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus == mode)?YES:NO);
}

unsigned char isModeChange(unsigned char gun_index)
{
	unsigned char result = NO;

	if(!isMode(gun_index, ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus))
	{
		result = YES;

		// For OCPP
		ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PreviousSystemStatus = ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].SystemStatus;

		// FOR CSU
		ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus = ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus;
	}

	return result;
}

//===============================================
// Get firmware version
//===============================================
void get_firmware_version(unsigned char gun_index)
{
	FILE *fp;
	char cmd[512];
	char buf[512];

	// Get CSU hardware version
	sprintf((char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev, "REV.XXXXXXX");

	// Get CSU boot loader version
	memcpy(ShmSysConfigAndInfo->SysInfo.CsuBootLoadFwRev, ShmSysConfigAndInfo->SysConfig.CsuBootLoadFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.CsuBootLoadFwRev));

	// Get CSU kernel version
	sprintf(cmd, "/bin/uname -r");
	fp = popen(cmd, "r");
	if(fp == NULL)
		sprintf((char*)ShmSysConfigAndInfo->SysInfo.CsuKernelFwRev, "Unknown version");
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			strcpy((char*)ShmSysConfigAndInfo->SysInfo.CsuKernelFwRev, buf);
		}
	}

	// Get CSU root file system version
	sprintf((char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev, "D0.01.40.0007.PH");

	// Get MCU firmware version
	strcpy((char*)ShmSysConfigAndInfo->SysInfo.CsuPrimFwRev, ShmCharger->gun_info[gun_index].ver.Version_FW);

	DEBUG_INFO("CSU hardware version: %s\r\n", ShmSysConfigAndInfo->SysInfo.CsuHwRev);
	DEBUG_INFO("CSU boot loader version: %s\r\n", ShmSysConfigAndInfo->SysInfo.CsuBootLoadFwRev);
	DEBUG_INFO("CSU kernel version: %s\r\n", ShmSysConfigAndInfo->SysInfo.CsuKernelFwRev);
	DEBUG_INFO("CSU root file system version: %s\r\n", ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev);
	DEBUG_INFO("CSU MCU-%2d firmware version: %s\r\n", gun_index, ShmCharger->gun_info[gun_index].ver.Version_FW);
}


//===============================================
// Upgrade firmware
//===============================================
int upgrade_check()
{
	int result = PASS;
	int fd;
	DIR *dir;
	struct dirent *file;
	char cmd[512];
	long int MaxLen=48*1024*1024;

	if ((dir = opendir ("/mnt")) != NULL)
	{
		/* print all the files and directories within directory */
		while ((file = readdir (dir)) != NULL)
		{
			if(strlen(file->d_name)>2)
			{
				memset(&ShmCharger->fwUpgradeInfo, 0xFF, sizeof(Fw_Upgrade_Info));
				DEBUG_INFO("New firmware file: %s\n", file->d_name);
				sprintf(ShmCharger->fwUpgradeInfo.location, "/mnt/%s", file->d_name);

				if((fd=open(ShmCharger->fwUpgradeInfo.location, O_RDONLY)) >= 0)
				{
					unsigned char *ptr = malloc(MaxLen); //-48 is take out the header
					memset(ptr, 0xFF, MaxLen);  //-48 is take out the header
					read(fd, ptr, MaxLen);
					close(fd);

					ShmCharger->fwUpgradeInfo.fwType = ((ptr[0x13]<<0) | (ptr[0x12]<<8) | (ptr[0x11]<<16) | (ptr[0x10]<<24));
					substr(ShmCharger->fwUpgradeInfo.modelName, (char *)ptr, 0, 0x10);
					DEBUG_INFO("New firmware type: %X\r\n", ShmCharger->fwUpgradeInfo.fwType);
					DEBUG_INFO("New firmware model name: %s, %s\r\n", ShmCharger->fwUpgradeInfo.modelName, ShmSysConfigAndInfo->SysConfig.ModelName);

					if((strcmp(ShmCharger->fwUpgradeInfo.modelName, (char*)ShmSysConfigAndInfo->SysConfig.ModelName)==0) && (ShmCharger->fwUpgradeInfo.fwType>0))
					{
						switch(ShmCharger->fwUpgradeInfo.fwType)
						{
							case CSU_BOOTLOADER:
							case CSU_KERNEL_CONFIGURATION:
							case CSU_KERNEL_IMAGE:
							case CSU_ROOT_FILE_SYSTEM:
							case CSU_USER_CONFIGURATION:
							case CSU_PRIMARY_CONTROLLER:
								Upgrade_Flash(ShmCharger->fwUpgradeInfo.fwType, ShmCharger->fwUpgradeInfo.location, ShmCharger->fwUpgradeInfo.modelName);

								sprintf(cmd, "yes|rm %s", ShmCharger->fwUpgradeInfo.location);
								system(cmd);
								break;
							case AC_WALLMOUNT_CONTROLLER:
								for(int gun_index = 0;gun_index<AC_QUANTITY;gun_index++)
									ShmCharger->gun_info[gun_index].mcuFlag.isMcuUpgradeReq = ON;
								break;
							default:
								DEBUG_WARN("Image file is unknown type.\r\n");
								sprintf(cmd, "yes|rm %s", ShmCharger->fwUpgradeInfo.location);
								system(cmd);
								break;
						}
					}

					free(ptr);
				}
				else
				{
					// File open error
					DEBUG_ERROR("New firmware open error.\r\n");
				}
			}
		}
		closedir (dir);
	}
	else
	{
	  DEBUG_ERROR("/mnt does not valid.\r\n");
	}

	return result;
}

//===============================================
// Check RFID is match with start user
//===============================================
int isMatchStartUser(unsigned char gun_index)
{
	uint8_t tmpUser[32];

	if(ShmSysConfigAndInfo->SysConfig.RfidCardNumEndian)
	{
		// Big endian
		switch(rfid.snType)
		{
			case RFID_SN_TYPE_6BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5]);
				break;
			case RFID_SN_TYPE_7BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5], rfid.currentCard[6]);
				break;
			case RFID_SN_TYPE_10BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5], rfid.currentCard[6], rfid.currentCard[7], rfid.currentCard[8], rfid.currentCard[9]);
				break;
			case RFID_SN_TYPE_4BYTE:
			default:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3]);
				break;
		}
	}
	else
	{
		// Little endian
		switch(rfid.snType)
		{
			case RFID_SN_TYPE_6BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X", rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
				break;
			case RFID_SN_TYPE_7BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[6], rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
				break;
			case RFID_SN_TYPE_10BYTE:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[9], rfid.currentCard[8], rfid.currentCard[7], rfid.currentCard[6], rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
				break;
			case RFID_SN_TYPE_4BYTE:
			default:
				sprintf((char*)tmpUser, "%02X%02X%02X%02X", rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
				break;
		}
	}

	return ((strcmp((char*)tmpUser, (char*)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId)==0)?YES:NO);
}

//===============================================
//	Read RFID Serial Number
//===============================================
int GetCardSerialNumber()
{
	int isSuccess = FAIL;
	int module_type = MODULE_EWT;

	if(getRequestCardSN(rfidFd,module_type,&rfid))
	{
		if(rfid.cardType == ISO14443A)
		{
			if(rfid.snType == RFID_SN_TYPE_4BYTE)
			{
				isSuccess = PASS;
			}
			else if (rfid.snType == RFID_SN_TYPE_7BYTE)
			{
				isSuccess = PASS;
			}
		}
		else if(rfid.cardType == IS014443B)
		{
			isSuccess = PASS;
		}
		else if(rfid.cardType == FELICA)
		{
			isSuccess = PASS;
		}
		else
		{}
	}
	else
	{}

	return isSuccess;
}

//===============================================
// Alarm value check
//===============================================
int GetAlarmValue()
{
	int result = PASS;

	if( (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[0] > 0) || (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[1] > 0) || (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[2] > 0) ||
		(ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[3] > 0) || (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[4] > 0) || (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[5] > 0) ||
		(ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[6] > 0) || (ShmStatusCodeData->AlarmCode.AlarmEvents.AlarmVal[7] > 0))
	{
		result = FAIL;
	}

	return result;
}

//===============================================
// Info value check
//===============================================
int GetInfoValue()
{
	int result = PASS;

	if( (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[0] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[1] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[2] > 0) ||
		(ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[3] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[4] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[5] > 0) ||
		(ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[6] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[7] > 0) || (ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[8] > 0) ||
		(ShmStatusCodeData->InfoCode.InfoEvents.InfoVal[9] > 0))
	{
		result = FAIL;
	}

	return result;
}

//===============================================
// Fault value check
//===============================================
int GetFaultValue()
{
	int result = PASS;

	if( (ShmStatusCodeData->FaultCode.FaultEvents.FaultVal[0] > 0) ||
		(ShmStatusCodeData->FaultCode.FaultEvents.FaultVal[1] > 0) ||
		(ShmStatusCodeData->FaultCode.FaultEvents.FaultVal[2] > 0) ||
		(ShmStatusCodeData->FaultCode.FaultEvents.FaultVal[3] > 0))
	{
		result = FAIL;
	}

	return result;
}

//===============================================
// Authorizing Response Status
//===============================================
int isAuthorizedPass()
{
	int result = FAIL;

	char tmp[16] = "";
	memset(tmp, 0, ARRAY_SIZE(tmp));

	for(int index = 0; index < strlen((char *)ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status);index++)
	{
		sprintf(tmp + (index - 1) * 2, "%02X",ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status[index]);
	}
	sprintf(tmp, "%s",	ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status);

	DEBUG_INFO("Authorize Response Id TagInfo: %s\r\n", ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status);

	if(strstr(tmp, "Accepted") != 0)
	{
		result = PASS;
	}
	else
	{}

	return result;
}

//===============================================
// BootNotification Response Status
//===============================================
int isBootNotificationSignature()
{
	int result = FAIL;

	char tmp[16] = "";
	memset(tmp, 0, ARRAY_SIZE(tmp));

	for(int index = 0; index < strlen((char *)ShmOCPP16Data->BootNotification.ResponseStatus);index++)
	{
		sprintf(tmp + (index - 1) * 2, "%02X",ShmOCPP16Data->BootNotification.ResponseStatus[index]);
	}
	sprintf(tmp, "%s",ShmOCPP16Data->BootNotification.ResponseStatus);

	DEBUG_INFO("BootNotification Response Status: %s\r\n", ShmOCPP16Data->BootNotification.ResponseStatus);
	if(strstr(tmp, "Accepted") != 0)
	{
		result = PASS;
	}
	else
	{}

	return result;
}

//===============================================
// Set led motion
//===============================================
void setLedMotion(unsigned char gun_index,unsigned char led_mode)
{
	switch(led_mode)
	{
		case LED_ACTION_INIT:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_INIT;
			break;
		case LED_ACTION_IDLE:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_IDLE;
			break;
		case LED_ACTION_AUTHED:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_AUTHED;
			break;
		case LED_ACTION_CONNECTED:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_CONNECTED;
			break;
		case LED_ACTION_CHARGING:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_CHARGING;
			break;
		case LED_ACTION_STOP:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_STOP;
			break;
		case LED_ACTION_ALARM:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_ALARM;
			break;
		case LED_ACTION_MAINTAIN:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_MAINTAIN;
			break;
		case LED_ACTION_RFID_PASS:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_RFID_PASS;
			break;
		case LED_ACTION_RFID_FAIL:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_RFID_FAIL;
			break;
		case LED_ACTION_BLE_CONNECT:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_BLE_CONNECT;
			break;
		case LED_ACTION_BLE_DISABLE:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_BLE_DISABLE;
			break;
		case LED_ACTION_DEBUG:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_DEBUG;
			break;
		case LED_ACTION_ALL_OFF:
			ShmCharger->gun_info[gun_index].primaryMcuLed.mode = LED_ACTION_ALL_OFF;
			break;
	}
}

//===============================================
// Relay on/off request
//===============================================
void setRelay(unsigned char gun_index,unsigned char isOn)
{
	if(isOn == ON)
	{
		if(ShmCharger->gun_info[gun_index].legacyRequest.isLegacyRequest == OFF)
		{
			ShmCharger->gun_info[gun_index].legacyRequest.isLegacyRequest = ON;
			DEBUG_INFO("Gun-%d Output relay sts: ON \r\n",gun_index);
		}
	}
	else
	{
		if(ShmCharger->gun_info[gun_index].legacyRequest.isLegacyRequest == ON)
		{
			ShmCharger->gun_info[gun_index].legacyRequest.isLegacyRequest = OFF;
			DEBUG_INFO("Gun-%d Output relay sts: OFF \r\n",gun_index);
		}
	}
}

//===============================================
// Set speaker on/off request
//===============================================
void setSpeaker(unsigned char isOn, unsigned char speaker_mode)
{
	if(isOn == ON)
	{
		ShmCharger->isSpeakerOn = ON;
		ShmCharger->speaker_type = speaker_mode;
	}
}

//===============================================
// Initialization RFID communication port
//===============================================
int InitRfidPort()
{
	int uartO2 = open(rfidPortName, O_RDWR);
		struct termios tios;

	if (uartO2 != FAIL)
	{
		ioctl (uartO2, TCGETS, &tios);
		tios.c_cflag = B19200 | CS8 | CLOCAL | CREAD;
		tios.c_lflag = 0;
		tios.c_iflag = 0;
		tios.c_oflag = 0;
		tios.c_cc[VMIN] = 0;
		tios.c_cc[VTIME] = (unsigned char) 1;
		tios.c_lflag = 0;
		tcflush(uartO2, TCIFLUSH);
		ioctl(uartO2, TCSETS, &tios);
	}

	if (uartO2 < 0)
	{
		ShmStatusCodeData->AlarmCode.AlarmEvents.bits.RfidModuleCommFail = 1;
	}

	return uartO2;
}




//===============================================
// Transfer RFID load data to string
//===============================================
//void cardNumberEndian(unsigned char gun_index, unsigned endian)
//{
//	int len = rfid.snType;
//	char tmp[32] = "";
//
//	if(endian == 0)
//	{
//		memcpy(tmp, rfid.currentCard, len);
//		memset(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber));
//		memset(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId));
//		for (int index = 0; index < len; index++)
//		{
//			sprintf((char *)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber + (index * 2), "%02X", tmp[index]);
//			sprintf((char *)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId + (index * 2), "%02X", tmp[index]);
//		}
//	}
//	else
//	{
//		memcpy(tmp, rfid.currentCard, len);
//		memset(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber));
//		memset(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId));
//		for(int i = 0,j = (rfid.snType-1); i < rfid.snType; i++,j--)
//		{
//			sprintf((char *)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber + (i * 2), "%02X", tmp[j]);
//			sprintf((char *)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId + (i * 2), "%02X", tmp[j]);
//		}
//	}
//
//	DEBUG_INFO("Card Number : %s\r\n", ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber);
//	DEBUG_INFO("Start User Id : %s\r\n", ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId);
//}



//===============================================
// Main process
//===============================================
int main(void)
{
	//Create all share memory
	if(CreatShareMemory()==0)
	{
		DEBUG_ERROR("CreatShareMemory NG\r\n");

		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory=1;
		}
		sleep(5);
		system("reboot -f");
		sleep(5);
		system("reboot -f");
	}
	else
	{
		DEBUG_INFO("CreatShareMemory OK\r\n");
	}

	for(int gun_index = 0;gun_index<AC_QUANTITY;gun_index++)
	{
		ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PreviousSystemStatus = 0xff;
		ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus = 0xff;
	}

	// Main loop
	for(;;)
	{
		//==========================================
		// Something need run in Idle mode
		//==========================================
		if((ShmSysConfigAndInfo->SysInfo.AcChargingData[0].SystemStatus==MODE_IDLE) &&
		   (AC_QUANTITY>1?(ShmSysConfigAndInfo->SysInfo.AcChargingData[1].SystemStatus==MODE_IDLE):true))
		{
			//======================================
			// Check restore factory setting request
			//======================================
			if(ShmSysConfigAndInfo->SysInfo.FactoryConfiguration)
			{
				system("cd /root;./Module_FactoryConfig -m");
				system("sync");
				sleep(5);
				system("reboot -f");
				sleep(5);
				system("reboot -f");
			}

			//======================================
			// Check upgrade firmware request
			//======================================
			if(ShmSysConfigAndInfo->SysInfo.FirmwareUpdate)
			{
				for(int gun_index = 0;gun_index<AC_QUANTITY;gun_index++)
					setChargerMode(gun_index, MODE_MAINTAIN);
			}
		}

		//==========================================
		// Check RFID
		//==========================================
		if(!ShmCharger->rfidReq)
		{
			if(GetCardSerialNumber()!= FAIL)
			{
				ShmCharger->rfidReq = ON;
			}
		}

		//==========================================
		// Connector loop
		//==========================================
		for(int gun_index = 0;gun_index<AC_QUANTITY;gun_index++)
		{
			/*
			 *	TODO:
			 *	1. How to identify each connector alarm code?...
			 *	2. Check alarm have occur
			 */
			if((ShmCharger->gun_info[gun_index].primaryMcuAlarm.InputAlarmCode>0))
			{
				if(ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus != MODE_ALARM)
				{
					/*
					 *	TODO:
					 *	1. CP switch to E state
					 */
					setChargerMode(gun_index, MODE_ALARM);
				}
			}
			else
			{
				if(ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus == MODE_ALARM)
				{
					/*
					 *	TODO:
					 *	1. CP switch to normal state
					 */
				}
				else if(ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus == 0XFF)
				{
					setChargerMode(gun_index, MODE_BOOTING);
				}
			}

			switch(ShmCharger->gun_info[gun_index].internalSystemStatus.SystemStatus)
			{
				case MODE_BOOTING:
					if(isModeChange(gun_index))
					{
						//CSU Initialization & task spawn
						if((Initialization() != PASS) ||
						   (SpawnTask() != PASS) )
						{
							ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed = ON;
						}
					}

					if(ShmCharger->gun_info[gun_index].mcuFlag.isReadFwVerPass &&
					   ShmCharger->gun_info[gun_index].mcuFlag.isSetModelNamePass &&
					   ShmCharger->gun_info[gun_index].mcuFlag.isSetSerialNumberPass)
					{
						/*
						 * TODO:
						 * 1. If control pilot state != STATE_A, cancel cold load pick up request
						 * 2. Debug mode into request check
						 */
						// Firmware version
						get_firmware_version(gun_index);

						// OCPP BootNotification info set
						memcpy((char*)ShmOCPP16Data->OcppServerURL, (char*)ShmSysConfigAndInfo->SysConfig.OcppServerURL, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.OcppServerURL));
						memcpy((char*)ShmOCPP16Data->ChargeBoxId, (char*)ShmSysConfigAndInfo->SysConfig.ChargeBoxId, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.ChargeBoxId));
						sprintf((char*)ShmSysConfigAndInfo->SysConfig.chargePointVendor, "Phihong Technology");
						sprintf((char*)ShmOCPP16Data->BootNotification.CpFwVersion, (char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev);
						sprintf((char*)ShmOCPP16Data->BootNotification.CpMeterSerialNumber, "N/A");
						sprintf((char*)ShmOCPP16Data->BootNotification.CpMeterType, "AC");

						DEBUG_INFO("Wifi mode: %d\r\n",  ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode);
						DEBUG_INFO("Wifi SSID: %s\r\n",  ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid);
						DEBUG_INFO("Wifi password: %s\r\n",  ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword);

						setChargerMode(gun_index, MODE_IDLE);
					}

					setLedMotion(gun_index,LED_ACTION_INIT);

					break;
				case MODE_IDLE:
					if(isModeChange(gun_index))
					{
						setRelay(gun_index,OFF);
						ShmCharger->rfidReq = OFF;
						ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStart = OFF;
						ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStop = OFF;
						ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = OFF;
						ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = OFF;
						ShmCharger->gun_info[gun_index].systemAlarmCode.SystemAlarmCode = 0x00;
						ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent = ShmCharger->gun_info[gun_index].primaryMcuState.rating_current;
						ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargedDuration = 0;
						ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargedEnergy = 0;
					}

					if(((ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PilotState == CP_STATE_B) && (ShmCharger->rfidReq == ON)) ||
					   (ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStart == ON) ||
					   (ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq == ON))
					{
						setSpeaker(ON,SPEAKER_SHORT);

						// User id
						memset(ShmSysConfigAndInfo->SysConfig.UserId, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.UserId));

						// Card Number
						memset(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber));

						if((ShmCharger->rfidReq == ON))
						{
							ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod = START_METHOD_RFID;
							DEBUG_INFO("StartMethod : START_METHOD_RFID...\r\n");

							if(ShmSysConfigAndInfo->SysConfig.RfidCardNumEndian)
							{
								// Big endian
								switch(rfid.snType)
								{
									case RFID_SN_TYPE_6BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5]);
										break;
									case RFID_SN_TYPE_7BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5], rfid.currentCard[6]);
										break;
									case RFID_SN_TYPE_10BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3], rfid.currentCard[4], rfid.currentCard[5], rfid.currentCard[6], rfid.currentCard[7], rfid.currentCard[8], rfid.currentCard[9]);
										break;
									case RFID_SN_TYPE_4BYTE:
									default:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X", rfid.currentCard[0], rfid.currentCard[1], rfid.currentCard[2], rfid.currentCard[3]);
										break;
								}
							}
							else
							{
								// Little endian
								switch(rfid.snType)
								{
									case RFID_SN_TYPE_6BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X", rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
										break;
									case RFID_SN_TYPE_7BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[6], rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
										break;
									case RFID_SN_TYPE_10BYTE:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", rfid.currentCard[9], rfid.currentCard[8], rfid.currentCard[7], rfid.currentCard[6], rfid.currentCard[5], rfid.currentCard[4], rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
										break;
									case RFID_SN_TYPE_4BYTE:
									default:
										sprintf((char*)ShmSysConfigAndInfo->SysConfig.UserId, "%02X%02X%02X%02X", rfid.currentCard[3], rfid.currentCard[2], rfid.currentCard[1], rfid.currentCard[0]);
										break;
								}
							}

							memcpy((char*)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber, ShmSysConfigAndInfo->SysConfig.UserId, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.UserId));

							DEBUG_INFO("User Id : %s\r\n", ShmSysConfigAndInfo->SysConfig.UserId);
							DEBUG_INFO("Card number : %s\r\n", ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].CardNumber);
						}
						else if(ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq)
						{
							ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod = START_METHOD_BACKEND;
							memcpy(ShmSysConfigAndInfo->SysConfig.UserId, ShmOCPP16Data->RemoteStartTransaction[gun_index].IdTag, ARRAY_SIZE(ShmOCPP16Data->RemoteStartTransaction[gun_index].IdTag));
							DEBUG_INFO("StartMethod : START_METHOD_BACKEND...\r\n");
						}
						else if(ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStart == ON)
						{
							ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod = START_METHOD_BLE;
							memcpy(ShmSysConfigAndInfo->SysConfig.UserId, ShmCharger->gun_info[gun_index].bleLoginCentralId.id, ARRAY_SIZE(ShmCharger->gun_info[gun_index].bleLoginCentralId.id));
							DEBUG_INFO("StartMethod : START_METHOD_BLE...\r\n");
						}
						else
						{
							ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod = START_METHOD_MANUAL;
							DEBUG_INFO("StartMethod : START_METHOD_MANUAL..\r\n");
						}

						ShmCharger->rfidReq = OFF;
						ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = OFF;
						ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStart = OFF;

						setChargerMode(gun_index, MODE_AUTHORIZING);
					}
					else
					{}

					setLedMotion(gun_index,LED_ACTION_IDLE);

					break;
				case MODE_AUTHORIZING:
					if(isModeChange(gun_index))
					{
						ftime(&startTime[TMR_IDX_AUTH]);
						/*
						 * TODO: Check on-line/off-line to determine authorization policy
						 */
						if((ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod == START_METHOD_BACKEND) ||
						   (ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod == START_METHOD_RFID))
						{
							switch(ShmSysConfigAndInfo->SysConfig.AuthorisationMode)
							{
								case AUTH_MODE_PH_RFID:
									break;
								case AUTH_MODE_OCPP:
									if(ShmOCPP16Data->OcppConnStatus)
									{
										// On line
										ShmOCPP16Data->SpMsg.bits.AuthorizeReq = ON;
									}
									else
									{
										// Off line
										switch(ShmSysConfigAndInfo->SysConfig.OfflinePolicy)
										{
											case OFF_POLICY_LOCALLIST:
												ShmOCPP16Data->SpMsg.bits.AuthorizeReq = ON;
												break;
											case OFF_POLICY_PH_RFID:
												break;
											case OFF_POLICY_FREEMODE:
												break;
											case OFF_POLICY_NOCHARGE:
											default:
												break;
										}
									}
									break;
								case AUTH_MODE_PH_BACKEND:
									break;
								case AUTH_MODE_FREEMODE:
								default:
									break;
							}
						}

						setLedMotion(gun_index,LED_ACTION_AUTHED);
					}

					if(DiffTimeb(startTime[TMR_IDX_AUTH]) > TIMEOUT_SPEC_AUTH)
					{
						/*
						 * 	TODO:
						 * 	1. Authorization timeout process.
						 */
						ShmCharger->rfidReq = OFF;
						setChargerMode(gun_index, MODE_IDLE);
					}
					else
					{
						switch(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartMethod)
						{
							case START_METHOD_BACKEND:
								setChargerMode(gun_index, MODE_PREPARING);
								break;
							case START_METHOD_RFID:
								if(ShmOCPP16Data->SpMsg.bits.AuthorizeConf ||
								  (ShmSysConfigAndInfo->SysConfig.AuthorisationMode == AUTH_MODE_FREEMODE) ||
								  !ShmOCPP16Data->OcppConnStatus)
								{
									if((strcmp((char*)ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status, "Accepted")==0) ||
									   (ShmSysConfigAndInfo->SysConfig.AuthorisationMode == AUTH_MODE_FREEMODE) ||
									   (!ShmOCPP16Data->OcppConnStatus&&(ShmSysConfigAndInfo->SysConfig.OfflinePolicy == OFF_POLICY_FREEMODE)))
									{
										setChargerMode(gun_index, MODE_PREPARING);
									}
									else
									{
										setChargerMode(gun_index, MODE_IDLE);
									}

									ShmCharger->rfidReq = OFF;
									ShmOCPP16Data->SpMsg.bits.AuthorizeConf = OFF;
								}
								break;
							case START_METHOD_BLE:
							case START_METHOD_MANUAL:
							default:
								setChargerMode(gun_index, MODE_PREPARING);
								break;
						}
					}

					setLedMotion(gun_index,LED_ACTION_AUTHED);

					break;
				case MODE_PREPARING:
					if(isModeChange(gun_index))
					{
						/*
						 *	TODO:
						 * 	1. Request MCU start charging
						 * 	2. Enable handshaking watch dog in 180 seconds
						 */
						ftime(&startTime[TMR_IDX_HANDSHAKING]);
						setRelay(gun_index,ON);
					}

					if(DiffTimeb(startTime[TMR_IDX_HANDSHAKING]) > TIMEOUT_SPEC_HANDSHAKING)
					{
						/*
						 * 	TODO:
						 * 	1. HANDSHAKING timeout process
						 */
						setChargerMode(gun_index, MODE_IDLE);
					}
					else if((ShmCharger->gun_info[gun_index].primaryMcuState.relay_state == ON))
					{
						ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].OutputEnergy = 0;
						ShmOCPP16Data->StartTransaction[gun_index].MeterStart = ((float)ShmCharger->gun_info[gun_index].powerConsumption.power_consumption/100);
						memcpy((char*)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].StartUserId, ShmSysConfigAndInfo->SysConfig.UserId, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.UserId));

						ShmOCPP16Data->CpMsg.bits[gun_index].StartTransactionReq = ON;
						setChargerMode(gun_index, MODE_CHARGING);
					}

					if((ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PilotState == CP_STATE_B))
					{
						setLedMotion(gun_index,LED_ACTION_CONNECTED);
					}

					break;
				case MODE_CHARGING:
					if(isModeChange(gun_index))
					{
						ShmCharger->rfidReq = OFF;
						ftime(&startChargingTime[gun_index]);
					}

					if((ShmCharger->rfidReq == ON) ||
					   (ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStop == ON) ||
					   (ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq == ON)||
					   (ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PilotState != CP_STATE_C))
					{
						setLedMotion(gun_index,LED_ACTION_STOP);
						setChargerMode(gun_index, MODE_TERMINATING);
					}
					else
					{
						/*
						 * 	TODO:
						 * 	1. Update charging cycle information.
						 * 	2. CP PWM duty dynamic control logic.
						 */

						setRelay(gun_index,ON);
						setLedMotion(gun_index,LED_ACTION_CHARGING);
						ShmOCPP16Data->StopTransaction[gun_index].MeterStop = ((float)ShmCharger->gun_info[gun_index].powerConsumption.power_consumption/100.0) - ShmOCPP16Data->StartTransaction[gun_index].MeterStart;
						ftime(&endChargingTime[gun_index]);
						ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargedDuration = DiffTimeb2(startChargingTime[gun_index], endChargingTime[gun_index]);
					}

					break;
				case MODE_TERMINATING:
					if(isModeChange(gun_index))
					{
						if(ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargedDuration != 0)
						{
							ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargedDuration = DiffTimeb2(startChargingTime[gun_index], endChargingTime[gun_index]);
						}
					}

					if(((ShmCharger->rfidReq == ON) && isMatchStartUser(gun_index)) ||
					   (ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStop == ON) ||
					   (ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq == ON) ||
					   (ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PilotState == CP_STATE_A))
					{
						/*
						 * 	TODO:
						 * 	1. Record charging history.
						 * 	2. OCPP send stop transaction request.
						 */
						setSpeaker(ON,SPEAKER_SHORT);
						ShmSysConfigAndInfo->SysConfig.Bluetooth.isRequestStop = OFF;
						ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = OFF;
						ShmOCPP16Data->CpMsg.bits[gun_index].StopTransactionReq = ON;

						setRelay(gun_index,OFF);
						ShmCharger->rfidReq = OFF;
						setChargerMode(gun_index, MODE_IDLE);
					}
					else
					{
						 if((ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PilotState == CP_STATE_C))
						 {
							 setChargerMode(gun_index, MODE_CHARGING);
						 }
					}

					setLedMotion(gun_index,LED_ACTION_STOP);
					break;
				case MODE_ALARM:
					if((ShmCharger->gun_info[gun_index].systemAlarmCode.SystemAlarmCode == 0))
					{
						if((ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus == MODE_CHARGING) ||
						   (ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus == MODE_TERMINATING))
						{
							setChargerMode(gun_index, ShmCharger->gun_info[gun_index].internalSystemStatus.PreviousSystemStatus);
						}
						else
						{
							setChargerMode(gun_index, MODE_IDLE);
						}
					}

					setLedMotion(gun_index,LED_ACTION_ALARM);
					break;
				case MODE_FAULT:
					if(isModeChange(gun_index))
					{
						/*
						 * TODO:
						 * 1. LED control depend on relay status
						 * 2. Alarm latch & release logic
						 */
					}

					break;
				case MODE_MAINTAIN:
					if(isModeChange(gun_index))
					{
						/*
						 * TODO:
						 * 1. LED control depend on relay status
						 * 2. Wait maintenance release command
						 */
					}

					//======================================
					// Check upgrade firmware request
					//======================================
					if(ShmSysConfigAndInfo->SysInfo.FirmwareUpdate)
					{
						if(upgrade_check())
						{
							ShmSysConfigAndInfo->SysInfo.FirmwareUpdate = OFF;
						}
					}

					if(!ShmCharger->gun_info[gun_index].mcuFlag.isMcuUpgradeReq && ((AC_QUANTITY>1)?!ShmCharger->gun_info[gun_index^1].mcuFlag.isMcuUpgradeReq:YES))
					{
						DEBUG_INFO("Firmware upgraded, reboot...\r\n");
						system("reboot -f");
						sleep(5);
						system("reboot -f");
					}

					setLedMotion(gun_index,LED_ACTION_MAINTAIN);
					break;
				case MODE_RESERVATION:
					if(isModeChange(gun_index))
					{
						/*
						 * TODO:
						 * 1. LED control depend on relay status
						 * 2. check reservation is overtime?
						 */
					}

					break;
				case MODE_BOOKING:
					if(isModeChange(gun_index))
					{
						/*
						 * TODO:
						 * 1. LED control depend on relay status
						 * 2. Check booking start is on time?
						 */
					}
					break;
				case MODE_DEBUG:
					if(isModeChange(gun_index))
					{
						/*
						 * TODO:
						 * 1. LED control depend on relay status
						 * 2. Check booking start is on time?
						 */
					}

					setLedMotion(gun_index,LED_ACTION_DEBUG);
					break;
			}
		}

		usleep(50000);
	}

	return FAIL;
}