#include 	<sys/time.h>
#include 	<sys/timeb.h>
#include    <sys/types.h>
#include    <sys/stat.h>
#include 	<sys/types.h>
#include 	<sys/ioctl.h>
#include 	<sys/socket.h>
#include 	<sys/ipc.h>
#include 	<sys/shm.h>
#include 	<sys/shm.h>
#include 	<sys/mman.h>
#include 	<linux/wireless.h>
#include 	<arpa/inet.h>
#include 	<netinet/in.h>

#include 	<unistd.h>
#include 	<stdarg.h>
#include    <stdio.h>      /*標準輸入輸出定義*/
#include    <stdlib.h>     /*標準函數庫定義*/
#include    <unistd.h>     /*Unix 標準函數定義*/
#include    <fcntl.h>      /*檔控制定義*/
#include    <termios.h>    /*PPSIX 終端控制定義*/
#include    <errno.h>      /*錯誤號定義*/
#include 	<errno.h>
#include 	<string.h>
#include	<time.h>
#include	<ctype.h>
#include 	<ifaddrs.h>
#include 	<math.h>
#include	"../../define.h"
#include	"PrimaryComm.h"
#include 	<stdbool.h>
#include    "Config.h"
#include    "Common.h"

#define COMM_FAIL_COUNT             10
#define STATE_CHANGE_COUNT          3
#define TILT_SENSOR_WAIT_TIME       500             // unit: 1ms
#define TILT_SENSOR_WAIT_RESPONSE   500             // unit: 1ms
#define TILT_SENSOR_WAIT_STOP       1               // unit: 1s

typedef unsigned char 		byte;

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;
struct PrimaryMcuData			*ShmPrimaryMcuData;
ChargerInfoData                 *ShmChargerInfo;

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 Uart1Fd;
char *priPortName = "/dev/ttyS1";
Ver ver;
Gpio_in gpio_in;
Rtc rtc;

byte _OutputDrv = 0;

byte _acStatus = 0;
byte _acChkCount = 0;
int _CommFailCount = 0;
int _CabinetSwitch = -1;
int _TempSwitch = -1;
int _TempSPD = -1;
int _TempDoor = -1;
int _TempEmg = -1;
byte _SwitchCnt = 0;
byte _SPDCnt = 0;
byte _DoorCnt = 0;
byte _EmgBtnCnt = 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);
	}
}

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

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

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

	//creat ShmStatusCodeData
	if ((MeterSMId = shmget(ShmPrimaryMcuKey, sizeof(struct PrimaryMcuData), 0777)) < 0)
	{
		#ifdef SystemLogMessage
		LOG_ERROR("shmget ShmPrimaryMcuData NG");
		#endif
		result = FAIL;
	}
	else if ((ShmPrimaryMcuData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		#ifdef SystemLogMessage
		LOG_ERROR("shmat ShmPrimaryMcuData NG");
		#endif
		result = FAIL;
	}

    if((MeterSMId = shmget(SM_ChargerInfoKey, sizeof(ChargerInfoData), 0777)) < 0)
    {
        #ifdef SystemLogMessage
        LOG_ERROR("shmat ChargerInfoData NG");
        #endif
        result = FAIL;
    }
    else if((ShmChargerInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        LOG_ERROR("shmat ChargerInfoData NG");
        #endif
        result = FAIL;
    }

    return result;
}

//================================================
// Function
//================================================
void GetFwAndHwVersion()
{
	if(Query_FW_Ver(Uart1Fd, Addr.IoExtend, &ver) == PASS)
	{
		LOG_INFO("s1 = %s", ver.Version_FW);
		strcpy((char *)ShmPrimaryMcuData->version, ver.Version_FW);
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.CsuPrimFwRev, ver.Version_FW);
	}
	else
	{
	    _CommFailCount++;
	}

	if (Query_HW_Ver(Uart1Fd, Addr.IoExtend, &ver) == PASS)
	{
		LOG_INFO("s2 = %s", ver.Version_HW);
	}
}

