csu3_am335x/EVSE/Projects/DM30/Apps/Module_PsuComm.c

#include "Module_PsuComm.h"

#define ARRAY_SIZE(A)       (sizeof(A) / sizeof(A[0]))
#define PASS                1
#define FAIL                -1
#define YES                 1
#define NO                  0
#define DERATING_COUNT      30
#define DERATING_GAP        30
#define ELEMENT_NOT_FIND    255
#define CHK_VOL_RANGE       50
#define CHK_CUR_RANGE       10
#define DERATING_RANGE      100
#define ZERO_CURRENT        10          // 該值須保持最小為 1A
#define ZERO_VOLTAGE        50
#define STOP_CURRENT        30
#define PSU_MIN_CUR         1000
#define PSU_MIN_VOL         1500
#define PRE_CHARG_STEP_CUR  30
#define PRE_CHARG_RANGE     50
#define EQUAL               0

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

bool libInitialize = false;
byte getAvailableCapOffset = 5;
byte deratingKeepCount = 0;

float evseOutVol = 0;
float evseOutCur = 0;

void PRINTF_FUNC(char *string, ...);

#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];
    va_list args;
    struct timeb  SeqEndTime;
    struct tm *tm;

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

    memset(Buf,0,sizeof(Buf));
    ftime(&SeqEndTime);
    SeqEndTime.time = time(NULL);
    tm=localtime(&SeqEndTime.time);

    if (ShmSysConfigAndInfo->SysConfig.SwitchDebugFlag == YES)
    {
        sprintf(Buf,"%02d:%02d:%02d.%03d - %s",
                tm->tm_hour,tm->tm_min,tm->tm_sec,SeqEndTime.millitm, buffer);
        printf("%s \n", Buf);
    }
    else
    {
        sprintf(Buf,"echo \"%04d-%02d-%02d %02d:%02d:%02d.%03d - %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,SeqEndTime.millitm,
                buffer,
                tm->tm_year+1900,tm->tm_mon+1);
        system(Buf);
    }

    return rc;
}

void PRINTF_FUNC(char *string, ...)
{
    va_list args;
    char buffer[4096];
    va_start(args, string);
    vsnprintf(buffer, sizeof(buffer), string, args);
    va_end(args);

    DEBUG_INFO("%s \n", buffer);
}
//=================================
// Common routine
//=================================
size_t FindIndex(const int a[], size_t size, int value, byte group)
{
    size_t index = 0;

    while ( index < size && a[index] != value ) ++index;
    return (index == size ? ELEMENT_NOT_FIND : group);
}

byte FindTargetGroup(byte address)
{
    byte _group = ELEMENT_NOT_FIND;

    if (ShmPsuData->GroupCount == 1)
        _group = 0;
    else
    {
        _group = FindIndex(connector_1, ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity, address, 0);

        if (_group == ELEMENT_NOT_FIND)
            _group = FindIndex(connector_2, ShmPsuData->PsuGroup[1].GroupPresentPsuQuantity, address, 1);
    }

    return _group;
}

bool IsOverModuleCount(byte count)
{
    bool result = false;

    if (count >= ShmPsuData->SystemPresentPsuQuantity)
        result = true;

    return result;
}
//=================================
// 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;
}

//=================================
// Alarm code mapping to share memory Function
//=================================
// 檢查 Byte 中某個 Bit 的值
// _byte : 欲改變的 byte
// _bit : 該 byte 的第幾個 bit
unsigned char mask_table[] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80 };
unsigned char DetectBitValue(unsigned char _byte, unsigned char _bit)
{
    return ( _byte & mask_table[_bit] ) != 0x00;
}

void AbnormalStopAnalysis(byte gun_index, int errCode)
{
    for (char i = 0; i < 3; i++)
    {
        unsigned char byteIndex = (errCode >> (8 * i)) & 0xff;

        for (char bitIndex = 0; bitIndex < 8; bitIndex++)
        {
            if(DetectBitValue(byteIndex , bitIndex) == 1)
            {
                switch(byteIndex)
                {
                    case 0:
                    {
                        if (bitIndex == 0)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuOutputShortCircuit = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = YES;
                    }
                        break;
                    case 1:
                    {
                        if (bitIndex == 1)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFailureAlarm = YES;
                        else if (bitIndex == 2)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuProtectionAlarm = YES;
                        else if (bitIndex == 3)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFanFailureAlarm = YES;
                        else if (bitIndex == 4)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuCriticalPointOTP = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = YES;
                    }
                    break;
                    case 2:
                    {
                        if (bitIndex == 0)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuPowerLimitedState = YES;
                        else if (bitIndex == 1)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDuplicateID = YES;
                        else if (bitIndex == 2)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseOnputImbalance = YES;
                        else if (bitIndex == 3)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = YES;
                        else if (bitIndex == 4)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputUVP = YES;
                        else if (bitIndex == 6)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputOVP = YES;
                    }
                    break;
                }
            }
//          else
//          {
//              switch (byteIndex) {
//              case 0: {
//                  if (bitIndex == 0)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuOutputShortCircuit = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = NO;
//              }
//                  break;
//              case 1: {
//                  if (bitIndex == 1)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFailureAlarm = NO;
//                  else if (bitIndex == 2)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuProtectionAlarm = NO;
//                  else if (bitIndex == 3)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFanFailureAlarm = NO;
//                  else if (bitIndex == 4)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuCriticalPointOTP = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = NO;
//              }
//                  break;
//              case 2: {
//                  if (bitIndex == 1)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDuplicateID = NO;
//                  if (bitIndex == 2)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseOnputImbalance = NO;
//                  else if (bitIndex == 3)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = NO;
//                  else if (bitIndex == 4)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputUVP = NO;
//                  else if (bitIndex == 6)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputOVP = NO;
//              }
//                  break;
//              }
//          }
        }
    }
}

//=================================
// Callback Function
//=================================
// no using -- GetOutputAndTempCallback
void GetStatusCallback(byte group, byte address, byte temp, int alarm)
{
//  if (ShmPsuData->Work_Step == INITIAL_START)
//      return;
//
//  if (IsOverModuleCount(address))
//      return;
//
//  byte group1 = FindTargetGroup(address);
//
//  if (group1 == 1)
//      address -= ShmPsuData->PsuGroup[group1 - 1].GroupPresentPsuQuantity;
//
//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp1 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp2 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp3 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].ExletTemp = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].AlarmCode = alarm;
//  AbnormalStopAnalysis(group1, alarm);

//  PRINTF_FUNC("***Status*** address = %d, temp = %d, err1 = %d, err2 = %d, err3 = %d, err4 = %d \n",
//          address, temp,
//          (alarm >> 24) & 0xFF,
//          (alarm >> 16) & 0xFF,
//          (alarm >> 8) & 0xFF,
//          alarm & 0xFF);
}
// no using -- GetOutputAndTempCallback End

void GetModuleCountCallback(byte group, byte count)
{
    if (group == SYSTEM_CMD)
        ShmPsuData->SystemPresentPsuQuantity = count;
    else
    {
        ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity = count;
    }
}

void GetMaxPowerAndCur(unsigned char mode, int ratingCur, int *pow, int *cur)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    unsigned short maxCurrent = ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent * 10;
    unsigned short maxPower = ShmSysConfigAndInfo->SysConfig.MaxChargingPower * 10;

    if (mode == _MAIN_CHARGING_MODE_AVER)
    {
        maxCurrent /= 2;
        maxPower /= 2;
    }

    if (maxPower != 0 && maxPower <= *pow)
        *pow = maxPower;

    if (maxCurrent != 0 && maxCurrent <= *cur)
        *cur = maxCurrent;

    if (ratingCur != 0 && ratingCur <= *cur)
        *cur = ratingCur;
}

