#include 	"Module_PsuComm.h"

#define Debug
#define ARRAY_SIZE(A)		(sizeof(A) / sizeof(A[0]))
#define PASS				1
#define FAIL				-1
#define YES					1
#define NO					0

#define SELF_TEST			0

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;
struct PsuData 					*ShmPsuData;

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);

bool libInitialize = false;
byte gun_count = CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY;
byte getAvailableCapOffset = 5;

float carReqVol = 0;
float carReqCur = 0;
float evseOutVol = 0;
float evseOutCur = 0;

int cmdDelayTime = 30000;

int StoreLogMsg(const char *fmt, ...);
#define DEBUG_INFO(format, args...) StoreLogMsg("[%s:%d][%s][Info] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_WARN(format, args...) StoreLogMsg("[%s:%d][%s][Warn] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_ERROR(format, args...) StoreLogMsg("[%s:%d][%s][Error] "format, __FILE__, __LINE__, __FUNCTION__, ##args)

unsigned long GetTimeoutValue(struct timeval _sour_time);

unsigned long GetTimeoutValue(struct timeval _sour_time)
{
	struct timeval _end_time;
	gettimeofday(&_end_time, NULL);

	return 1000000 * (_end_time.tv_sec - _sour_time.tv_sec) + _end_time.tv_usec - _sour_time.tv_usec;
}

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);
	system(Buf);

	return rc;
}

int DiffTimeb(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;
}

//=================================
// Common routine
//=================================
char* getTimeString(void)
{
	char *result=malloc(21);
	time_t timep;
	struct tm *p;
	time(&timep);
	p=gmtime(&timep);

	sprintf(result, "[%04d-%02d-%02d %02d:%02d:%02d]", (1900+p->tm_year), (1+p->tm_mon), p->tm_mday, p->tm_hour, p->tm_hour, p->tm_sec);

	return result;
}

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);
	}
}

//=================================
// Save data to share memory Function
//=================================
bool FindChargingInfoData(byte target, struct ChargingInfoData **chargingData)
{
	for (byte index = 0; index < CHAdeMO_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index];
			return true;
		}
	}

	for (byte index = 0; index < CCS_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.CcsChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.CcsChargingData[index];
			return true;
		}
	}

	for (byte index = 0; index < GB_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.GbChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.GbChargingData[index];
			return true;
		}
	}

	return false;
}

void GetPsuRequestCallback(byte phy_id, char *serial_number)
{
	if (ShmSysConfigAndInfo->SysInfo.AcContactorStatus == NO)
		return;

	//printf("++++++++++++++++++++phy_id = %d \n", phy_id);
	// ********************** 每次送電後，需判斷要把所有的模塊分配到哪個 Group **********************
	byte group = 0;
	if(ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING || gun_count == 1)
	{
		// 初始化狀態，則直接先分配到同個群
		group = 0;
	}
	else
	{
		group = ShmSysConfigAndInfo->SysInfo.CurGunSelected;
	}

	bool isNewPsu = true;
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].PhysicalID == phy_id &&
			strncmp((char *)ShmPsuData->PsuGroup[group].PsuModule[index].SerialNumber, serial_number, 7) == 0)
		{
			isNewPsu = false;
		}
	}

	if (isNewPsu)
	{
		ShmPsuData->SystemPresentPsuQuantity++;
		printf("get psu********Membar = %d, group = %d \n", ShmPsuData->SystemPresentPsuQuantity, group);

		if (ShmPsuData->Work_Step >= _TEST_LINE_STEP && ShmPsuData->Work_Step <= _TEST_COMPLETE)
		{
			// 已經進入火線上的驗證動作
			ShmPsuData->NeedBackTest = YES;
		}
		else if (ShmPsuData->Work_Step == _WORK_CHARGING)
		{
			// 一旦進入火線，分配一個不會用到的給該模塊
			group++;
		}

		ShmPsuData->PsuGroup[group].PsuModule[ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity].Address = ShmPsuData->SystemPresentPsuQuantity;
		ShmPsuData->PsuGroup[group].PsuModule[ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity].PhysicalID = phy_id;
		ShmPsuData->PsuGroup[group].PsuModule[ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity].AssignID = (group >> 6) + ShmPsuData->SystemPresentPsuQuantity;
		strcpy((char *)ShmPsuData->PsuGroup[group].PsuModule[ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity].SerialNumber, serial_number);
		ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity++;

		//PsuAddressAssignment(phy_id, serial_number, ShmPsuData->SystemPresentPsuQuantity, group);
		PsuAddressAssignment(phy_id, group);

		if (ShmPsuData->Work_Step != _WORK_CHARGING)
		{
			ShmPsuData->GroupCount = group + 1;
		}
	}
}