void GetInputGpioStatus()
{
	//LOG_INFO("GetInputGpioStatus");
	if (Query_Gpio_Input(Uart1Fd, Addr.IoExtend, &gpio_in) == PASS)
	{
		if (_acStatus != gpio_in.AC_Connector)
		{
			if (_acChkCount >= 3)
			{
				_acStatus = gpio_in.AC_Connector;

				// DO360 AC_Connector Status is the inverse of DS's
				if(ShmChargerInfo->Control.PrimaryCtrl.bits.AcContactorReverse)
				{
                    ShmSysConfigAndInfo->SysInfo.AcContactorStatus = gpio_in.AC_Connector ? 0 : 1;
                    ShmPrimaryMcuData->InputDet.bits.AcContactorDetec = gpio_in.AC_Connector ? 0 : 1;
				}
				else
				{
                    ShmSysConfigAndInfo->SysInfo.AcContactorStatus = gpio_in.AC_Connector ? 1 : 0;
                    ShmPrimaryMcuData->InputDet.bits.AcContactorDetec = gpio_in.AC_Connector ? 1 : 0;
				}
				LOG_INFO("Ac Contactor Status %s", ShmPrimaryMcuData->InputDet.bits.AcContactorDetec > 0 ? "On" : "Off");
			}
			else
				_acChkCount++;
		}
		else
			_acChkCount = 0;

		ShmPrimaryMcuData->InputDet.bits.AcMainBreakerDetec = gpio_in.AC_MainBreaker;

		if(_TempSPD != gpio_in.SPD)
		{
		    _SPDCnt++;
		    if(_SPDCnt >= STATE_CHANGE_COUNT || _TempSPD < 0)
		    {
		        _TempSPD = gpio_in.SPD;
		        _SPDCnt = 0;
		    }
		}
		if(_TempSPD >= 0)
		{
		    ShmPrimaryMcuData->InputDet.bits.SpdDetec = _TempSPD;
		}
#if 0
		ShmPrimaryMcuData->InputDet.bits.SpdDetec = gpio_in.SPD;
#endif

		// DO360 Door Status is the inverse of DS's
#if 0
        if(ShmChargerInfo->Control.PrimaryCtrl.bits.DoorSensorReverse)
        {
            ShmPrimaryMcuData->InputDet.bits.DoorOpen = gpio_in.Door_Open ? 0 : 1;
        }
        else
        {
            ShmPrimaryMcuData->InputDet.bits.DoorOpen = gpio_in.Door_Open ? 1 : 0;
        }
#endif
        if(_TempDoor != gpio_in.Door_Open)
        {
            _DoorCnt++;
            if(_DoorCnt >= STATE_CHANGE_COUNT || _TempDoor < 0)
            {
                _TempDoor = gpio_in.Door_Open;
                _DoorCnt = 0;
            }
        }
        if(_TempDoor >= 0)
        {
            if(ShmChargerInfo->Control.PrimaryCtrl.bits.DoorSensorReverse)
            {
                ShmPrimaryMcuData->InputDet.bits.DoorOpen = _TempDoor > 0 ? 0 : 1;
            }
            else
            {
                ShmPrimaryMcuData->InputDet.bits.DoorOpen = _TempDoor > 0 ? 1 : 0;
            }
        }
		// Bypass door open
		//ShmPrimaryMcuData->InputDet.bits.DoorOpen = 0;

		ShmPrimaryMcuData->InputDet.bits.Button1 = gpio_in.Button[0];
		ShmPrimaryMcuData->InputDet.bits.Button2 = gpio_in.Button[1];

		if(_TempEmg != gpio_in.Emergency_Btn)
		{
		    _EmgBtnCnt++;
		    if(_EmgBtnCnt >= STATE_CHANGE_COUNT || _TempEmg < 0)
		    {
		        _TempEmg = gpio_in.Emergency_Btn;
		        _EmgBtnCnt = 0;
		    }
		}
		if(_TempEmg >= 0)
		{
		    ShmPrimaryMcuData->InputDet.bits.EmergencyButton = _TempEmg > 0 ? 1 : 0;
		}
#if 0
		ShmPrimaryMcuData->InputDet.bits.EmergencyButton = gpio_in.Emergency_Btn;
#endif
		//LOG_INFO("left = %d", ShmPrimaryMcuData->InputDet.bits.Button1);
		//LOG_INFO("right = %d", ShmPrimaryMcuData->InputDet.bits.Button2);
		//LOG_INFO("ShmSysConfigAndInfo->SysInfo.AcContactorStatus = %d", ShmSysConfigAndInfo->SysInfo.AcContactorStatus);
		//if (ShmPrimaryMcuData->InputDet.bits.AcMainBreakerDetec == YES)
		//	LOG_INFO("AC Mainbreaker occur.");

		ShmPrimaryMcuData->InputDet.bits.Key0 = gpio_in.Key[0] ? 0 : 1;
		ShmPrimaryMcuData->InputDet.bits.Key1 = gpio_in.Key[1] ? 0 : 1;
		ShmPrimaryMcuData->InputDet.bits.Key2 = gpio_in.Key[2] ? 0 : 1;
		ShmPrimaryMcuData->InputDet.bits.Key3 = gpio_in.Key[3] ? 0 : 1;

		char _SwValue = (ShmPrimaryMcuData->InputDet.bits.Key0 << 0) |
		                (ShmPrimaryMcuData->InputDet.bits.Key1 << 1) |
                        (ShmPrimaryMcuData->InputDet.bits.Key2 << 2) |
                        (ShmPrimaryMcuData->InputDet.bits.Key3 << 3);
		if(_TempSwitch != _SwValue)
		{
		    _SwitchCnt++;
		    if(_SwitchCnt >= STATE_CHANGE_COUNT)
		    {
		        _TempSwitch = _SwValue;
		        _SwitchCnt = 0;
		    }
		}

		if(_CabinetSwitch != _TempSwitch)
		{
            LOG_INFO("Switch: %d, Key3: %d, Key2: %d, Key1: %d, Key0: %d",
                _TempSwitch,
                ShmPrimaryMcuData->InputDet.bits.Key3,
                ShmPrimaryMcuData->InputDet.bits.Key2,
                ShmPrimaryMcuData->InputDet.bits.Key1,
                ShmPrimaryMcuData->InputDet.bits.Key0);
		}
        _CabinetSwitch = _TempSwitch;
        if(_CabinetSwitch >= 0 && ShmChargerInfo->Control.PrimaryCtrl.bits.CabinetSwitchDetect)
        {
            if(ShmChargerInfo->Control.CabinetSwitch != _CabinetSwitch)
            {
                LOG_INFO("Set Cabinet Switch: %d", _CabinetSwitch);
            }
            ShmChargerInfo->Control.CabinetSwitch = _CabinetSwitch;
        }
	}
}

