#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

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

	// ********************** 單槍的狀態，分配同一個 **********************
	byte group = 0;
	if(ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING || gun_count == 1)
	{
		// 初始化狀態，則直接先分配到同個群
		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********id = %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);

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

void SaveStatusCallback(byte group, byte address, int alarm, int fault)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].AlarmCode = alarm;
			ShmPsuData->PsuGroup[group].PsuModule[index].FaultCode = fault;
			break;
		}
	}
}

void SaveAlarmNotifyCallback(byte group, byte address, byte *alarm)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			printf("PSU Alarm = %s \n", alarm);
			//ShmPsuData->PsuGroup[group].PsuModule[index].AlarmCode = alarm;
			break;
		}
	}
}

void SaveFaultNotifyCallback(byte group, byte address, byte *fault)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			printf("PSU Fault = %s \n", fault);
			//ShmPsuData->PsuGroup[group].PsuModule[index].FaultCode = fault;
			break;
		}
	}
}

void SaveFirmwareVersion(byte group, byte address, unsigned char packageIndex, unsigned char type , unsigned char *data)
{
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			if (packageIndex == 0 || packageIndex == 1)
				strncpy((char *)ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion + (packageIndex * 6), (char *)data, 6);
			else
				strncpy((char *)ShmPsuData->PsuGroup[group].PsuModule[index].FwVersion + (packageIndex * 6), (char *)data, 4);
		}
	}

	if (packageIndex == 1)
	{
		char string[3];

		memcpy(string, (data + 2), 2);
		string[2] = '\0';

//		if (maxChargingVol != 0)
//		{
//			chargingInfo[group]->MaximumChargingVoltage = maxChargingVol;
//		}
//		else
		{
			if (strcmp(string, "50") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 5000;
			else if (strcmp(string, "70") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 7000;
			else if (strcmp(string, "75") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 7500;
			else if (strcmp(string, "80") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 8000;
			else if (strcmp(string, "95") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 9500;
			else if (strcmp(string, "A0") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 10000;
			else if (strcmp(string, "C0") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 12000;
			else if (strcmp(string, "F0") == 0)
				chargingInfo[group]->MaximumChargingVoltage = 15000;
		}

		//printf("index = %d, max vol = %f \n", group, chargingInfo[group]->MaximumChargingVoltage);
	}
}

void SaveFanSpeedCallback(byte group, byte address, byte fan1, byte fan2, byte fan3, byte fan4)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].FanSpeed_1 = fan1;
			ShmPsuData->PsuGroup[group].PsuModule[index].FanSpeed_2 = fan2;
			ShmPsuData->PsuGroup[group].PsuModule[index].FanSpeed_3 = fan3;
			ShmPsuData->PsuGroup[group].PsuModule[index].FanSpeed_4 = fan4;
			break;
		}
	}
}

void SaveTemperatureCallback(byte group, byte address, char cri_temp1, char cri_temp2, char cri_temp3, char ex_temp, char in_temp1, char in_temp2, char out_temp)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].CriticalTemp1 = cri_temp1;
			ShmPsuData->PsuGroup[group].PsuModule[index].CriticalTemp2 = cri_temp2;
			ShmPsuData->PsuGroup[group].PsuModule[index].CriticalTemp3 = cri_temp3;
			ShmPsuData->PsuGroup[group].PsuModule[index].ExletTemp = ex_temp;
			ShmPsuData->PsuGroup[group].PsuModule[index].InletTemp_1 = in_temp1;
			ShmPsuData->PsuGroup[group].PsuModule[index].InletTemp_2 = in_temp2;
			ShmPsuData->PsuGroup[group].PsuModule[index].OutletTemp = out_temp;
			break;
		}
	}
}

void SavePresentInputVoltageCallback(byte group, byte address, byte vol_type, unsigned short vol1, unsigned short vol2, unsigned short vol3)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].InputVoltage_Type = vol_type;
			ShmPsuData->PsuGroup[group].PsuModule[index].InputVoltageL1 = vol1;
			ShmPsuData->PsuGroup[group].PsuModule[index].InputVoltageL2 = vol2;
			ShmPsuData->PsuGroup[group].PsuModule[index].InputVoltageL3 = vol3;
			break;
		}
	}
}

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

	// PSU
	for (int index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputVoltage = out_vol;
			ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputCurrent = out_cur;
			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);
}