void SaveStatusCallback(byte group, byte address, char cri_temp1, int alarm)
{
	//EVSE
	//for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
    //{
    	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
    	{
    		if (ShmPsuData->PsuGroup[group].PsuModule[index].PhysicalID == address)
    		{
    		    ShmPsuData->PsuGroup[group].PsuModule[index].CriticalTemp1 = cri_temp1;
    			//ShmPsuData->PsuGroup[group].PsuModule[index].INFYPWR_Alarm.AlarmCode = alarm;
    			break;
    		}
    	}
    	//printf ("psu alarm = %08x \n", alarm);
    //}
}

//模組三向輸入電壓
void SavePresentInputVoltageCallback(byte address, unsigned short vol1, unsigned short vol2, unsigned short vol3)
{
    bool isSearch = false;
	//EVSE
	//search group
    for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
    { 
    	for (byte index = 0; index < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; index++)
    	{
    	    //search id
    		if (ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PhysicalID == address)
    		{
    		    //update module msg
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].InputVoltageL1 = vol1;
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].InputVoltageL2 = vol2;
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].InputVoltageL3 = vol3;
    			isSearch = true;
    			break;
    		}
    	}
    	if(isSearch) break;
    }
}

// 模塊輸出的電壓電流
void SavePresentOutputCallback(byte address, unsigned short out_vol, unsigned short out_cur)
{
	unsigned short outputVol = 0;
	unsigned short outputCur = 0;
	unsigned short group = 0;
	bool isChange = false;

	// PSU
	for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
    {
    	for (int index = 0; index < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; index++)
    	{
    		if (ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PhysicalID == address)
    		{
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PresentOutputVoltage = out_vol;
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PresentOutputCurrent = out_cur;   			
    			
    		    for (int loop = 0; loop < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; loop++)
    		    {
    		        //update voltage
   		            if (ShmPsuData->PsuGroup[groupIndex].PsuModule[loop].PresentOutputVoltage > outputVol)
   		            {
    			        outputVol = ShmPsuData->PsuGroup[groupIndex].PsuModule[loop].PresentOutputVoltage;
    			    }
    			    //update total current
    			    outputCur += ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PresentOutputCurrent;
    			    group = groupIndex;
    			    isChange = true;
    		    }
    		}

            /*
    		if (ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputVoltage > outputVol)
    			outputVol = ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputVoltage;

    		outputCur += ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputCurrent;
    		*/
    	}
    }

	if (isChange)
	{
		// PSU Group
		// 電壓
		ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage = outputVol;
		// 電流
		ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent = outputCur;

		//EVSE - 槍端的輸出電壓
		chargingInfo[group]->PresentChargingVoltage = ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage;
		//EVSE - 槍端的輸出電流
		chargingInfo[group]->PresentChargingCurrent = ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent;
	}

	//printf("GroupPresentOutputVoltage = %d \n", ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage);
	//printf("GroupPresentOutputCurrent = %d \n", ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent);
}