void SetOutputGpio(byte flash)
{
	Gpio_out gpio;

	if(_OutputDrv != flash)
	{
        gpio.Button_LED[0] = (flash & 0x01) > 0 ? 1 : 0;
        gpio.Button_LED[1] = (flash & 0x02) > 0 ? 1 : 0;
        gpio.System_LED[0] = (flash & 0x04) > 0 ? 1 : 0;
        gpio.System_LED[1] = (flash & 0x08) > 0 ? 1 : 0;
        gpio.System_LED[2] = (flash & 0x10) > 0 ? 1 : 0;
        gpio.System_LED[3] = (flash & 0x20) > 0 ? 1 : 0;
        gpio.AC_Connector  = (flash & 0x40) > 0 ? 1 : 0;
        gpio.AC_Breaker    = (flash & 0x80) > 0 ? 1 : 0;

        if (Config_Gpio_Output(Uart1Fd, Addr.IoExtend, &gpio) == PASS)
        {
            _OutputDrv = flash;
        }
	}
	else
	{
	    // do nothing when no change
	}
}

void SetRtcData()
{
	struct timeb csuTime;
	struct tm *tmCSU;

	ftime(&csuTime);
	tmCSU = localtime(&csuTime.time);
//	LOG_INFO("Time : %04d-%02d-%02d %02d:%02d:%02d", tmCSU->tm_year + 1900,
//			tmCSU->tm_mon + 1, tmCSU->tm_mday, tmCSU->tm_hour, tmCSU->tm_min,
//			tmCSU->tm_sec);

	rtc.RtcData[0] = '0' + (tmCSU->tm_year + 1900) / 1000 % 10;
	rtc.RtcData[1] = '0' + (tmCSU->tm_year + 1900) / 100 % 10;
	rtc.RtcData[2] = '0' + (tmCSU->tm_year + 1900) / 10 % 10;
	rtc.RtcData[3] = '0' + (tmCSU->tm_year + 1900) / 1 % 10;

	rtc.RtcData[4] = '0' + (tmCSU->tm_mon + 1) / 10 % 10;
	rtc.RtcData[5] = '0' + (tmCSU->tm_mon + 1) / 1 % 10;

	rtc.RtcData[6] = '0' + (tmCSU->tm_mday) / 10 % 10;
	rtc.RtcData[7] = '0' + (tmCSU->tm_mday) / 1 % 10;

	rtc.RtcData[8] = '0' + (tmCSU->tm_hour) / 10 % 10;
	rtc.RtcData[9] = '0' + (tmCSU->tm_hour) / 1 % 10;

	rtc.RtcData[10] = '0' + (tmCSU->tm_min) / 10 % 10;
	rtc.RtcData[11] = '0' + (tmCSU->tm_min) / 1 % 10;

	rtc.RtcData[12] = '0' + (tmCSU->tm_sec) / 10 % 10;
	rtc.RtcData[13] = '0' + (tmCSU->tm_sec) / 1 % 10;

	if (Config_Rtc_Data(Uart1Fd, Addr.IoExtend, &rtc) == PASS)
	{
		//LOG_INFO("SetRtc sucessfully.");
	}
	else
	{
		//LOG_INFO("SetRtc fail.");
	}
}