void GetAvailableCapCallback(byte address, short maxVol, short minVol, short maxCur, short totalPow)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    int _groupPower = 0, _groupCurrent = 0;
    byte group = FindTargetGroup(address);
    float _chargingVol = 0, _targetVol = 0;

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
    {
        for (byte groupIndex = 0; groupIndex < _gunCount; groupIndex++)
        {
            if (chargingInfo[groupIndex]->EvBatteryMaxVoltage > 0)
            {
                _chargingVol = chargingInfo[groupIndex]->EvBatteryMaxVoltage;
                _targetVol = chargingInfo[groupIndex]->EvBatterytargetVoltage;
                break;
            }
        }
    }

    if (chargingInfo[group]->DeratingChargingCurrent == 0)
    {
        // 在還沒取得真正可輸出的電流前，依照 GFD 階段得到的真正 POWER / 模塊個數 / 車子電池最大電壓
        if (ShmPsuData->PsuGroup[group].GroupRealOutputPower > 0 && _chargingVol > 0)
        {
            ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent =
                    ((ShmPsuData->PsuGroup[group].GroupRealOutputPower / ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity) * 1000 / (int)_chargingVol) * 10;
        }
        else
        {
            // 注一 : 獲取模塊最大輸出能力 (忽視 Derating 狀態)，所以這邊需要限制實際可輸出的電流
            if (ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent <= 0)
                ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent = PSU_MIN_CUR;
        }
    }
    else
    {
        ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent = maxCur;
    }

    ShmPsuData->PsuGroup[group].PsuModule[address].AvailablePower = totalPow;
    // 總和該 Group 的可輸出電流
    for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
    {
        _groupCurrent += ShmPsuData->PsuGroup[group].PsuModule[index].AvailableCurrent;
        _groupPower += ShmPsuData->PsuGroup[group].PsuModule[index].AvailablePower;
    }

    // 各群得到最大輸出能力 (電流、Power)
    ShmPsuData->PsuGroup[group].GroupAvailableCurrent = _groupCurrent;
    ShmPsuData->PsuGroup[group].GroupAvailablePower = _groupPower;

    chargingInfo[group]->MaximumChargingVoltage = maxVol;

    int _power = 0, _current = 0, _ratingcurrent = 0, _sysRealPower = 0;
    bool isGetAllDeratingCurrent = true;

    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
    {
        _power += ShmPsuData->PsuGroup[index].GroupAvailablePower;
        _current += ShmPsuData->PsuGroup[index].GroupAvailableCurrent;
        _ratingcurrent += chargingInfo[index]->DeratingChargingCurrent;
        _sysRealPower += ShmPsuData->PsuGroup[index].GroupRealOutputPower;
        if (ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage >= PSU_MIN_VOL &&
            chargingInfo[index]->DeratingChargingCurrent == 0)
            isGetAllDeratingCurrent = false;
    }

    // 如果不是所有群都得到 Derating current，則先不採樣該次的 ratingCurrent
    if (!isGetAllDeratingCurrent) _ratingcurrent = 0;
    if (_ratingcurrent != 0) _current = _ratingcurrent;

    ShmPsuData->SystemAvailableCurrent = _current;
    ShmPsuData->SystemAvailablePower = _power;

    if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER ||
        (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_GET_NEW_CAP &&
         ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A))
    {
        int halfPow = ShmPsuData->PsuGroup[group].GroupAvailablePower;
        int halfCur = ShmPsuData->PsuGroup[group].GroupAvailableCurrent;
        int ratingCur = chargingInfo[group]->DeratingChargingCurrent;
        int gpRealPow = ShmPsuData->PsuGroup[group].GroupRealOutputPower;

        if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_GET_NEW_CAP &&
            ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
        {
            if (chargingInfo[group]->DividChargingCurrent == 0)
                return;
            else
            {
                halfCur = chargingInfo[group]->DividChargingCurrent;
                ratingCur = 0;
            }
        }

        GetMaxPowerAndCur(_MAIN_CHARGING_MODE_AVER, ratingCur, &halfPow, &halfCur);


        // 以下狀況 -> 槍資訊中的最大輸出能力，為該群的輸出能力
        // 1. 如不是最大充
        // 2. 智能切換成均充過程
        chargingInfo[group]->AvailableChargingCurrent = halfCur;
        chargingInfo[group]->AvailableChargingPower = halfPow;
        chargingInfo[group]->RealRatingPower = gpRealPow;

        if(chargingInfo[group]->DeratingChargingCurrent > 0)
        {
            unsigned short _powBuf = 0;
            _powBuf = ((chargingInfo[group]->DeratingChargingCurrent / 10) * ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage / 10) / 1000; // 單位是 KW

            if (_powBuf > ShmPsuData->PsuGroup[group].GroupRealOutputPower ||
                chargingInfo[group]->EvBatterytargetVoltage > 0)
            {
                ShmPsuData->PsuGroup[group].GroupRealOutputPower = _powBuf;
            }
        }
    }
    else if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
    {
        GetMaxPowerAndCur(_MAIN_CHARGING_MODE_MAX, _ratingcurrent, &_power, &_current);

        if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
        {
            for (byte count = 0; count < ShmSysConfigAndInfo->SysConfig.TotalConnectorCount; count++)
            {
                chargingInfo[count]->MaximumChargingVoltage = maxVol;
                chargingInfo[count]->AvailableChargingCurrent = _current;
                chargingInfo[count]->AvailableChargingPower = _power;
                chargingInfo[count]->RealRatingPower = _sysRealPower;
            }
        }
        else
        {
            // 如果是最大充，該槍資訊中的輸出能力為各群輸出能力的和
            chargingInfo[group]->AvailableChargingCurrent = _current;
            chargingInfo[group]->AvailableChargingPower = _power;
            chargingInfo[group]->RealRatingPower = _sysRealPower;
        }

        if(chargingInfo[group]->DeratingChargingCurrent > 0)
        {
            unsigned short _powBuf = 0;
            _powBuf = ((chargingInfo[group]->DeratingChargingCurrent / 10) * ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage / 10) / 1000; // 單位是 KW

            if (_powBuf > ShmPsuData->PsuGroup[group].GroupRealOutputPower ||
                    chargingInfo[group]->EvBatterytargetVoltage > 0 ||
                    _targetVol > 0)
            {
                ShmPsuData->PsuGroup[group].GroupRealOutputPower = _powBuf;
            }
        }
    }
}

void GetFwCallback(byte address, short dcSwVer, short pfcSwVer, short hwVer)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    sprintf((char *)ShmPsuData->PsuVersion[address].FwPrimaryVersion, "DC %d.%02d", (dcSwVer & 0xFF00) >> 8, dcSwVer & 0xFF);
    sprintf((char *)ShmPsuData->PsuVersion[address].FwSecondVersion, "PFC %d.%02d", (pfcSwVer & 0xFF00) >> 8, pfcSwVer & 0xFF);
    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;
    sprintf((char *)ShmPsuData->PsuGroup[group].PsuModule[address].FwVersion, "DC %d.%02d", (dcSwVer & 0xFF00) >> 8, dcSwVer & 0xFF);
    //DEBUG_INFO("fw Ver. = %s \n", ShmPsuData->PsuGroup[group].PsuModule[address].FwVersion);
}