//PSU able_power = KW (單位 0.1) exp. 300 = 30kw
//PSU able_cur = A (單位 0.1) exp. 400 = 40A
void SaveAvailableCapCallback(byte address, unsigned short maxv,unsigned short minv, unsigned short able_cur, unsigned short able_power)
{
	unsigned int power = 0;
	unsigned int current = 0;
	unsigned int group = 0;
	bool isChange = false;
	//bool sameGroup = false;

    //search group
    for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
    {        
    	for (int index = 0; index < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; index++)
    	{
    	    //search group-id
    		if (ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PhysicalID == address)
    		{
    		    //先更新該模組資訊    
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].AvailablePower = able_power;
    			ShmPsuData->PsuGroup[groupIndex].PsuModule[index].AvailableCurrent = able_cur;
    				    
    		    //該對應的模組群重新計算總合		    
    		    for (int loop = 0; loop < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; loop++)
    		    {
    		        power += ShmPsuData->PsuGroup[groupIndex].PsuModule[loop].AvailablePower;
    			    current += ShmPsuData->PsuGroup[groupIndex].PsuModule[loop].AvailableCurrent;
    			    group = groupIndex;
    			    isChange = true;
    		    }
    		    
    		    /*
    			if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == YES)
    			{
    				for (int index = 0; index < ShmPsuData->SystemPresentPsuQuantity; index++)
    				{
    					if (recordPsuData[index]._phy_addr == ShmPsuData->PsuGroup[group].PsuModule[index].PhysicalID)
    					{
    						if (recordPsuData[index]._fire_index == group)
    						{
    							sameGroup = true;
    						}
    						break;
    					}
    				}
    			}
    			else
    				sameGroup = true;

    			if(sameGroup)
    			{
    				ShmPsuData->PsuGroup[group].PsuModule[index].AvailablePower = able_power;
    				ShmPsuData->PsuGroup[group].PsuModule[index].AvailableCurrent =	able_cur;
    				isChange = true;
    			}
    			*/
    		}
            /*
    		if(sameGroup)
    		{
    			power += ShmPsuData->PsuGroup[group].PsuModule[index].AvailablePower;
    			current += ShmPsuData->PsuGroup[group].PsuModule[index].AvailableCurrent;
    		}
    		*/
    	}
    }
    
	if (isChange)
	{
		// PSU Group
		// Available Power
		ShmPsuData->PsuGroup[group].GroupAvailablePower = power;
		// Available Current
		ShmPsuData->PsuGroup[group].GroupAvailableCurrent = current;

		//EVSE
		if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == NO)
		{
			if (ShmPsuData->PsuGroup[group].GroupAvailablePower < chargingInfo[group]->AvailableChargingPower)
			{
				printf("Power derating old = %f, new  = %d ***************************************************************** \n",
						chargingInfo[group]->AvailableChargingPower,
						ShmPsuData->PsuGroup[group].GroupAvailablePower);
			}
		}

		chargingInfo[group]->MaximumChargingVoltage = maxv;
		chargingInfo[group]->AvailableChargingCurrent =	ShmPsuData->PsuGroup[group].GroupAvailableCurrent;
		chargingInfo[group]->AvailableChargingPower = ShmPsuData->PsuGroup[group].GroupAvailablePower;
	}
	
	//printf("GetAvb:%d  maxV= %d, minV = %d, mixA =%d pwr =%d\n", address, maxv, minv, able_cur, able_power);
}

void SaveHardwareVersion(byte group, byte address, int hw_ver)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion[0] = (hw_ver >> 24) & 0xFF;
			ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion[1] = (hw_ver >> 16) & 0xFF;
			ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion[2] = (hw_ver >> 8) & 0xFF;
			ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion[3] = hw_ver & 0xFF;
			break;
		}
	}
}

void GetOutputPowerSwitchStatusCallback(byte address, unsigned char value)
{
    for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
    {
    	for (byte index = 0; index < ShmPsuData->PsuGroup[groupIndex].GroupPresentPsuQuantity; index++)
    	{
    		if (ShmPsuData->PsuGroup[groupIndex].PsuModule[index].PhysicalID == address)
    		{
    			//printf("PowerSwitch = %d, group = %d, address = %d \n", value, group, address);
    			if(value){
    			    ShmPsuData->PsuGroup[groupIndex].PsuModule[index].OutputPowerSwitch = 0x00;
    			}else{
    			    ShmPsuData->PsuGroup[groupIndex].PsuModule[index].OutputPowerSwitch = 0x01;
    			}
    			
    			break;
    		}
    	}
    }
}