//================================================
// Main process
//================================================
int InitComPort()
{
	int fd;
	struct termios tios;

	fd = open(priPortName, O_RDWR);
	if(fd<=0)
	{
		#ifdef SystemLogMessage
		LOG_ERROR("open 407 Communication port NG");
		#endif
		return -1;
	}
	ioctl (fd, TCGETS, &tios);
	tios.c_cflag = B115200| 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(fd, TCIFLUSH);
	ioctl (fd, TCSETS, &tios);

	return fd;
}

int preTiltSensorStep = _TILT_SENSOR_NONE;
struct timespec _tiltSensor_time;

void CheckTiltSensor(void)
{
    if(ShmPrimaryMcuData->InputDet.bits.AcContactorDetec)
    {
        int step = ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep;

        switch(step)
        {
            case _TILT_SENSOR_NONE:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                }
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_WAIT;
                break;
            case _TILT_SENSOR_WAIT:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    GetClockTime(&_tiltSensor_time);
                }
                if((GetTimeoutValue(_tiltSensor_time) / mSEC_VAL) > TILT_SENSOR_WAIT_TIME)
                {
                    ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_STEP_1;
                }
                break;
            case _TILT_SENSOR_STEP_1:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    LOG_INFO("Tilt sensor self test: step 1");
                }
                ShmPrimaryMcuData->OutputDrv.bits.SystemLed4Drv = true;
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_WAIT_STEP_1;
                break;
            case _TILT_SENSOR_WAIT_STEP_1:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    GetClockTime(&_tiltSensor_time);
                }
                if((GetTimeoutValue(_tiltSensor_time) / mSEC_VAL) > TILT_SENSOR_WAIT_RESPONSE)
                {
                    ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_STEP_2;
                }
                break;
            case _TILT_SENSOR_STEP_2:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    LOG_INFO("Tilt sensor self test: step 2");
                }
                ShmPrimaryMcuData->OutputDrv.bits.SystemLed3Drv = true;
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_WAIT_STEP_2;
                break;
            case _TILT_SENSOR_WAIT_STEP_2:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    GetClockTime(&_tiltSensor_time);
                }
                if((GetTimeoutValue(_tiltSensor_time) / mSEC_VAL) > TILT_SENSOR_WAIT_RESPONSE)
                {
                    ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_OUTCOME;
                }
                break;
            case _TILT_SENSOR_OUTCOME:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    //LOG_INFO("Tilt sensor self test check");
                }
                LOG_INFO("Tilt sensor self test result [%s]", ShmPrimaryMcuData->InputDet.bits.DoorOpen ? "OK" : "NG");
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorFail = ShmPrimaryMcuData->InputDet.bits.DoorOpen ? NO : YES;
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_STOP;
                break;
            case _TILT_SENSOR_STOP:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    //LOG_INFO("Tilt sensor self test stop!");
                }
                ShmPrimaryMcuData->OutputDrv.bits.SystemLed3Drv = false;
                ShmPrimaryMcuData->OutputDrv.bits.SystemLed4Drv = false;
                ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_WAIT_STOP;
                break;
            case _TILT_SENSOR_WAIT_STOP:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    GetClockTime(&_tiltSensor_time);
                }
                if((GetTimeoutValue(_tiltSensor_time) / uSEC_VAL) > TILT_SENSOR_WAIT_STOP)
                {
                    ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_FINISH;
                }
                break;
            case _TILT_SENSOR_FINISH:
                if(preTiltSensorStep != step)
                {
                    preTiltSensorStep = step;
                    LOG_INFO("Tilt sensor self test done!");
                }
                break;
        }
    }
    else
    {
        if(ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep != _TILT_SENSOR_NONE &&
            ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep != _TILT_SENSOR_FINISH)
        {
            ShmPrimaryMcuData->OutputDrv.bits.SystemLed3Drv = false;
            ShmPrimaryMcuData->OutputDrv.bits.SystemLed4Drv = false;
            LOG_INFO("Tilt sensor self test reset");
        }
        ShmChargerInfo->Control.CustomizedInfo.TiltSensorStep = _TILT_SENSOR_NONE;
    }
}