void SaveAvailableCapCallback(byte group, byte address, unsigned short able_power, unsigned short able_cur)
{
	unsigned int power = 0;
	unsigned int current = 0;
	bool isChange = false;

	//printf("SaveAvailableCapCallback : index = %d, cur = %d \n", address, able_cur);
	// PSU
	for (int index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].AvailablePower = able_power;
			ShmPsuData->PsuGroup[group].PsuModule[index].AvailableCurrent =	able_cur;
			isChange = true;
		}

		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 (ShmPsuData->PsuGroup[group].GroupAvailablePower < chargingInfo[group]->AvailableChargingPower)
		{
			chargingInfo[group]->CurrentDerating = 0x01;
			printf("Power derating old = %f, new  = %d ***************************************************************** \n",
					chargingInfo[group]->AvailableChargingPower,
					ShmPsuData->PsuGroup[group].GroupAvailablePower);
		}
		chargingInfo[group]->AvailableChargingCurrent =	ShmPsuData->PsuGroup[group].GroupAvailableCurrent;
		chargingInfo[group]->AvailableChargingPower = ShmPsuData->PsuGroup[group].GroupAvailablePower;
	}
}

void SavePresentInputCurrentCallback(byte group, byte address, unsigned short in_cur1, unsigned short in_cur2, unsigned short in_cur3)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].InputCurrentL1 = in_cur1;
			ShmPsuData->PsuGroup[group].PsuModule[index].InputCurrentL2 = in_cur2;
			ShmPsuData->PsuGroup[group].PsuModule[index].InputCurrentL3 = in_cur3;
			break;
		}
	}
}

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 SaveStatusNotifyCallback(byte group, byte address, byte st_machine, unsigned short out_vol, unsigned short out_cur)
{
	//EVSE
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			ShmPsuData->PsuGroup[group].PsuModule[index].StateMachine = st_machine;
			//ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputVoltage = out_vol;
			//ShmPsuData->PsuGroup[group].PsuModule[index].PresentOutputCurrent = out_cur;
			//printf("psu state = %d, vol = %d, cur = %d \n", st_machine, out_vol, out_cur);
			break;
		}
	}
}

void GetSerialNumberCallback(byte group, byte address, unsigned char packageIndex, unsigned char *data)
{
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			strcpy((char *)ShmPsuData->PsuGroup[group].PsuModule[index].SerialNumber + (packageIndex * 7), (char *)data);
			break;
		}
	}
}