//==========================================
// Init all share memory
//==========================================
int InitShareMemory()
{
	int result = PASS;
	int MeterSMId;

	//creat ShmSysConfigAndInfo
	if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0)
    {
		#ifdef SystemLogMessage
		DEBUG_ERROR("shmget ShmSysConfigAndInfo NG %d \n");
		#endif
		result = FAIL;
	}
    else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
    	#ifdef SystemLogMessage
    	DEBUG_ERROR("shmat ShmSysConfigAndInfo NG \n");
		#endif
    	result = FAIL;
   	 }
    else
    {}

   	//creat ShmStatusCodeData
   	if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData),  0777)) < 0)
    {
		#ifdef SystemLogMessage
   		DEBUG_ERROR("shmget ShmStatusCodeData NG \n");
		#endif
   		result = FAIL;
	}
    else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
    	#ifdef SystemLogMessage
    	DEBUG_ERROR("shmat ShmStatusCodeData NG \n");
		#endif
    	result = FAIL;
   	}
    else
    {}

   	//creat ShmPsuData
	if ((MeterSMId = shmget(ShmPsuKey, sizeof(struct PsuData),  0777)) < 0)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("shmget ShmPsuData NG \n");
		#endif
		result = FAIL;
	}
	else if ((ShmPsuData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("shmat ShmPsuData NG \n");
		#endif
		result = FAIL;
	 }
	memset(ShmPsuData,0,sizeof(struct PsuData));

    return result;
}

//================================================
// Main process
//================================================
void InitialPsuData()
{
	ShmPsuData->SystemPresentPsuQuantity = 0;

	for (byte _groupCount = 0; _groupCount < ARRAY_SIZE(ShmPsuData->PsuGroup);	_groupCount++)
	{
		ShmPsuData->PsuGroup[_groupCount].GroupPresentPsuQuantity = 0;
		ShmPsuData->PsuGroup[_groupCount].GroupAvailablePower = 0;
		ShmPsuData->PsuGroup[_groupCount].GroupAvailableCurrent = 0;
	}
	
	ShmPsuData->Work_Step = 0xff;
}

void Initialization()
{
	bool isPass = false;
	while(!isPass)
	{
		isPass = true;
		for (byte _index = 0; _index < _gunCount; _index++)
		{
			if (!FindChargingInfoData(_index, &chargingInfo[0]))
			{
				DEBUG_ERROR("EvComm (main) : FindChargingInfoData false \n");
				isPass = false;
				break;
			}
		}
	}
}