int main(void)
{
	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		LOG_ERROR("InitShareMemory NG");
		#endif
		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = 1;
		}
		sleep(5);
		return 0;
	}
	Uart1Fd = InitComPort();
	LOG_INFO("407 Port id = %d", Uart1Fd);

	if(Uart1Fd < 0)
	{
		#ifdef SystemLogMessage
		LOG_ERROR("InitComPort (Uart1 : AM3352 - STM32) NG");
		#endif

		if (ShmStatusCodeData != NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed = 1;
		}
		sleep(5);
		return 0;
	}

	SetRtcData();

	// update ac contact status
	//_acStatus = ShmSysConfigAndInfo->SysInfo.AcContactorStatus;
	_acStatus = ShmSysConfigAndInfo->SysInfo.AcContactorStatus > 0 ? 0 : 1;

	// set wrong value, it will update at the first time
	_OutputDrv = ~ShmPrimaryMcuData->OutputDrv.OutputDrvValue[0];

	for(;;)
	{
	    if(!ShmChargerInfo->Control.PrimaryCtrl.bits.Paused)
	    {
            // 程序開始之前~ 必須先確定 FW 版本與硬體版本，確認後!!~ 該模組才算是真正的 Initial Comp.
            // 模組更新 FW 後，需重新做
            if(ShmPrimaryMcuData->SelfTest_Comp != PASS)
            {
                memset(ShmPrimaryMcuData->version, 0x00, 16);

                GetFwAndHwVersion();
                sleep(1);

                if(strlen((char *)ShmPrimaryMcuData->version) != 0)
                {
                    ShmPrimaryMcuData->SelfTest_Comp = YES;
                }

                if(_CommFailCount >= COMM_FAIL_COUNT)
                {
                    LOG_INFO("Primary MCU Communication Fail: %d", _CommFailCount);
                    _CommFailCount = 0;
                }
            }
            else
            {
                GetInputGpioStatus();
                //LOG_INFO("Input Status: %02X %02X", ShmPrimaryMcuData->InputDet.InputDetValue[1], ShmPrimaryMcuData->InputDet.InputDetValue[0]);

                if(ShmChargerInfo->Control.CustomizedInfo.Flag.bits.TiltSensorEnable)
                {
                    CheckTiltSensor();
                }

                SetOutputGpio(ShmPrimaryMcuData->OutputDrv.OutputDrvValue[0]);
            }
	    }
		usleep(100000);
	}

	return FAIL;
}