// no using -- GetInputVoltageCallback
void GetInputVoltageCallback(byte address, unsigned short vol1, unsigned short vol2, unsigned short vol3)
{
//  if (ShmPsuData->Work_Step < GET_SYS_CAP)
//      return;
//
//  if (IsOverModuleCount(address))
//      return;
//
//  byte group = FindTargetGroup(address);
//
//  if (group == 1)
//      address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;
//
//  ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL1 = vol1;
//  ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL2 = vol2;
//  ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL3 = vol3;
//
//  PRINTF_FUNC("***Input*** address = %d, R = %d, S = %d, T = %d, gp = %d \n",
//              address, vol1, vol2, vol3, group);
}
// no using -- GetInputVoltageCallback End

// no using -- GetOutputAndTempCallback
void GetPresentOutputCallback(byte group, unsigned short outVol, unsigned short outCur)
{
//  if (ShmPsuData->Work_Step < GET_SYS_CAP)
//      return;

    //if (outCur != ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent)
    //{
    //  PRINTF_FUNC("Gp_%d, gp output cur = %d \n", group, outCur);
    //}

//  // PSU Group - 電壓
//  ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage = outVol;
//  // PSU Group - 電流
//  ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent = outCur;
//
//  if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX ||
//          (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER &&
//          (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_WAITING &&
//          ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_COMP))
//      )
//  {
//      unsigned short outputVol = 0;
//      unsigned short outputCur = 0;
//
//      for (byte index = 0; index < ShmPsuData->GroupCount; index++)
//      {
//          bool needtoAdd = true;
//
//          if (ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage > outputVol)
//              outputVol = ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage;
//
//          if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_ADJUST_M_TO_A &&
//                  ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
//          {
////                PRINTF_FUNC("Gp_%d, DividChargingCurrent = %d \n", index,
////                        chargingInfo[index]->DividChargingCurrent);
//              if (chargingInfo[index]->DividChargingCurrent == 0)
//                  needtoAdd = false;
//          }
//
//          if (needtoAdd)
//              outputCur += ShmPsuData->PsuGroup[index].GroupPresentOutputCurrent;
//      }
//
//      // 黑白機
//      if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
//      {
//          for (byte count = 0; count < ShmSysConfigAndInfo->SysConfig.TotalConnectorCount; count++)
//          {
//              float _vol_buf = outputVol;
//              float _cur_buf = outputCur;
//
//              // EVSE - 電壓
//              _vol_buf /= 10;
//              chargingInfo[count]->PresentChargingVoltage = _vol_buf;
//              // EVSE - 電流
//              _cur_buf /= 10;
//              chargingInfo[count]->PresentChargingCurrent = _cur_buf;
//          }
//      }
//
//      if ((chargingInfo[group]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[group]->SystemStatus <= S_COMPLETE) ||
//          (chargingInfo[group]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[group]->SystemStatus <= S_CCS_PRECHARGE_ST1))
//      {
//          float _vol_buf = outputVol;
//          float _cur_buf = outputCur;
//
//          // EVSE - 電壓
//          _vol_buf /= 10;
//          chargingInfo[group]->PresentChargingVoltage = _vol_buf;
//          // EVSE - 電流
//          _cur_buf /= 10;
//          chargingInfo[group]->PresentChargingCurrent = _cur_buf;
//      }
//  }
//  else
//  {
//      float _vol_buf = ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage;
//      float _cur_buf = ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent;
//
//      // EVSE - 電壓
//      _vol_buf /= 10;
//      chargingInfo[group]->PresentChargingVoltage = _vol_buf;
//      // EVSE - 電流
//      _cur_buf /= 10;
//      chargingInfo[group]->PresentChargingCurrent = _cur_buf;
//  }
//
//  PRINTF_FUNC("Gun_%d, PresentChargingVoltage = %f, PresentChargingCurrent = %f \n", group,
//          chargingInfo[group]->PresentChargingVoltage,
//          chargingInfo[group]->PresentChargingCurrent);
}
// no using -- GetOutputAndTempCallback End

void GetFanSpeedCallback(byte address, unsigned int fanSpeed)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_1 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_2 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_3 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_4 = fanSpeed;
}

void GetIavailableCallback(byte address, unsigned short Iavail, unsigned short Vext)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    //PRINTF_FUNC("address = %d, Iavail = %d \n", address, Iavail);
    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    //PRINTF_FUNC("group = %d, address_%d, Iavail = %d \n", group, address, Iavail);
    ShmPsuData->PsuGroup[group].PsuModule[address].IAvailableCurrent = Iavail;

    bool isPass = true;
    int totalCur = 0;
    byte sampleCount = 8;

    if (Iavail == 0)
    {
        for (byte count = 0; count < sampleCount; count++)
        {
            chargingInfo[group]->SampleChargingCur[count] = Iavail;
        }
    }
    else
    {
        // 該群的可輸出電流
        for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
        {
            totalCur += ShmPsuData->PsuGroup[group].PsuModule[index].IAvailableCurrent;
        }

        for (byte count = 0; count < sampleCount; count++)
        {
            if (chargingInfo[group]->SampleChargingCur[count] == 0)
            {
                chargingInfo[group]->SampleChargingCur[count] = totalCur;
                return;
            }
            else
            {
                if (chargingInfo[group]->SampleChargingCur[count] != totalCur)
                {
                    chargingInfo[group]->SampleChargingCur[count] = totalCur;
                    isPass = false;
                    break;
                }
            }
        }
    }

    if (isPass)
    {
        chargingInfo[group]->DeratingChargingCurrent = totalCur;
    }
}

void GetPresentOutputFCallback(byte group, float outVol, float outCur)
{
    // isinf : -1 = 負無窮，1 = 正無窮，0 = 其他
    if (isinf(outVol) == 0)
        ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage = (unsigned short)(outVol * 10);
    else
        ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage = 0;

    if (isinf(outCur) == 0)
        ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent = (unsigned short)(outCur * 10);
    else
        ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent = 0;

    if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX ||
                (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER &&
                (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_WAITING &&
                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_COMP))
            )
        {
            unsigned short outputVol = 0;
            unsigned short outputCur = 0;
            unsigned char _maxTOaver = 0;

            for (byte index = 0; index < ShmPsuData->GroupCount; index++)
            {
                bool needtoAdd = true;

                if (ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage > outputVol)
                    outputVol = ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage;

                if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_ADJUST_M_TO_A &&
                        ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
                {
                    if (chargingInfo[index]->DividChargingCurrent == 0)
                        needtoAdd = false;
                    else
                        _maxTOaver = index;
                }

                if (needtoAdd)
                    outputCur += ShmPsuData->PsuGroup[index].GroupPresentOutputCurrent;
            }

            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_ADJUST_M_TO_A &&
                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
            {
                if (chargingInfo[_maxTOaver]->DividChargingCurrent != 0)
                {
                    float _cur_buf = outputCur;
                    _cur_buf /= 10;
                    chargingInfo[_maxTOaver]->PresentChargingCurrent = _cur_buf;
                }
            }

            // 黑白機
            if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
            {
                for (byte count = 0; count < ShmSysConfigAndInfo->SysConfig.TotalConnectorCount; count++)
                {
                    float _vol_buf = outputVol;
                    float _cur_buf = outputCur;

                    // EVSE - 電壓
                    _vol_buf /= 10;
                    chargingInfo[count]->PresentChargingVoltage = _vol_buf;

                    _cur_buf /= 10;
                    chargingInfo[count]->PresentChargingCurrent = _cur_buf;
                }
            }

            if ((chargingInfo[group]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[group]->SystemStatus <= S_COMPLETE) ||
                (chargingInfo[group]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[group]->SystemStatus <= S_CCS_PRECHARGE_ST1))
            {
                float _vol_buf = outputVol;
                float _cur_buf = outputCur;

                // EVSE - 電壓
                _vol_buf /= 10;
                chargingInfo[group]->PresentChargingVoltage = _vol_buf;

                _cur_buf /= 10;
                chargingInfo[group]->PresentChargingCurrent = _cur_buf;
            }
        }
        else
        {
            float _vol_buf = ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage;
            float _cur_buf = ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent;

            // EVSE - 電壓
            _vol_buf /= 10;
            chargingInfo[group]->PresentChargingVoltage = _vol_buf;

            _cur_buf /= 10;
            chargingInfo[group]->PresentChargingCurrent = _cur_buf;
        }
}