int main(void)
{
	printf("Psu Task boot .... \n");
	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("InitShareMemory NG\n");
		#endif
		if(ShmStatusCodeData != NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = 1;
		}
		sleep(5);
		return 0;
	}

	printf("InitShareMemory OK\n");

	// register callback function
	GetPsuAddressReq(&GetPsuRequestCallback);
	RefreshStatus(&SaveStatusCallback);
	RefreshInputVol(&SavePresentInputVoltageCallback);
	RefreshGetOutput(&SavePresentOutputCallback);
	RefreshAvailableCap(&SaveAvailableCapCallback);
	RefreshOutputPowerSwitch(&GetOutputPowerSwitchStatusCallback);

	// initial object
	InitialPsuData();
	Initialization();
	libInitialize = InitialCommunication();

	//main loop
	while (libInitialize)
	{
		// 斷電狀態
		if (ShmSysConfigAndInfo->SysInfo.AcContactorStatus == NO)
		{
		    //一但 AC Off PSU 斷電全部的 PSU Group ID 會全部清 0 
			InitialPsuData();
			sleep(1);
			ShmPsuData->Work_Step = ASSIGN_START;
			continue;
		}

		// update psu fw req
//		if(psu update req ?)
//		{
//
//			continue;
//		}

		// 自檢失敗
		if (ShmPsuData->Work_Step == _NO_WORKING)
		{
			DEBUG_ERROR("== PSU == self test fail. \n");
			printf("== PSU == self test fail. \n");
			sleep(5);
		}

		switch(ShmPsuData->Work_Step)
		{
			case ASSIGN_START:
			{
				printf("== PSU == ASSIGN_COMP \n");
				gettimeofday(&_id_assign_time, NULL);
				ShmPsuData->Work_Step = ASSIGN_COMP;
			}
				break;
			case ASSIGN_COMP:
			{
				// 等待十秒 (英飛源開機須等待約10讓模組互相通訊
				if (GetTimeoutValue(_id_assign_time) >= 10000000)
				{
					//發送取得目前全部模組數量
					RequestModuleTotalMumbert();
					usleep(cmdDelayTime);
					if (ShmPsuData->SystemPresentPsuQuantity)
					{
						printf("== PSU == ENABLE_POW \n");
						ShmPsuData->Work_Step = ENABLE_POW;
					}
				}
			}
				break;
			case ENABLE_POW:
			{
				if (ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING)
				{
					// 電樁在 Booting 的狀態 - 自檢
					printf("== PSU == _TEST_LINE_STEP \n");
					ShmPsuData->Work_Step = _TEST_LINE_STEP;
				}
				else
				{
					printf("== PSU == _WORK_CHARGING \n");
					ShmPsuData->Work_Step = _WORK_CHARGING;
					gettimeofday(&_workModePriority_time, NULL);
				}
			}
				break;
			case _TEST_LINE_STEP:
			{
				printf("cur total psu count = %d \n", ShmPsuData->SystemPresentPsuQuantity);
				if (ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity <= 0)
				{
					sleep(1);
					continue;
				}

#if (SELF_TEST)
				// 對整個 Group 保持通訊
				bool isFind = false;
				while(ShmPsuData->Work_Step != _NO_WORKING &&
						_curCheckPsuIndexForFireLine < ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity)
				{
					GetStatus(SET_GROUP_CMD, 0);
					usleep(cmdDelayTime);
					GetAvailableCap(SET_GROUP_CMD, 0, 0);
					usleep(cmdDelayTime);

                    //如果在開機自我測試中又加入新的模組就全部重做自我測試
					if (ShmPsuData->NeedBackTest == YES)
					{
						ShmPsuData->NeedBackTest = NO;
						_curCheckPsuIndexForFireLine = 0x00;
					}

					if (isFind)
					{
						GetPresentOutput(SET_MODULE_CMD, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID);
						usleep(cmdDelayTime);
						//printf("isFind ****** \n");
						//printf("stop vor = %d \n", ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage);
						if (ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage <= 200)
						{
							// 檢查下一個
							_curCheckPsuIndexForFireLine++;
							isFind = false;
						}
						//英飛源無法設定 v = 0
						//程序放在此
						//1.前一個模組會被關閉
						//2.++後的新的模組也會先被先關閉 1 次
						SetPresentOutput(SET_MODULE_CMD, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID,
								ZERO_VOL, ZERO_CUR, chargingInfo[0]->AvailableChargingCurrent);
						usleep(cmdDelayTime);
						EnableOutputPower(SET_MODULE_CMD ,ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID , SWITCH_OFF);
						usleep(cmdDelayTime);
						EnableGreenLedFlash(SET_MODULE_CMD ,ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID , SWITCH_OFF);
						usleep(cmdDelayTime);
					}
					else
					{
						printf("AvailableCurrent[%d] = %d \n",_curCheckPsuIndexForFireLine, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].AvailableCurrent);
						if (ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].AvailableCurrent > 0)
						{
							if (ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage == 0)
							{
								//printf("set output vol = %d, cur = %d \n", SELF_TEST_VOL, SELF_TEST_CUR);
								SetPresentOutput(SET_MODULE_CMD, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID,
									SELF_TEST_VOL, SELF_TEST_CUR, chargingInfo[0]->AvailableChargingCurrent);
								usleep(cmdDelayTime);
								EnableOutputPower(SET_MODULE_CMD ,ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID , SWITCH_ON);
								usleep(cmdDelayTime);
								EnableGreenLedFlash(SET_MODULE_CMD ,ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID , SWITCH_ON);
								usleep(cmdDelayTime);
							}

							if(!isCheckOutputTimeStart)
							{
								gettimeofday(&_chk_output_time, NULL);
								isCheckOutputTimeStart = true;
							}
							else
							{
								// 如果五秒內火線上都沒有偵測到電壓，則代表異常
								if (GetTimeoutValue(_chk_output_time) >= 20000000)
								{
									// 自檢失敗
									printf("self test timeout \n");
									EnableOutputPower(SET_GROUP_CMD, 0, SWITCH_OFF);
									usleep(cmdDelayTime);
									EnableGreenLedFlash(SET_MODULE_CMD, 0, SWITCH_OFF);
									usleep(cmdDelayTime);
									ShmPsuData->Work_Step = _NO_WORKING;
									continue;
								}
							}

							for (byte gunIndex = 0; gunIndex < _gunCount; gunIndex ++)
							{
								GetPresentOutput(SET_MODULE_CMD, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID);
								usleep(cmdDelayTime);
								printf("Cur psu[%d] output voltage = %d \n",_curCheckPsuIndexForFireLine, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage);
								printf("Fire[%d] voltage = %f \n", gunIndex, chargingInfo[gunIndex]->FuseChargingVoltage);
								// 該模組的輸出電壓與火線上的電壓一致
								if (chargingInfo[gunIndex]->FuseChargingVoltage >= 1500 &&
									((chargingInfo[gunIndex]->FuseChargingVoltage >= ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage - 300) &&
									(chargingInfo[gunIndex]->FuseChargingVoltage <= ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage + 300)))
								{
								    printf("Cur psu output voltage2 = %d \n", ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage);
									// 找到火線上的電壓了，這邊紀錄火線是紀錄屬於哪一把槍的火線
									if (_curCheckPsuIndexForFireLine < ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity)
									{
										// 紀錄當前 PSU 是哪個火線上的
										recordPsuData[_curCheckPsuIndexForFireLine]._fire_index = gunIndex;
										recordPsuData[_curCheckPsuIndexForFireLine]._phy_addr = ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PhysicalID;
										strcpy(recordPsuData[_curCheckPsuIndexForFireLine]._serial_num, (char *)ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].SerialNumber);
										printf("Find Fire Line Number end~~~~~~~~~~~~~~~ = %d \n", gunIndex);
										usleep(300000);
										isCheckOutputTimeStart = false;
										isFind = true;
										break;
									}
								}
							}
						}
					}

					usleep(100000);
				}

				if (ShmPsuData->Work_Step != _NO_WORKING)
				{
					printf("== PSU == TEST_POWER_STEP \n");
					ShmPsuData->Work_Step = _TEST_POWER_STEP;
				}

				EnableOutputPower(SET_GROUP_CMD, 0, SWITCH_OFF);
				usleep(cmdDelayTime);
				EnableGreenLedFlash(SET_MODULE_CMD, 0, SWITCH_OFF);
				usleep(cmdDelayTime);