void GetOutputPowerSwitchStatusCallback(byte group, byte address, unsigned char value)
{
	for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
	{
		if (ShmPsuData->PsuGroup[group].PsuModule[index].Address == address)
		{
			//printf("PowerSwitch = %d, group = %d, address = %d \n", value, group, address);
			ShmPsuData->PsuGroup[group].PsuModule[index].OutputPowerSwitch = value;
			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;
	}
}

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);
	RefreshFanSpeed(&SaveFanSpeedCallback);
	RefreshTemp(&SaveTemperatureCallback);
	RefreshInputVol(&SavePresentInputVoltageCallback);
	RefreshGetOutput(&SavePresentOutputCallback);
	RefreshAvailableCap(&SaveAvailableCapCallback);
	RefreshInputCur(&SavePresentInputCurrentCallback);

	RefreshAlarmNotify(&SaveAlarmNotifyCallback);
	RefreshFaultNotify(&SaveFaultNotifyCallback);
	RefreshStatusNotify(&SaveStatusNotifyCallback);

	RefreshSerialNumber(&GetSerialNumberCallback);
	RefreshOutputPowerSwitch(&GetOutputPowerSwitchStatusCallback);
	RefreshFWVersion(&SaveFirmwareVersion);
	//RefreshHWVersion(&SaveHardwareVersion);

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

	byte priorityLow = 1;
	//main loop
	while (libInitialize)
	{
		// 斷電狀態
		if (ShmSysConfigAndInfo->SysInfo.AcContactorStatus == NO)
		{
			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)
		{
			// 開始分配模塊的 Group & Address
			case ASSIGN_START:
			{
				gettimeofday(&_id_assign_time, NULL);
				ShmPsuData->Work_Step = ASSIGN_COMP;
			}
				break;
			case ASSIGN_COMP:
			{
				if (priorityLow == 1)
				{
					// 一旦被分配好的模塊，將透過以下指令保持通訊，並取得一些資訊
					for (byte psuIndex = 0;	psuIndex < ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity;	psuIndex++)
					{
						if (ShmPsuData->PsuGroup[0].PsuModule[psuIndex].Address == NONE_CARE_ADDRESS)
							continue;

						GetStatus(0, ShmPsuData->PsuGroup[0].PsuModule[psuIndex].Address);
						usleep(50000);
						if (strlen((char *)ShmPsuData->PsuGroup[0].PsuModule[psuIndex].FwVersion) == 0 &&
								ShmPsuData->PsuGroup[0].PsuModule[psuIndex].FwVersion[0] == '\0')
						{
							GetFwVersion(0, ShmPsuData->PsuGroup[0].PsuModule[psuIndex].Address, 0x02);
							usleep(50000);
						}

						if (strlen((char *)ShmPsuData->PsuGroup[0].PsuModule[psuIndex].SerialNumber) == 0 &&
								ShmSysConfigAndInfo->SysInfo.RelayModuleFwRev[0] == '\0')
						{
							GetSerialNumber(0, ShmPsuData->PsuGroup[0].PsuModule[psuIndex].Address);
							usleep(50000);
						}
					}
				}

				priorityLow >= 20 ? priorityLow = 1 : priorityLow++;
				// 最多等待 15 秒 (在這個時間內的模塊都將被分配好並進入下一個狀態)
				if (GetTimeoutValue(_id_assign_time) >= 15000000)
				{
					ShmPsuData->Work_Step = ENABLE_POW;
				}
			}
				break;
			case ENABLE_POW:
			{
				if (ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING)
				{
					// 電樁在 Booting 的狀態 - 自檢
					ShmPsuData->Work_Step = _TEST_LINE_STEP;
				}
				else
				{
					ShmPsuData->Work_Step = _WORK_CHARGING;
					gettimeofday(&_workModePriority_time, NULL);
				}
			}
				break;
			case _TEST_LINE_STEP:
			{
				// 自檢開始 :
				// 1. 取得模塊火線上的位置
				// (假設有兩個模塊對應到兩把槍，線路上一定是一個模塊對應到一把槍，需要找出對應的槍與模塊 Index)
				printf("cur total psu count = %d \n", ShmPsuData->SystemPresentPsuQuantity);
				if (ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity <= 0)
				{
					sleep(1);
					continue;
				}
				// 對整個 Group 保持通訊
				bool isFind = false;
				while(ShmPsuData->Work_Step != _NO_WORKING &&
						_curCheckPsuIndexForFireLine < ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity)
				{
					GetStatus(0, NONE_CARE_ADDRESS);
					usleep(50000);
					GetAvailableCap(0, NONE_CARE_ADDRESS, getAvailableCapOffset);
					usleep(50000);
					EnableOutputPower(0, NONE_CARE_ADDRESS, OUTPUT_POWER_SWITCH_ON);
					usleep(50000);

					if (ShmPsuData->NeedBackTest == YES)
					{
						ShmPsuData->NeedBackTest = NO;
						_curCheckPsuIndexForFireLine = 0x00;
					}

					if (isFind)
					{
						GetPresentOutput(0, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].Address);
						usleep(50000);
						//printf("stop vor = %d \n", ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage);
						if (ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage <= 1000)
						{
							// 檢查下一個
							_curCheckPsuIndexForFireLine++;
							isFind = false;
						}
						SetPresentOutput(0, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].Address,
								ZERO_VOL, ZERO_CUR, chargingInfo[0]->AvailableChargingCurrent);
						usleep(50000);
					}
					else
					{
						printf("AvailableCurrent = %d \n", 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(0, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].Address,
									SELF_TEST_VOL, SELF_TEST_CUR, chargingInfo[0]->AvailableChargingCurrent);
								usleep(50000);
							}

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

							for (byte gunIndex = 0; gunIndex < _gunCount; gunIndex ++)
							{
								GetPresentOutput(0, ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].Address);
								usleep(50000);
								printf("Cur psu output voltage = %d \n", ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage);
								printf("Fire voltage = %f \n", chargingInfo[gunIndex]->FuseChargingVoltage);
								// 該模組的輸出電壓與火線上的電壓一致
								if (chargingInfo[gunIndex]->FuseChargingVoltage >= 1200 &&
									((chargingInfo[gunIndex]->FuseChargingVoltage >= ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage - 300) &&
									(chargingInfo[gunIndex]->FuseChargingVoltage <= ShmPsuData->PsuGroup[0].PsuModule[_curCheckPsuIndexForFireLine].PresentOutputVoltage + 300)))
								{
									// 找到火線上的電壓了，這邊紀錄火線是紀錄屬於哪一把槍的火線
									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(100000);
										isCheckOutputTimeStart = false;
										isFind = true;
										break;
									}
								}
							}
						}
					}

					usleep(100000);
				}

				if (ShmPsuData->Work_Step != _NO_WORKING)
					ShmPsuData->Work_Step = _TEST_POWER_STEP;

				EnableOutputPower(0, NONE_CARE_ADDRESS, OUTPUT_POWER_SWITCH_OFF);
				usleep(50000);
			}
				break;
			case _TEST_POWER_STEP:
			{
				// 2. 取得火線上最大輸出能量
				if(!_chkTotalCapStart)
				{
					_chkTotalCapStart = true;
					gettimeofday(&_chk_cap_time, NULL);
				}

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

				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);
					}
					ShmPsuData->Work_Step = _TEST_COMPLETE;
				}
			}
				break;
			case _TEST_COMPLETE:
			{
				// PSU 自檢結束
				priorityLow = 1;
				sleep(1);
			}
				break;
			case _WORK_CHARGING:
			{
				// 一旦自檢結束後，重新分配模塊結束即會跳到該狀態，等待輸出
				int time = GetTimeoutValue(_workModePriority_time) / 1000;
				//printf("cur total psu count = %d \n", ShmPsuData->SystemPresentPsuQuantity);
				for (byte groupIndex = 0; groupIndex < ShmPsuData->GroupCount; groupIndex++)
				{
					if (time > 5000)
					{
//						GetStatus(groupIndex, NONE_CARE_ADDRESS);
//						usleep(50000);
						GetAvailableCap(groupIndex, NONE_CARE_ADDRESS, getAvailableCapOffset);
						usleep(50000);
						gettimeofday(&_workModePriority_time, NULL);
					}

					GetPresentOutput(groupIndex, NONE_CARE_ADDRESS);
					usleep(50000);
					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)
						{
							EnableOutputPower(groupIndex, NONE_CARE_ADDRESS, OUTPUT_POWER_SWITCH_ON);
							usleep(50000);
							float targetVol = chargingInfo[groupIndex]->EvBatterytargetVoltage;
							float targetCur = chargingInfo[groupIndex]->EvBatterytargetCurrent;

							if (targetVol != 0)
							{
								if (targetCur <= 10)
									targetCur = 10;
							}
							else
							{
								targetVol = 0;
								targetCur = 0;
							}

							//printf("Charging : Set Present Output V = %f, C = %f \n",
							//	chargingInfo[groupIndex]->EvBatterytargetVoltage, targetCur);

							// 該充電槍的目標電壓與目標電流
							SetPresentOutput(groupIndex, NONE_CARE_ADDRESS,
								targetVol,
								targetCur,
								chargingInfo[groupIndex]->AvailableChargingCurrent);
							usleep(50000);
						}
					}
					else if (chargingInfo[groupIndex]->SystemStatus >= S_TERMINATING &&
							chargingInfo[groupIndex]->SystemStatus <= S_COMPLETE)
					{
						SetPresentOutput(groupIndex, NONE_CARE_ADDRESS, ZERO_VOL, ZERO_CUR, chargingInfo[groupIndex]->AvailableChargingCurrent);
						usleep(50000);
//						if (chargingInfo[groupIndex]->RelayK1K2Status == NO)
//						{
//							//printf("DD OFF ---------------------------------------------------\n");
//							// EnableOutputPower(groupIndex, NONE_CARE_ADDRESS, OUTPUT_POWER_SWITCH_OFF);
//							usleep(50000);
//						}
					}
				}
				priorityLow >= 200 ? priorityLow = 1 : priorityLow++;
				break;
			}
		}
		usleep(45000);
	}
	return FAIL;
}