//==========================================
// 特規用指令
//==========================================
void GetOutputAndTempCallback(byte address, unsigned short outputVol_s,
                              unsigned short outputCur_s, unsigned short outputPower,
                              unsigned char Temperature)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group].PsuModule[address].CriticalTemp1 = Temperature;
    ShmPsuData->PsuGroup[group].PsuModule[address].CriticalTemp2 = Temperature;
    ShmPsuData->PsuGroup[group].PsuModule[address].CriticalTemp3 = Temperature;
    ShmPsuData->PsuGroup[group].PsuModule[address].ExletTemp = Temperature;

//  PRINTF_FUNC("***Output Value and Temp*** group = %d, Vol = %d, Cur = %d \n",
//          group, outputVol_s, outputCur_s);
}

void GetModuleStatusCallback(byte address, unsigned char isErr, unsigned char status,
                             unsigned char err1, unsigned char err2, unsigned char err3,
                             unsigned char err4)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    byte group1 = FindTargetGroup(address);

    if (group1 == 1)
        address -= ShmPsuData->PsuGroup[group1 - 1].GroupPresentPsuQuantity;

    int alarm = (err2 << 24) | (err3 << 16) | (err4 << 8);

//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp1 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp2 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp3 = temp;
//  ShmPsuData->PsuGroup[group1].PsuModule[address].ExletTemp = temp;
    ShmPsuData->PsuGroup[group1].PsuModule[address].AlarmCode = alarm;
    AbnormalStopAnalysis(group1, alarm);

    // err2 == state 2
    // err3 == state 1
    // err4 == state 0
    //PRINTF_FUNC("***Status*** address = %d, alarm = %d \n", address, alarm);
}

void GetModuleInputCallback(byte address, unsigned short inputR,
                            unsigned short inputS, unsigned short inputT)
{
    if (ShmPsuData->Work_Step < GET_SYS_CAP)
        return;

    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL1 = inputR;
    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL2 = inputS;
    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL3 = inputT;

//    PRINTF_FUNC("***Input*** address = %d, R = %d, S = %d, T = %d \n",
//            address, inputR, inputS, inputT);
}
//==========================================
// 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;
    ShmPsuData->SystemAvailablePower = 0;

    PRINTF_FUNC("************ psu Group = %d \n", ShmPsuData->GroupCount);
    for (byte _groupCount = 0; _groupCount < ShmPsuData->GroupCount; _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("Module_PsuComm : FindChargingInfoData false \n");
                isPass = false;
                break;
            }
        }
        sleep(1);
    }

    if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
        ShmPsuData->GroupCount = 1;
    else
        ShmPsuData->GroupCount = _gunCount;

    char EvsePower[2];

    EvsePower[2] = '\0';
    char count = 0;
    byte psuTarIndex = 0;
    // 解析 ModelName 取得各槍各有幾個 PSU 及編號
    if (strlen((char *) ShmSysConfigAndInfo->SysConfig.ModelName) >= 6)
    {
        strncpy(EvsePower, (char *)(ShmSysConfigAndInfo->SysConfig.ModelName + 4), 2);
        if (strcmp(EvsePower, "15") == EQUAL)
            count = 5;
        else if (strcmp(EvsePower, "30") == EQUAL)
            count = 1;
        else if (strcmp(EvsePower, "60") == EQUAL)
            count = 2;
        else if (strcmp(EvsePower, "12") == EQUAL)
            count = 4;
        else if (strcmp(EvsePower, "18") == EQUAL)
            count = 6;
        else if (strcmp(EvsePower, "36") == EQUAL)
            count = 12;

        if (count > 0)
        {
            if (ShmPsuData->GroupCount == 1)
                conn_1_count = count;
            else if (ShmPsuData->GroupCount == 2)
            {
                if(count % 2 > 0)
                    conn_1_count = (count / 2) + 1;
                else
                    conn_1_count = (count / 2);

                conn_2_count = count - conn_1_count;
            }

            for(byte psuIndex = 0; psuIndex < conn_1_count; psuIndex++)
            {
                connector_1[psuIndex] = psuTarIndex;
                psuTarIndex++;
            }

            for(byte psuIndex = 0; psuIndex < conn_2_count; psuIndex++)
            {
                connector_2[psuIndex] = psuTarIndex;
                psuTarIndex++;
            }

            for(byte psuIndex = 0; psuIndex < conn_1_count; psuIndex++)
                PRINTF_FUNC("Connector 1 - Number = %d \n", connector_1[psuIndex]);

            for(byte psuIndex = 0; psuIndex < conn_2_count; psuIndex++)
                PRINTF_FUNC("Connector 2 - Number = %d \n", connector_2[psuIndex]);
        }
        else
            DEBUG_ERROR("Module_PsuComm : Can't parsing model name. \n");
    }
}

void CheckSmartChargingStep(bool isWaitingAver, bool isCharging, bool canAverageCharging)
{
    if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_PREPARE_M_TO_A)
    {
        if (isWaitingAver)
        {
            if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
            {
                ShmSysConfigAndInfo->SysInfo.CanAverageCharging = canAverageCharging;

                if (canAverageCharging)
                {
                    PRINTF_FUNC("=============Smart Charging : _REASSIGNED_GET_NEW_CAP============= Step 2 \n");
                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_GET_NEW_CAP;
                }
                else
                {
                    PRINTF_FUNC("=============Smart Charging : _REASSIGNED_NONE============= Step 0 \n");
                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_NONE;
                }
            }
            else
            {
                PRINTF_FUNC("=============Smart Charging : _REASSIGNED_NONE============= Step 0 \n");
                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_NONE;
            }
        }
    }
    else  if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag ==  _REASSIGNED_PREPARE_A_TO_M)
    {
        if (isCharging)
        {
            if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER)
            {
                PRINTF_FUNC("=============Smart Charging : _REASSIGNED_ADJUST_A_TO_M============= Step 12 \n");
                preChargingCur = preChargingTarget = 0;
                gettimeofday(&_max_time, NULL);
                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_ADJUST_A_TO_M;
            }
            else
            {
                PRINTF_FUNC("=============Smart Charging_1  : _REASSIGNED_COMP============= Step 15 \n");
                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_COMP;
            }
        }
        else
        {
            PRINTF_FUNC("=============Smart Charging_2 : _REASSIGNED_COMP============= Step 15 \n");
            ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_COMP;
        }
    }
}