#else
				ShmPsuData->Work_Step = _TEST_POWER_STEP;
#endif
			}
				break;
			case _TEST_POWER_STEP:
			{
				if(!_chkTotalCapStart)
				{
					_chkTotalCapStart = true;
					gettimeofday(&_chk_cap_time, NULL);
				}

				for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
				{
					GetStatus(SET_GROUP_CMD, groupIndex);
					usleep(cmdDelayTime);
					GetAvailableCap(SET_GROUP_CMD, groupIndex,  0);
					usleep(cmdDelayTime);
				}

				if (GetTimeoutValue(_chk_cap_time) >= 2000000)
				{
					printf("AvailableChargingCurrent = %f, AvailableChargingPower = %f \n",
							chargingInfo[0]->AvailableChargingCurrent, chargingInfo[0]->AvailableChargingPower);
					for (byte index = 0; index < ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity; index++)
					{
						printf("index = %d, fire index = %d, phy addr = %d \n",
								index, recordPsuData[index]._fire_index, recordPsuData[index]._phy_addr);
					}

					printf("== PSU == TEST_COMPLETE \n");
					ShmPsuData->Work_Step = _TEST_COMPLETE;
				}
			}
				break;
			case _TEST_COMPLETE:
			{
				sleep(1);
			}
				break;
			case _WORK_CHARGING:
			{
				int time = GetTimeoutValue(_workModePriority_time) / 1000;
				//printf("GroupCount = %d \n", ShmPsuData->GroupCount);
				//printf("cur total psu count = %d \n", ShmPsuData->SystemPresentPsuQuantity);

				for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
				{
					if (time > 1000)
					{
						GetStatus(SET_GROUP_CMD, groupIndex);
						usleep(cmdDelayTime);
						GetAvailableCap(SET_GROUP_CMD, groupIndex,  0);
						usleep(cmdDelayTime);
						gettimeofday(&_workModePriority_time, NULL);
					}

					GetPresentOutput(SET_GROUP_CMD, groupIndex);

					if (carReqVol != chargingInfo[groupIndex]->EvBatterytargetVoltage)
					{
						carReqVol = chargingInfo[groupIndex]->EvBatterytargetVoltage;
						DEBUG_INFO("ev need vol = %f \n", chargingInfo[groupIndex]->EvBatterytargetVoltage);
					}

					if (carReqCur != chargingInfo[groupIndex]->EvBatterytargetCurrent)
					{
						carReqCur = chargingInfo[groupIndex]->EvBatterytargetCurrent;
						DEBUG_INFO("ev need cur = %f \n", chargingInfo[groupIndex]->EvBatterytargetCurrent);
					}

					if (evseOutVol != chargingInfo[groupIndex]->FireChargingVoltage)
					{
						evseOutVol = chargingInfo[groupIndex]->FireChargingVoltage;
						printf("evse output vol = %f \n",	chargingInfo[groupIndex]->FireChargingVoltage);
					}

					if (evseOutCur != chargingInfo[groupIndex]->PresentChargingCurrent)
					{
						evseOutCur = chargingInfo[groupIndex]->PresentChargingCurrent;
						printf("evse output cur = %f \n", chargingInfo[groupIndex]->PresentChargingCurrent);
					}

					// 針對各槍當前狀態，傳送需要回傳的資料指令
					if (((chargingInfo[groupIndex]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[groupIndex]->SystemStatus <= S_CHARGING) && chargingInfo[groupIndex]->RelayK1K2Status) ||
						(chargingInfo[groupIndex]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[groupIndex]->SystemStatus <= S_CCS_PRECHARGE_ST1))
					{
						if (ShmPsuData->PsuGroup[groupIndex].GroupAvailableCurrent > 0)
						{
							// 該充電槍的目標電壓與目標電流
							SetPresentOutput(SET_GROUP_CMD, groupIndex,
								chargingInfo[groupIndex]->EvBatterytargetVoltage,
								chargingInfo[groupIndex]->EvBatterytargetCurrent,
								chargingInfo[groupIndex]->AvailableChargingCurrent);
							usleep(cmdDelayTime);
						}

						if (chargingInfo[groupIndex]->EvBatterytargetVoltage == 0)
						{
							EnableOutputPower(SET_GROUP_CMD, groupIndex, SWITCH_OFF);
							usleep(cmdDelayTime);
							EnableGreenLedFlash(SET_GROUP_CMD , groupIndex , SWITCH_OFF);
							usleep(cmdDelayTime);
						}
						else
						{
							EnableOutputPower(SET_GROUP_CMD, groupIndex, SWITCH_ON);
							usleep(cmdDelayTime);
							EnableGreenLedFlash(SET_GROUP_CMD , groupIndex, SWITCH_ON);
							usleep(cmdDelayTime);
						}
					}
					else if (chargingInfo[groupIndex]->SystemStatus >= S_TERMINATING &&
							chargingInfo[groupIndex]->SystemStatus <= S_COMPLETE)
					{
						SetPresentOutput(SET_GROUP_CMD, groupIndex, ZERO_VOL, ZERO_CUR, chargingInfo[groupIndex]->AvailableChargingCurrent);
						usleep(cmdDelayTime);
						EnableGreenLedFlash(SET_GROUP_CMD , groupIndex , SWITCH_OFF);
						usleep(cmdDelayTime);
						EnableOutputPower(SET_GROUP_CMD, groupIndex, SWITCH_OFF);
						usleep(cmdDelayTime);
					}
				}
				break;
			}
		}
		usleep(45000);
	}
	return FAIL;
}