int main(void)
{
    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_FUNC("InitShareMemory OK\n");

    // register callback function
    RefreshStatus(&GetStatusCallback);
    RefreshModuleCount(&GetModuleCountCallback);
    RefreshAvailableCap(&GetAvailableCapCallback);
    RefreshFwVersion(&GetFwCallback);
    RefreshInputVol(&GetInputVoltageCallback);
    RefreshGetOutput(&GetPresentOutputCallback);
    RefreshFanInfo(&GetFanSpeedCallback);
    RefreshIavailable(&GetIavailableCallback);
    RefreshGetOutputF(&GetPresentOutputFCallback);

    // GetPresentOutputCallback & GetStatusCallback
    AutoMode_RefreshOutputAndTemp(&GetOutputAndTempCallback);
    // GetStatusCallback
    AutoMode_RefreshModuleStatus(&GetModuleStatusCallback);
    // GetInputVoltageCallback
    AutoMode_RefreshModuleInput(&GetModuleInputCallback);

    sleep(2);
    _gunCount = ShmSysConfigAndInfo->SysConfig.TotalConnectorCount;
    // initial object
    InitialPsuData();
    Initialization();
    libInitialize = InitialCommunication();
    byte isInitialComp = NO;
    PRINTF_FUNC("ALTERNATIVE_CONG = %d \n", ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf);
    //main loop
    while (libInitialize)
    {
        // 斷電狀態
        if (ShmSysConfigAndInfo->SysInfo.AcContactorStatus == NO)
        {
            //一但 AC Off PSU 斷電全部的 PSU Group ID 會全部清 0
            if (!isInitialComp)
            {
                ShmPsuData->Work_Step = INITIAL_START;
                sleep(2);
                InitialPsuData();
                isInitialComp = YES;
            }
            sleep(1);
            continue;
        }
        else
            isInitialComp = NO;

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

        switch(ShmPsuData->Work_Step)
        {
            case INITIAL_START:
            {
                PRINTF_FUNC("== PSU == INITIAL_START \n");
                gettimeofday(&_cmdSubPriority_time, NULL);
                sleep(5);
                SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                SetWalkInConfig(SYSTEM_CMD, NO, 0);
                for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    isStartOutputSwitch[index] = false;
                ShmPsuData->Work_Step = GET_PSU_COUNT;
            }
                break;
            case GET_PSU_COUNT:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;
                byte moduleCount = 0;

                if (time > 2000)
                {
//                  if (ShmPsuData->GroupCount == 0)
//                      ShmPsuData->GroupCount = ShmSysConfigAndInfo->SysConfig.TotalConnectorCount;
                    // 分別取各群模組數量
                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        // 總和各群模組數量
                        moduleCount += ShmPsuData->PsuGroup[index].GroupPresentPsuQuantity;

                        // 取各群模組數量
                        GetModuleCount(index);
                    }
                    PRINTF_FUNC("== PSU == indexCount = %d, moduleCount = %d, sysCount = %d\n",
                                ShmPsuData->GroupCount, moduleCount, ShmPsuData->SystemPresentPsuQuantity);

                    // 發送取得目前全部模組數量
                    GetModuleCount(SYSTEM_CMD);

                    // 判斷系統數量與各群數量一致
                    if(moduleCount == ShmPsuData->SystemPresentPsuQuantity && moduleCount > 0)
                    {
                        PRINTF_FUNC("Psu Count = %d \n", moduleCount);
                        if (ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING)
                        {
                            // 電樁在 Booting 的狀態 - 自檢
                            PRINTF_FUNC("== PSU == GET_SYS_CAP \n");
                            ShmPsuData->Work_Step = GET_SYS_CAP;
                        }
                        else
                        {
                            PRINTF_FUNC("== PSU == _WORK_CHARGING \n");
                            ShmPsuData->Work_Step = _WORK_CHARGING;

                            //sdlu test
                            gettimeofday(&_test_time, NULL);
                        }
                    }

                    gettimeofday(&_cmdSubPriority_time, NULL);
                }
            }
                break;
            case GET_SYS_CAP:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;

                if (time > 1000)
                {
                    bool isFinish = true;
                    for (byte psuCount = 0; psuCount < ShmPsuData->SystemPresentPsuQuantity; psuCount++)
                    {
                        if (strcmp((char *)ShmPsuData->PsuVersion[psuCount].FwPrimaryVersion, "") == EQUAL ||
                                ShmPsuData->SystemAvailablePower <= 0 || ShmPsuData->SystemAvailableCurrent <= 0)
                        {
                            isFinish = false;
                            break;
                        }
                    }

                    if (!isFinish)
                    {
                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                        {
                            // Pooling Status
                            //GetStatus(index);

                            // 取系統總輸出能力
                            GetModuleCap(index);

                            // 取版號
                            GetModuleVer(index);
                        }
                    }
                    else
                    {
                        // 判斷系統輸出額定功率與電流
                        PRINTF_FUNC("SystemAvailableCurrent = %d, SystemAvailablePower = %d \n",
                                    ShmPsuData->SystemAvailableCurrent, ShmPsuData->SystemAvailablePower);

                        PRINTF_FUNC("== PSU == BOOTING_COMPLETE \n");
                        ShmPsuData->Work_Step = BOOTING_COMPLETE;
                    }

                    gettimeofday(&_cmdSubPriority_time, NULL);
                }
            }
                break;
            case BOOTING_COMPLETE:
            {
                bool isSelfTestPass = true;
                for (byte groupIndex = 0; groupIndex < _gunCount; groupIndex++)
                {
                    if (chargingInfo[groupIndex]->SystemStatus == S_BOOTING)
                    {
                        isSelfTestPass = false;
                    }
                }

                if (isSelfTestPass)
                    ShmPsuData->Work_Step = _WORK_CHARGING;
                sleep(1);
            }
                break;
            case _WORK_CHARGING:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;

                // 低 Priority 的指令
                if (time > 1500)
                {
                    isCharging = false;
                    isWaitingAver = false;
                    isReadToCharging = false;
                    CanAverageCharging = true;

                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        if (chargingInfo[index]->SystemStatus == S_CHARGING)
                        {
                            isCharging = true;
                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_PREPARE_M_TO_A)
                            {
                                if (toAverVolPoint > 0 &&
                                    toAverVolPoint == (chargingInfo[index]->EvBatterytargetCurrent * 10))
                                {
                                    // 欲最大充 -> 均充需要等待充電中的輸出電流拉高到目標電流
                                    if ((chargingInfo[index]->PresentChargingCurrent * 10) >=
                                        (chargingInfo[index]->EvBatterytargetCurrent * 10) - CHK_CUR_RANGE)
                                    {
                                        if (toAverVolCount == 0)
                                            isWaitingAver = true;
                                        else
                                            toAverVolCount--;
                                    }
                                }
                                else
                                {
                                    toAverVolPoint = (chargingInfo[index]->EvBatterytargetCurrent * 10);
                                    toAverVolCount = 3;
                                }
                            }
                            else
                            {
                                toAverVolPoint = 0;
                                toAverVolCount = 3;
                            }
                        }

                        if ((chargingInfo[index]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[index]->SystemStatus <= S_CHARGING) ||
                            (chargingInfo[index]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[index]->SystemStatus <= S_CCS_PRECHARGE_ST1))
                        {
                            isReadToCharging = true;
                        }

                        if (chargingInfo[index]->DeratingChargingCurrent < STOP_CURRENT)
                        {
                            CanAverageCharging = false;
                        }
                    }

                    gettimeofday(&_cmdSubPriority_time, NULL);
                    CheckSmartChargingStep(isWaitingAver, isCharging, CanAverageCharging);
                }

                for (byte groupIndex = 0; groupIndex < _gunCount; groupIndex++)
                {
                    // 取系統總輸出能力
                    GetModuleCap(groupIndex);

                    // 取各群輸出電壓電流 (float)
                    GetModuleOutputF(groupIndex);

                    // 取得模塊輸出額定電流能力
                    GetModuleIavailable(groupIndex);

                    // 針對各槍當前狀態，傳送需要回傳的資料指令
                    if (((chargingInfo[groupIndex]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[groupIndex]->SystemStatus <= S_CHARGING) && chargingInfo[groupIndex]->RelayK1K2Status) ||
                        (chargingInfo[groupIndex]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[groupIndex]->SystemStatus <= S_CHARGING && chargingInfo[groupIndex]->Type == _Type_GB) ||
                        (chargingInfo[groupIndex]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[groupIndex]->SystemStatus <= S_CCS_PRECHARGE_ST1))
                    {
                        if (chargingInfo[groupIndex]->FireChargingVoltage > 0 &&
                        evseOutVol != (chargingInfo[groupIndex]->FireChargingVoltage / 10))
                        {
                            evseOutVol = (chargingInfo[groupIndex]->FireChargingVoltage / 10);
                            PRINTF_FUNC("groupIndex = %d, ev need vol = %f, evse output vol = %f \n", groupIndex,
                                        (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                        chargingInfo[groupIndex]->FireChargingVoltage);
                        }

                        if ((chargingInfo[groupIndex]->PresentChargingCurrent * 10) > 0 &&
                            evseOutCur != (chargingInfo[groupIndex]->PresentChargingCurrent * 10))
                        {
                            evseOutCur = (chargingInfo[groupIndex]->PresentChargingCurrent * 10);
                            PRINTF_FUNC("groupIndex = %d, ev need cur = %f, evse output cur = %f \n", groupIndex,
                                        (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10),
                                        (chargingInfo[groupIndex]->PresentChargingCurrent * 10));
                        }

                        if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
                        {
//                          PRINTF_FUNC("index = %d, SystemStatus = %d, Ev = %f, curCur = %f \n", groupIndex,
//                                  chargingInfo[groupIndex]->SystemStatus, chargingInfo[groupIndex]->EvBatterytargetCurrent,
//                                  (chargingInfo[groupIndex]->PresentChargingCurrent * 10));
                            // 智能判斷 Start -----------------------------------------------------------
                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_GET_NEW_CAP &&
                                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
                            {
                                if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_GET_NEW_CAP)
                                {
                                    //PRINTF_FUNC("group = %d, GroupPresentOutputCurrent = %d \n",
                                    //      groupIndex, ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent);

                                    if (chargingInfo[groupIndex]->DividChargingCurrent == 0)
                                    {
                                        chargingInfo[groupIndex]->DividChargingCurrent = ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent;
                                    }
                                }
                                PRINTF_FUNC("Index = %d, DividChargingCurrent = %f \n", groupIndex, chargingInfo[groupIndex]->DividChargingCurrent);
                            }
                            else
                            {
                                chargingInfo[groupIndex]->DividChargingCurrent = 0;
                                chargingInfo[groupIndex]->MaxChargingToAverPassFlag = 0;
                            }

                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_GET_NEW_CAP)
                            {
                                if (ShmPsuData->SystemAvailableCurrent != chargingInfo[groupIndex]->AvailableChargingCurrent)
                                {
                                    // 車端要求電流為該充電槍的額定輸出電流的範圍內
                                    if ((chargingInfo[groupIndex]->EvBatterytargetCurrent * 10) <= ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent + DERATING_GAP ||
                                        deratingKeepCount >= DERATING_COUNT)
                                    {
                                        // 車端降載完成
                                        PRINTF_FUNC("Index = %d, newEvCurrent = %f \n", groupIndex, (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));
                                        PRINTF_FUNC("=============Smart Charging : _REASSIGNED_ADJUST_M_TO_A============= Step 3 \n");
                                        ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_ADJUST_M_TO_A;
                                        gettimeofday(&_derating_time, NULL);
                                        deratingKeepCount = 0;
                                    }
                                    else
                                    {
                                        deratingKeepCount++;
                                        PRINTF_FUNC("** Step 2 ** : Index = %d, EvBatterytargetCurrent = %f, TargetCurrent = %d, Count = %d \n",
                                                    groupIndex,
                                                    (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10),
                                                    (ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent + DERATING_GAP),
                                                    deratingKeepCount);
                                    }
                                }

                            }
                            else if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_ADJUST_M_TO_A)
                            {
                                bool isChanged = false;

                                if (chargingInfo[groupIndex]->AvailableChargingCurrent <= (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10))
                                {
                                    PRINTF_FUNC("** _REASSIGNED_ADJUST_M_TO_A ** Gun_%d, PresentChargingCurrent = %f, AvailableChargingCurrent = %f, EvBatterytargetCurrent = %f \n", groupIndex,
                                                (chargingInfo[groupIndex]->PresentChargingCurrent * 10),
                                                chargingInfo[groupIndex]->AvailableChargingCurrent,
                                                (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));

                                    for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                    {
                                        if (chargingInfo[subIndex]->SystemStatus == S_REASSIGN)
                                        {
                                            // 當 B 模塊輸出電流小於 5A 及退開 relay
                                            if ((ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent) <= 50)
                                                isChanged = true;
                                            break;
                                        }
                                    }
                                }
                                else if (((chargingInfo[groupIndex]->PresentChargingCurrent * 10) >= ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent - CHK_CUR_RANGE) &&
                                         ((chargingInfo[groupIndex]->PresentChargingCurrent * 10) <= ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent + CHK_CUR_RANGE))
                                {
                                    for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                    {
                                        if (chargingInfo[subIndex]->SystemStatus == S_REASSIGN)
                                        {
                                            if ((ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent) <= CHK_CUR_RANGE)
                                                isChanged = true;
                                            break;
                                        }
                                    }
                                }

                                if (isChanged)
                                {
                                    PRINTF_FUNC("** _REASSIGNED_ADJUST_M_TO_A To 4** Gun_%d, PresentChargingCurrent = %f, GroupPresentOutputCurrent = %d \n", groupIndex,
                                                (chargingInfo[groupIndex]->PresentChargingCurrent * 10),
                                                ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent);

                                    // 輸出端與車端要求電流接近
                                    PRINTF_FUNC("=============Smart Charging : _REASSIGNED_RELAY_M_TO_A============= Step 4 \n");
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_RELAY_M_TO_A;
                                }
                                else
                                {
                                    if ((GetTimeoutValue(_derating_time) / 1000) > 1000)
                                    {
                                        gettimeofday(&_derating_time, NULL);
                                    }
                                }
                            }

                            //if (ShmPsuData->SystemAvailablePower > 0)
                            {
                                // 調整輸出電流 : 漸進調整方式
                                if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_GET_NEW_CAP &&
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag < _REASSIGNED_RELAY_M_TO_A)
                                {
                                    if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_ADJUST_M_TO_A)
                                    {
                                        // 當前充電中的目標電壓
                                        float targetVol = (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10);
                                        byte reassignIndex = ELEMENT_NOT_FIND;

                                        // 找到等待分配的槍
                                        for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                        {
                                            if (chargingInfo[subIndex]->SystemStatus == S_REASSIGN)
                                            {
                                                reassignIndex = subIndex;
                                                break;
                                            }
                                        }

                                        if (reassignIndex != ELEMENT_NOT_FIND)
                                        {
                                            if ((GetTimeoutValue(_derating_time) / 1000) <= 50 ||
                                                chargingInfo[groupIndex]->MaxChargingToAverPassFlag == 0)
                                            {
                                                chargingInfo[groupIndex]->MaxChargingToAverPassFlag = 1;
                                                // A 模塊維持當前電壓電流
                                                //PRINTF_FUNC("A : index = %d, cur = %d \n", groupIndex, ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent);
                                                //PresentOutputVol(groupIndex, targetVol, ShmPsuData->PsuGroup[groupIndex].GroupPresentOutputCurrent);
                                                //PRINTF_FUNC("set out (%d) value = %f******** 1 \n", groupIndex, chargingInfo[groupIndex]->EvBatterytargetCurrent);
                                                PresentOutputVol(groupIndex, targetVol, (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));
//                                          }
//
//                                          if ((GetTimeoutValue(_derating_time) / 1000) <= 50)
//                                          {
                                                // 直接拉掉 B 模塊的電流
                                                //PRINTF_FUNC("B : index = %d, cur = %d \n", reassignIndex, CHK_CUR_RANGE);
                                                //PRINTF_FUNC("set out (%d) value = %d******** 2 \n", reassignIndex, CHK_CUR_RANGE);
                                                PresentOutputVol(reassignIndex, targetVol, CHK_CUR_RANGE);
                                            }
                                        }
                                    }

                                    if ((chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) == 0)
                                    {
                                        //PRINTF_FUNC("sys ******** 1 \n");
                                        bool isNeedToClosePower = false;
                                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                        {
                                            if (isStartOutputSwitch[index])
                                            {
                                                isNeedToClosePower = true;
                                            }
                                            isStartOutputSwitch[index] = false;
                                        }

                                        if (isNeedToClosePower)
                                        {
                                            SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                            FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                                        }
                                    }
                                }
                                else if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_RELAY_M_TO_A)
                                {
                                    //PRINTF_FUNC("set out (%d) value = %f******** 3 \n", groupIndex, chargingInfo[groupIndex]->EvBatterytargetCurrent);
                                    PresentOutputVol(groupIndex,
                                                     (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                                     (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));
                                }
                                else if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
                                {
                                    // 該充電槍的目標電壓與目標電流
                                    PresentOutputVol(SYSTEM_CMD,
                                                     (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                                     (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));

                                    if ((chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) == 0)
                                    {
                                        //PRINTF_FUNC("sys ******** 2 \n");
                                        bool isNeedToClosePower = false;
                                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                        {
                                            if (isStartOutputSwitch[index])
                                            {
                                                isNeedToClosePower = true;
                                            }
                                            isStartOutputSwitch[index] = false;
                                        }

                                        if (isNeedToClosePower)
                                        {
                                            SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                            FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                                        }
                                    }
                                    else
                                    {
                                        bool isNeedToOpenPower = false;
                                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                        {
                                            if (!isStartOutputSwitch[index])
                                            {
                                                isNeedToOpenPower = true;
                                            }
                                            isStartOutputSwitch[index] = true;
                                        }

                                        if (isNeedToOpenPower)
                                        {
                                            SwitchPower(SYSTEM_CMD, PSU_POWER_ON);
                                            FlashLed(SYSTEM_CMD, PSU_FLASH_ON);
                                        }
                                    }
                                }
                            }
                        }
                        else if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER)
                        {
                            // 智能判斷 Start -----------------------------------------------------------
                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_ADJUST_A_TO_M)
                            {
                                bool balanceVol = true;

                                for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                {
                                    if (chargingInfo[subIndex]->SystemStatus == S_IDLE ||
                                        chargingInfo[subIndex]->SystemStatus == S_RESERVATION)
                                    {
                                        // 各群電壓接近平衡
                                        if (((chargingInfo[subIndex]->PresentChargingVoltage * 10) < (chargingInfo[groupIndex]->PresentChargingVoltage * 10) - ZERO_VOLTAGE) ||
                                            ((chargingInfo[subIndex]->PresentChargingVoltage * 10) < (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) - CHK_VOL_RANGE))
                                        {
                                            PRINTF_FUNC("** _REASSIGNED_ADJUST_A_TO_M ** Gun_%d, PresentChargingVoltage = %f, PresentChargingVoltage_V = %f, EvBatterytargetVoltage = %f \n", subIndex,
                                                        (chargingInfo[subIndex]->PresentChargingVoltage * 10),
                                                        ((chargingInfo[groupIndex]->PresentChargingVoltage * 10) - ZERO_VOLTAGE),
                                                        ((chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) - CHK_VOL_RANGE));
                                            balanceVol = false;
                                        }
                                        break;
                                    }
                                }

                                if (balanceVol)
                                {
                                    // 閒置端與車端要求電壓接近
                                    PRINTF_FUNC("=============Smart Charging : _REASSIGNED_RELAY_A_TO_M============= Step 13 \n");
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_RELAY_A_TO_M;
                                }
                                else
                                {
                                    if ((GetTimeoutValue(_max_time) / 1000) > 500)
                                    {
                                        gettimeofday(&_max_time, NULL);
                                    }
                                }
                            }
                            else if(ShmSysConfigAndInfo->SysInfo.ReAssignedFlag == _REASSIGNED_WAITING)
                            {
                                int idleCurrent = 0;
                                int chargingCurrent = 0;

                                for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                {
                                    if (chargingInfo[subIndex]->SystemStatus == S_IDLE ||
                                        chargingInfo[subIndex]->SystemStatus == S_RESERVATION ||
                                        chargingInfo[subIndex]->SystemStatus == S_REASSIGN_CHECK)
                                        idleCurrent = ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent;
                                    else
                                        chargingCurrent = ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent;
                                }

                                if (idleCurrent >= chargingCurrent - PRE_CHARG_RANGE)
                                {
                                    PRINTF_FUNC("=============Smart Charging_0 : _REASSIGNED_COMP============= Step 15 \n");
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_COMP;
                                }
                                else
                                {
                                    if ((GetTimeoutValue(_max_time) / 1000) > 500)
                                    {
                                        gettimeofday(&_max_time, NULL);
                                    }
                                }
                            }

                            if (chargingInfo[groupIndex]->AvailableChargingCurrent > 0)
                            {
                                if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_ADJUST_A_TO_M)
                                {
                                    byte reassignIndex = ELEMENT_NOT_FIND;

                                    for (byte subIndex = 0; subIndex < ShmPsuData->GroupCount; subIndex++)
                                    {
                                        if (chargingInfo[subIndex]->SystemStatus == S_IDLE ||
                                            chargingInfo[subIndex]->SystemStatus == S_RESERVATION ||
                                            chargingInfo[subIndex]->SystemStatus == S_REASSIGN_CHECK)
                                        {
                                            reassignIndex = subIndex;
//                                          if ((GetTimeoutValue(_max_time) / 1000) <= 50)
//                                          {
//                                              // 閒置模塊升壓，另對剛分配近來的模塊，預上升電流值 (preChargingCur)
//                                              PresentOutputVol(subIndex,
//                                                      chargingInfo[groupIndex]->EvBatterytargetVoltage,
//                                                      ZERO_CURRENT + preChargingTarget);
//                                          }
                                            //PRINTF_FUNC("preChargingCur = %d \n", preChargingCur);
                                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_WAITING)
                                            {
                                                preChargingCur = ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent;
                                            }
                                            else
                                                preChargingCur = 0;
                                        }
                                        else
                                        {
                                            //PRINTF_FUNC("CurOutputCurrent = %d \n", ShmPsuData->PsuGroup[subIndex].GroupPresentOutputCurrent - preChargingCur);
                                            // 充電中的模塊維持輸出
//                                          if ((GetTimeoutValue(_max_time) / 1000) <= 50)
//                                          {
//                                              PresentOutputVol(subIndex,
//                                                  chargingInfo[subIndex]->EvBatterytargetVoltage,
//                                                  chargingInfo[subIndex]->EvBatterytargetCurrent - preChargingCur);
//                                          }

                                            if ((ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_WAITING) &&
                                                (preChargingCur >= preChargingTarget - ZERO_CURRENT))
                                                preChargingTarget += PRE_CHARG_STEP_CUR;

                                            if (preChargingTarget >= (chargingInfo[subIndex]->EvBatterytargetCurrent * 10) / 2)
                                                preChargingTarget = (chargingInfo[subIndex]->EvBatterytargetCurrent * 10) / 2;
                                        }
                                    }

                                    if (reassignIndex != ELEMENT_NOT_FIND)
                                    {
                                        if ((GetTimeoutValue(_max_time) / 1000) <= 50)
                                        {
                                            //PRINTF_FUNC("set out (%d) value = %d******** 5 \n", reassignIndex, ZERO_CURRENT + preChargingTarget);
                                            // 閒置模塊升壓，另對剛分配近來的模塊，預上升電流值 (preChargingCur)
                                            PresentOutputVol(reassignIndex,
                                                             (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                                             ZERO_CURRENT + preChargingTarget);

                                            byte _ovCahrgingCur = 0;
                                            if (preChargingCur > PRE_CHARG_STEP_CUR)
                                                _ovCahrgingCur = PRE_CHARG_STEP_CUR;

                                            //PRINTF_FUNC("set out (%d) value = %f******** 6 \n", groupIndex, chargingInfo[groupIndex]->EvBatterytargetCurrent - preChargingCur - _ovCahrgingCur);
                                            PresentOutputVol(groupIndex,
                                                             (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                                             (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10) - preChargingCur - _ovCahrgingCur);
                                        }
                                    }

                                    if ((chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) == 0)
                                    {
                                        //PRINTF_FUNC("sys ******** 3 \n");
                                        bool isNeedToClosePower = false;
                                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                        {
                                            if (isStartOutputSwitch[index])
                                            {
                                                isNeedToClosePower = true;
                                            }
                                            isStartOutputSwitch[index] = false;
                                        }

                                        if (isNeedToClosePower)
                                        {
                                            SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                            FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                                        }
                                    }
                                    else
                                    {
                                        bool isNeedToOpenPower = false;
                                        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                        {
                                            if (!isStartOutputSwitch[index])
                                            {
                                                isNeedToOpenPower = true;
                                            }
                                            isStartOutputSwitch[index] = true;
                                        }

                                        if (isNeedToOpenPower)
                                        {
                                            SwitchPower(SYSTEM_CMD, PSU_POWER_ON);
                                            FlashLed(SYSTEM_CMD, PSU_FLASH_ON);
                                        }
                                    }
                                }
                                else
                                {
                                    //PRINTF_FUNC("set out (%d) value = %f******** 7 \n", groupIndex, chargingInfo[groupIndex]->EvBatterytargetCurrent);
                                    PresentOutputVol(groupIndex,
                                                     (chargingInfo[groupIndex]->EvBatterytargetVoltage * 10),
                                                     (chargingInfo[groupIndex]->EvBatterytargetCurrent * 10));

                                    if ((chargingInfo[groupIndex]->EvBatterytargetVoltage * 10) == 0)
                                    {
                                        //PRINTF_FUNC("%d ******** 4 \n", groupIndex);
                                        if (isStartOutputSwitch[groupIndex])
                                        {
                                            isStartOutputSwitch[groupIndex] = false;
                                            SwitchPower(groupIndex, PSU_POWER_OFF);
                                            FlashLed(groupIndex, PSU_FLASH_NORMAL);
                                        }
                                    }
                                    else
                                    {
                                        if (!isStartOutputSwitch[groupIndex])
                                        {
                                            isStartOutputSwitch[groupIndex] = true;
                                            SwitchPower(groupIndex, PSU_POWER_ON);
                                            FlashLed(groupIndex, PSU_FLASH_ON);
                                        }
                                    }
                                }
                            }
                        }
                    }
                    else if (chargingInfo[groupIndex]->SystemStatus >= S_TERMINATING &&
                             chargingInfo[groupIndex]->SystemStatus <= S_COMPLETE)
                    {
                        if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
                        {
                            if (!isCharging)
                            {
                                //PRINTF_FUNC("sys ******** 5 \n");
                                bool isNeedToClosePower = false;
                                for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                {
                                    if (isStartOutputSwitch[index])
                                    {
                                        isNeedToClosePower = true;
                                    }
                                    isStartOutputSwitch[index] = false;
                                }

                                if (isNeedToClosePower)
                                {
                                    SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                    FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                                }

                                if (chargingInfo[groupIndex]->SystemStatus == S_COMPLETE)
                                {
                                    if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_PREPARE_M_TO_A &&
                                        ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
                                    {
                                        // 代表在切換的過程中，停止充電了
                                        if ((chargingInfo[groupIndex]->PresentChargingCurrent * 10) <= STOP_CURRENT)
                                            ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_RELAY_M_TO_A;
                                    }
                                }
                            }
                            else if (chargingInfo[groupIndex]->SystemStatus == S_COMPLETE)
                            {
                                // 代表充電的槍依舊在充電，欲進入充電的槍取消充電了
                                if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_PREPARE_M_TO_A &&
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag <= _REASSIGNED_RELAY_M_TO_A)
                                {
                                    ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_NONE;
                                }
                            }
                        }
                        else if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER)
                        {
                            if (!isReadToCharging)
                            {
                                bool isNeedToClosePower = false;
                                for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                                {
                                    if (isStartOutputSwitch[index])
                                    {
                                        isNeedToClosePower = true;
                                    }
                                    isStartOutputSwitch[index] = false;
                                }

                                if (isNeedToClosePower)
                                {
                                    SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                    FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                                }
                            }
                            else
                            {
                                if (isStartOutputSwitch[groupIndex])
                                {
                                    isStartOutputSwitch[groupIndex] = false;
                                    SwitchPower(groupIndex, PSU_POWER_OFF);
                                    FlashLed(groupIndex, PSU_FLASH_NORMAL);
                                }
                            }

                            if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_WAITING)
                                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_COMP;
                            else
                                ShmSysConfigAndInfo->SysInfo.ReAssignedFlag = _REASSIGNED_NONE;
                        }
                    }
                    else if ((chargingInfo[groupIndex]->SystemStatus >= S_PREPARNING && chargingInfo[groupIndex]->SystemStatus <= S_PREPARING_FOR_EV) &&
                             ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER)
                    {
                        //PRINTF_FUNC("%d ******** 7 \n", groupIndex);
                        if (isStartOutputSwitch[groupIndex])
                        {
                            isStartOutputSwitch[groupIndex] = false;
                            SwitchPower(groupIndex, PSU_POWER_OFF);
                            FlashLed(groupIndex, PSU_FLASH_NORMAL);
                        }
                    }
                }
                    break;
            }
            case _TEST_MODE:
            {
                // 在測試模式中，保持與模塊的通訊
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;

                if (time > 1500)
                {
                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        // 取系統總輸出能力
                        GetModuleCap(index);

                        // 取各群輸出電壓電流 (float)
                        GetModuleOutputF(index);
                    }

                    gettimeofday(&_cmdSubPriority_time, NULL);
                }

                byte _switch = 0x00;
                if ((chargingInfo[0]->EvBatterytargetVoltage * 10) > 0 && (chargingInfo[0]->EvBatterytargetCurrent * 10) > 0)
                    _switch = 0x01;

                for (byte _groupCount_1 = 0; _groupCount_1 < conn_1_count; _groupCount_1++)
                {
                    SetDirModulePresentOutput(connector_1[_groupCount_1],
                        (chargingInfo[0]->EvBatterytargetVoltage * 10),
                        (chargingInfo[0]->EvBatterytargetCurrent * 10),
                        _switch, _switch);
                }

                for (byte _groupCount_2 = 0; _groupCount_2 < conn_2_count; _groupCount_2++)
                {
                    SetDirModulePresentOutput(connector_2[_groupCount_2],
                        (chargingInfo[0]->EvBatterytargetVoltage * 10),
                        (chargingInfo[0]->EvBatterytargetCurrent * 10),
                        _switch, _switch);
                }
            }
                break;
        }
        usleep(20000);
    }
    return FAIL;
}