#include "plt.h"

static struct state_t pws1_states[] = {
    { SMST_LAUNCH, "launch" },
    { SMST_STDBY, "stdby" },
    { SMST_STOP, "stop" },
    { SMST_READY, "ready" },
    { SMST_DHG, "dhg" },
    { SMST_CHG, "chg" },
    { SMST_OFFGRID, "offgrid" },
    { SMST_ERR, "err" },
};

static struct err_t pws1_errs[] = {
    {PWS1ERR_NONE, "none"},

    // launch PWS1ERR_LAUNCH_COMMERR
    {PWS1ERR_LAUNCH_COMMERR, "launch, comm err"},

    // err
    {PWS1ERR_ERR_COMMERR, "err, comm err"},
    {PWS1ERR_ERR_PWRUP, "err, pwrup"},
    
    // stdby
    {PWS1ERR_STDBY_COMMERR, "stdby, comm err"},
    {PWS1ERR_STDBY_CHK_DEVAPS0_FAIL_AFTER_SET, "stdby, chk aps = 0 fail"},
    {PWS1ERR_STDBY_CHK_STOP_FAIL_AFTER_SET, "stdby, chk stop fail"},
    {PWS1ERR_STDBY_WAIT_STOP_TIMEOUT, "stdby, wait stop timeout"},
    {PWS1ERR_STDBY_WAIT_RUN_TIMEOUT, "stdby, wait run state timeout"},
    {PWS1ERR_STDBY_UNKOWN_RUNSTAT, "stdby, unknown run state"},

    // stop
    {PWS1ERR_STOP_COMMERR, "stop, comm err"},
    {PWS1ERR_STOP_NONE_STOP_DETECTED, "stop, none stop run state detected"},
    {PWS1ERR_STOP_CHK_DEVAPS0_FAIL_AFTER_SET, "stop, chk dev aps = 0 fail"},
    {PWS1ERR_STOP_CHK_ONGRID_FAIL_AFTER_SET, "stop, chk on grid fail after set"},
    {PWS1ERR_STOP_CHK_DISPATCH_AC_FAIL_AFTER_SET, "stop, chk dispatch ac fail after set"},
    {PWS1ERR_STOP_CHK_AC_CONTROL_MODEL_FIX_AC_POWER_FIAL_AFTER_SET, "stop, chk ac control fix power fail fater set"},
    {PWS1ERR_STOP_CHK_STATE_RUNNING_FAIL_AFTER_SET, "stop, chk state running fail after set"},
    
    // ready
    {PWS1ERR_READY_COMMERR, "ready, comm err"},
    {PWS1ERR_READY_WAIT_STOP_TIMEOUT, "ready, wait stop timeout"},
    {PWS1ERR_READY_WAIT_AP4DHG_TIMEOUT, "ready, wait ap for dhg timeout"},
    {PWS1ERR_READY_WAIT_DHG_STATE_TIME_OUT, "ready, wait dhg state timeout"},
    {PWS1ERR_READY_WAIT_AP4CHG_TIMEOUT, "ready, wait ap for chg timeout"}, 
    {PWS1ERR_READY_WAIT_CHG_STATE_TIME_OUT, "ready, wait chg state timeout"}, 
    {PWS1ERR_READY_WAIT_RUNSTATE_STOP_TIMEOUT_AFTER_SET, "ready, wait run state stop time out after set"},
    {PWS1ERR_READY_WAIT_RUNMODE_OFFGRID_TIMEOUT_AFTER_SET, "stop, wait run mode offgrid time out after set"},
    {PWS1ERR_READY_WAIT_RUNSTATE_RUNING_TIMEOUT_AFTER_SET, "stop, wait run state running time out after set"},    
    {PWS1ERR_READY_WAIT_TARGET_VOLTAGE_FOR_OFFGRID_TIMEOUT, "stop, wait target voltage for offgrid timeout"}, 
    
    
    // dhg
    {PWS1ERR_DHG_COMMERR, "dhg, comm err"},
    {PWS1ERR_DHG_STATE_NOT_RUN_DETECTED, "dhg, not run detected"},    
    {PWS1ERR_DHG_STATE_NOT_DHG_DETECTED, "dhg, non-dhg detected"},
    {PWS1ERR_DHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT, "dhg, wait ap = 0 timeout"},    
    {PWS1ERR_DHG_WAIT_DEV_NO_DHG_TIME_OUT, "dhg, wait no dhg timeout"},   

    // chg
    {PWS1ERR_CHG_COMMERR, "chg, comm err"},
    {PWS1ERR_CHG_STATE_NOT_RUN_DETECTED, "chg, not run detected"},    
    {PWS1ERR_CHG_STATE_NOT_CHG_DETECTED, "chg, non-chg detected"},
    {PWS1ERR_CHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT, "chg, wait ap = 0 timeout"},    
    {PWS1ERR_CHG_WAIT_DEV_NO_CHG_TIME_OUT, "chg, wait no chg timeout"},  

    // offgrid
    {PWS1ERR_OFFGRID_ERRSTAT_DETECTED, "offgrid, err state detected"},
    {PWS1ERR_OFFGRID_NOT_RUNNING_DETECTED, "offgrid, not run detected"},  
    {PWS1ERR_OFFGRID_WAIT_STOP_TIME_OUT, "offgrid, wait stop time out"},
    {PWS1ERR_OFFGRID_WAIT_RUN_MODE_ON_GIRD_TIME_OUT, "offgrid, wait run mode on grid time out"},    
};

int pws1_sm_init(int idx)
{
    struct statemachine_t* sm = &pws1[idx].sm;

    sm_reset_timing(sm, 10, 10);
    sm->states = pws1_states;
    sm->state_nbr = sizeof(pws1_states)/sizeof(struct state_t);
    sm->errs = pws1_errs;
    sm->err_nbr = sizeof(pws1_errs)/sizeof(struct err_t);
    sm_set_state( sm, SMST_LAUNCH, PWS1ERR_NONE );
    
    return 0;
}

static void pws1_sm_launch( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &dev->comm;
    struct statemachine_t* sm = &dev->sm;    

    if( sm_get_step(sm) == 0 ){ // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        pws1_reset_cmd(idx);
        pws1_comm_reset(idx);
        sm_set_step(sm, 20);
    }else if( sm_get_step(sm) == 20 ){
        if( comm_get_state(comm) != COMMST_NORMAL){
            log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",__func__, idx,sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, PWS1ERR_LAUNCH_COMMERR);   
        }else if(pws1_get_runstat(idx) != PWS1_RUNSTAT_PCS_INIT && pws1_get_runstat(idx) != PWS1_RUNSTAT_UNKOWNING){
            log_dbg("%s, idx:%d, state:%s, step:%d, comm ok and pcs init finished,goto stdby!!",__func__, idx,sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STDBY, PWS1ERR_NONE);
        }
    }
}

static void pws1_sm_err( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;    
    static double ts_last_try;
    double ts;

    if( sm_get_step(sm) == 0 ){ // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        pws1_reset_cmd(idx);
        sm_set_step(sm, 10);
        ts_last_try = sm_get_timeofday();
    }else if( sm_get_step(sm) == 10 ){ // wait cmd
        ts = sm_get_timeofday();
        if( pws1_get_cmd(idx) == CMD_SM_STDBY){ // stdby cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stdby cmd, reset comm then chk",
                                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            pws1_comm_reset(idx);
            sm_set_step(sm, 20);
         }else if( comm_get_state(comm) != COMMST_NORMAL && (ts - ts_last_try > 60000) ){ // 60s
            log_dbg("%s, idx:%d, state:%s, step:%d,comm:%s,reset comm",
                                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm),comm_get_state_str(comm));
            ts_last_try = ts;
            pws1_comm_reset(idx);
        }
    }else if( sm_get_step(sm) == 20 ){ /* chk comm state */
        if( comm_get_state(comm) == COMMST_NORMAL){
            log_dbg("%s, idx:%d, state:%s, step:%d, comm ok, goto stdby",
                                        __func__, idx,sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STDBY, PWS1ERR_NONE);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, stay err",
                                        __func__, idx,sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, PWS1ERR_ERR_COMMERR);            
        }
    }
}

static void pws1_sm_stdby( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;

    /* chk comm state */ 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_STDBY_COMMERR);
        return;
    }

    if( sm_get_step(sm) == 0 ){ /* entry */
        pws1_reset_cmd(idx);
        sm_set_step(sm, 10);
    }else if( sm_get_step(sm) == 10 ){ /* wait cmd */ 
        if( pws1_get_cmd(idx) == CMD_SM_STOP ){ /* stop cmd */
            log_dbg("%s, idx:%d, state:%s, step:%d, get stop cmd, chk devaps = 0 ?",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            sm_set_step(sm, 20);
        }
    }else if( sm_get_step(sm) == 20 ){ /* chk devaps = 0 ? */
        if( pws1_get_dev_aps(idx) != 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps != 0 detected, set it to 0, wait and chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 30);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps = 0 detected, chk stop state",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }        
    }else if( sm_get_step(sm) == 30 ){ /* wait and chk devaps = 0 */
        if( pws1_get_dev_aps(idx) == 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps = 0 detected, chk stop state",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, chk devaps = 0 fail after set, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, PWS1ERR_STDBY_CHK_DEVAPS0_FAIL_AFTER_SET);
        }
    }else if( sm_get_step(sm) == 40 ){ /* chk runstate = stop ? */
        if( PWS1_RUNSTAT_STOP != pws1_get_runstat(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, non-stop run state detected, send stop cmd, wait and chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_dev_stopcmd(idx);
            sm_set_step(sm, 50);
            sm_set_count(sm, 0);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, stop run state detected, goto stop",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, PWS1ERR_NONE);
        }
    }else if( sm_get_step(sm) == 50 ){ /* wait run state = stop after stopcmd*/
        if( PWS1_RUNSTAT_STOP == pws1_get_runstat(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, stop run state detected, goto stop",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, PWS1ERR_NONE);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100){
                log_dbg("%s, idx:%d, state:%s, step:%d, wait stop run state timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_STDBY_WAIT_STOP_TIMEOUT);
            }else if( sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting stop run state, count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }

        }        
    }
}

static void pws1_sm_offgrid( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;

    // chk comm state 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_COMMERR);
        return;
    }

    if( sm_get_step(sm) == 0 ){ // entry 
        pws1_reset_cmd(idx);
        sm_set_step(sm, 10);
    }else if( sm_get_step(sm) == 10 ){ // wait cmd and chk 
        if( pws1_get_cmd(idx) == CMD_SM_READY ){ // ready cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get ready cmd, set dev stopcmd, goto wait pcs stop",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            pws1_comm_set_dev_stopcmd(idx);
            sm_set_step(sm,20);
            sm_set_count(sm,0);
        }else if( pws1_get_errstat(idx) != 0){
            log_dbg("%s, idx:%d, state:%s, step:%d, err state detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, PWS1ERR_OFFGRID_ERRSTAT_DETECTED);
        }else if( pws1_get_runstat(idx) != PWS1_RUNSTAT_CHG || pws1_get_runstat(idx) != PWS1_RUNSTAT_DHG || pws1_get_runstat(idx) != PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, runstate != DHG||CHG||IDLE,goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, PWS1ERR_OFFGRID_NOT_RUNNING_DETECTED);
        }else{

        }
    }else if( sm_get_step(sm) == 20){
        if(pws1_get_runstat(idx) == PWS1_RUNSTAT_STOP){
            log_dbg("%s, idx:%d, state:%s, step:%d,chk pcs run state stop ok,set pws1 on grid mode",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
            pws1_comm_set_grid_mode(idx,0);
            sm_set_step(sm,21);
            sm_set_count(sm,0);
        }else{
            sm_inc_count(sm);
            if(sm_get_count(sm) > 50){
                log_dbg("%s, idx:%d, state:%s, step:%d,wait pcs stop timeout,goto err",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
                sm_set_state(sm,SMST_ERR,PWS1ERR_OFFGRID_WAIT_STOP_TIME_OUT);
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d,wait pcs goto stop run mode",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
            }
        }
    }else if( sm_get_step(sm) == 21){
        if(pws1_get_runmod(idx) == 0){
            log_dbg("%s, idx:%d, state:%s, step:%d,chk pcs run mode == ongride",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
            if(pws1_get_runstat(idx) == PWS1_RUNSTAT_IDLE){
                log_dbg("%s, idx:%d, state:%s, step:%d,chk pcs runstate == idle,goto ready",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
                sm_set_state(sm, SMST_READY, PWS1ERR_NONE);
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d,chk pcs runstate != idle,goto err",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
                sm_set_state(sm,SMST_ERR,PWS1ERR_OFFGRID_CHK_RUNSTATE_NOT_IDLE_AFTER_SET_ONGRIDE);
            }
        }else{
            sm_inc_count(sm);
            if(sm_get_count(sm) > 50){
                log_dbg("%s, idx:%d, state:%s, step:%d,wait pcs goto on gride mode time out,goto err",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
                sm_set_state(sm,SMST_ERR,PWS1ERR_OFFGRID_WAIT_RUN_MODE_ON_GIRD_TIME_OUT);
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d,wait pcs goto on gride mode",__func__,idx,sm_get_szstate(sm),sm_get_step(sm));
            }
        }
    }   
}

static void pws1_sm_stop( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;

    /* chk comm state */ 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_COMMERR);
        return;
    }

    if( sm_get_step(sm) == 0 ){ // entry 
        pws1_reset_cmd(idx);
        pws1_set_aps(idx, 0);
        sm_set_step(sm, 10);
    }else if( sm_get_step(sm) == 10 ){ // wait cmd and chk 
        if( pws1_get_cmd(idx) == CMD_SM_STDBY ){ // stdby cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stdby cmd, goto stdby",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            sm_set_state(sm, SMST_STDBY, PWS1ERR_NONE);
        }else if( pws1_get_cmd(idx) == CMD_SM_READY ){ // ready cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get ready cmd, set dev resetcmd, chk dev aps",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            sm_set_step(sm, 20);
            pws1_comm_set_dev_resetcmd(idx);
        }else{ /* no cmd, do chking */
            if( PWS1_RUNSTAT_STOP != pws1_get_runstat(idx) ){
                log_dbg("%s, idx:%d, state:%s, step:%d, run state != stop detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_NONE_STOP_DETECTED);
            }            
        }
    }else if( sm_get_step(sm) == 20 ){ /* chk devaps = 0 ? */
        if( pws1_get_dev_aps(idx) != 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps != 0 detected, set it to 0, then chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 21);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps = 0 detected, goto check runmode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 30);
        }        
    }else if( sm_get_step(sm) == 21 ){ /* wait and chk devaps = 0 */
        if( pws1_get_dev_aps(idx) == 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps = 0 detected,goto check runmode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 30);
        }else{
            sm_inc_count(sm);
            if(sm_get_count(sm) > 50){
                log_dbg("%s, idx:%d, state:%s, step:%d, chk wait aps=0 time out, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_CHK_DEVAPS0_FAIL_AFTER_SET);
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, chk waiting aps=0",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                //wait
            } 
        }
    }else if( sm_get_step(sm) == 30 ){ // chk pcs runmod == ongrid
        if( PWS1_RUNMOD_ONGRID == pws1_get_runmod(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs runmod == ongrid ok, goto check dispatch mode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs runmod != ongrid, set pcs ongrid mode and got check",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_grid_mode(idx,0);
            sm_set_step(sm,31);
            sm_set_count(sm,0);
        }        
    }else if( sm_get_step(sm) == 31){
        if( pws1_get_runmod(idx) == PWS1_RUNMOD_ONGRID){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs runmod == ongrid ok, goto check dispatch mode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }else{
            sm_inc_count(sm);
            if(sm_get_count(sm) > 50){
                log_dbg("%s, idx:%d, state:%s, step:%d, wait runmod == ongrid time out, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_CHK_ONGRID_FAIL_AFTER_SET);
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting runmod == ongrid",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                //wait
            } 
        }
    }else if( sm_get_step(sm) == 40 ){ // chk pcs dispatching mode == ac dispatching
        if( PWS1_DISPATCH_MODE_AC == pws1_get_dispatch_mode(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs dispatch mode == AC, goto chk power control mode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,50);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, set pcs dispatch mode = ac and goto check",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_dispatching_mode(idx,0);
            sm_set_step(sm,41);
            sm_set_count(sm,0);
        }        
    }else if( sm_get_step(sm) == 41){
        if( PWS1_DISPATCH_MODE_AC == pws1_get_dispatch_mode(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs dispatch mode == AC, goto chk power control mode",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,50);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 50 ){ /* 2.5s */
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_CHK_DISPATCH_AC_FAIL_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dispatch mode == ac time out goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting dispatch mode == ac count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        } 
    }else if( sm_get_step(sm) == 50 ){ // chk pcs dispatching mode == ac dispatching
        if( PWS1_AC_POWER_CONTROL_MODE_FIX_AC_POWER == pws1_get_active_power_control_mode(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs ac power_control == fix ac power, goto chk run state",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,60);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, set pcs ac power_control == fix ac power and goto check",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_active_power_control_mode(idx,0);
            sm_set_step(sm,51);
            sm_set_count(sm,0);
        }        
    }else if( sm_get_step(sm) == 51){
        if( PWS1_AC_POWER_CONTROL_MODE_FIX_AC_POWER == pws1_get_active_power_control_mode(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs ac power_control == fix ac power, goto chk run state",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,60);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 50 ){ /* 2.5s */
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_CHK_AC_CONTROL_MODEL_FIX_AC_POWER_FIAL_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ac power_control == fix ac power time out goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting ac power_control == fix ac power count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        } 
    }else if( sm_get_step(sm) == 60){
        if(pws1_get_runstat(idx) == PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, set pcs running state == idle and goto ready",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_READY, PWS1ERR_NONE);
        }else{
            log_dbg("%s, idx:%d, state:%s, step:%d, set pcs running state,and goto chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_dev_startcmd(idx);
            sm_set_step(sm,61);
            sm_set_count(sm,0);
        }
    }else if( sm_get_step(sm) == 61){
        if(pws1_get_runstat(idx) == PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, set pcs running state == idle and goto ready",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_READY, PWS1ERR_NONE);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 50 ){ /* 2.5s */
                sm_set_state(sm, SMST_ERR, PWS1ERR_STOP_CHK_STATE_RUNNING_FAIL_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait runstate == idle time out goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting runstate == idle count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }  
}


static void pws1_sm_ready( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;
    int aps = pws1_get_aps(idx);
    int ap = pws1_get_ap(idx);

    /* chk comm state */ 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_READY_COMMERR);
        return;
    }

    if( sm_get_step(sm) == 0 ){ // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        pws1_reset_cmd(idx);
        pws1_reset_aps(idx);
        sm_set_step(sm, 10);
    }else if( sm_get_step(sm) == 10 ){ // wait and chk
        if( pws1_get_cmd(idx) == CMD_SM_STOP){ // stop cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stop cmd, dev stop cmd sent, wait and chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            pws1_comm_set_dev_stopcmd(idx);
            sm_set_step(sm, 20);
            sm_set_count(sm, 0);
        }else if( aps > 0 ){ // new aps, prepare to dhg
            log_dbg("%s, idx:%d, state:%s, step:%d, aps > 0 detected, %d, set dev aps, wait and chk",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            pws1_comm_set_active_power(idx,(double)(-aps));                                       
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);                                        
        }else if( aps < 0 ){ // new aps, prepare to chg
            log_dbg("%s, idx:%d, state:%s, step:%d, aps < 0 detected, %d, set dev aps, wait and chk",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            pws1_comm_set_active_power(idx,(double)(-aps));                                       
            sm_set_step(sm, 40);
            sm_set_count(sm, 0);    
        }else if( pws1_get_cmd(idx) == CMD_SM_OFFGRID ){ // offgrid cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get offgrid cmd, set dev run mode = offgrid, wait and chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_reset_cmd(idx);
            pws1_comm_set_dev_stopcmd(idx);                                       
            sm_set_step(sm, 500);
            sm_set_count(sm, 0);
        }else if( pws1_get_runstat(idx) != PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != running detected, %d, goto err",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), pws1_get_runstat(idx));
            sm_set_state(sm, SMST_ERR, PWS1ERR_READY_RUNSTATE_NOT_RUN_DETECTED);                                        
        }
    }else if( sm_get_step(sm) == 20){ /* wait and chk run state = stop */
        if( pws1_get_runstat(idx) == PWS1_RUNSTAT_STOP ){
            log_dbg("%s, idx:%d, state:%s, step:%d, dev run state = stop detected, goto stop",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, PWS1ERR_NONE);            
        }else{
            sm_inc_count(sm);
            if(sm_get_count(sm) >= 100){
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev run state = stop timeout, goto err",
                                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_STOP_TIMEOUT);                  
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev run state = stop, count:%d",
                                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 30 ){ // chk ap > 0
        if( ap > 0 ){ /* chk ok */
            log_dbg("%s, idx:%d, state:%s, step:%d, ap = %d > 0 detected, goto check run state",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
            sm_set_step(sm,31);
            sm_set_count(sm,0);                           
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_AP4DHG_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap > 0 for dhg timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if(sm_get_count(sm)%10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap > 0 for dhg, count:%d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 31){
        if( pws1_get_runstat(idx) == PWS1_RUNSTAT_DHG){
            log_dbg("%s, idx:%d, state:%s, step:%d,check dhg state ok,goto dhg",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_DHG, PWS1ERR_NONE); 
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_DHG_STATE_TIME_OUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dhg state time out, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if(sm_get_count(sm)%10 == 0){
                
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting dhg state time out, count:%d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 40 ){ // chk ap for chg
        if( ap < 0 ){ /* chk ok */
            log_dbg("%s, idx:%d, state:%s, step:%d, ap = %d < 0 detected, goto chk run state",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
            sm_set_step(sm,41);
            sm_set_count(sm,0); 
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_AP4CHG_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap < 0 for chg timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap < 0 for chg, count:%d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 41){
        if( pws1_get_runstat(idx) == PWS1_RUNSTAT_CHG){
            log_dbg("%s, idx:%d, state:%s, step:%d,check chg state ok,goto chg",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_CHG, PWS1ERR_NONE); 
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_CHG_STATE_TIME_OUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait chg state time out, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if(sm_get_count(sm)%10 == 0){
                
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting chg state time out, count:%d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 500 ){ // wait run mode = offgrid for offgrid
        if( PWS1_RUNSTAT_STOP == pws1_get_runstat(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk run state = stop ok ,set offgrid mode and goto chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_grid_mode(idx,1);
            sm_set_step(sm, 501);
            sm_set_count(sm, 0);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){ // 5s
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_RUNSTATE_STOP_TIMEOUT_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait run state = stop timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if( sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting run state = stop, count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }else{
                //wait
            }
        }   
    }else if( sm_get_step(sm) == 501 ){
        if( PWS1_RUNMOD_OFFGRID == pws1_get_runmod(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk run mode = offgrid ok ,set pcs start and goto chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_dev_startcmd(1);
            sm_set_step(sm, 502);
            sm_set_count(sm, 0);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){ // 5s
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_RUNMODE_OFFGRID_TIMEOUT_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait run mode = offgrid timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if( sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting run mode = offgrid, count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }else{
                //wait
            }
        }
    }else if( sm_get_step(sm) == 502 ){
        if( PWS1_RUNSTAT_IDLE == pws1_get_runstat(idx) || PWS1_RUNSTAT_CHG == pws1_get_runstat(idx) || PWS1_RUNSTAT_DHG == pws1_get_runstat(idx)){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk run state = idl||chg||dhg ok ,goto chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 503);
            sm_set_count(sm, 0);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){ // 5s
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_RUNSTATE_RUNING_TIMEOUT_AFTER_SET);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait run state = idl||chg||dhg timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if( sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting run state = idl||chg||dhg, count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }else{
                //wait
            }
        }

    }else if( sm_get_step(sm) == 503){ // chk voltage for offgrid
        if( pws1_get_uab(idx) > 340.0 && pws1_get_uab(idx) < 420.0){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk voltage ok, %.1f, goto offgrid",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), pws1_get_uab(idx));
            sm_set_state(sm, SMST_OFFGRID, PWS1ERR_NONE);  
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 300 ){ // 15s
                log_dbg("%s, idx:%d, state:%s, step:%d, wait voltage to 380 for offgrid timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, PWS1ERR_READY_WAIT_TARGET_VOLTAGE_FOR_OFFGRID_TIMEOUT);                
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, wait target voltage for offgrid, count:%d",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }      
    }
}

static void pws1_sm_dhg( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;
    int aps = pws1_get_aps(idx);
    int last_aps = pws1_get_last_aps(idx);
    int ap = pws1_get_ap(idx);

    /* chk comm state */ 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_DHG_COMMERR);
        return;
    }  

    if( sm_get_step(sm) == 0 ){ // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));    
        sm_set_step(sm, 10);
        pws1_reset_bsytikchk(idx);
    }else if( sm_get_step(sm) == 10 ){ // wait and chk
        if( pws1_get_cmd(idx) == CMD_SM_READY ){ // ready cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, got ready cmd, set dev aps = 0, wait and chk", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));            
            pws1_reset_cmd(idx);
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);        
        }else if( aps <= 0 ){ // aps <= 0
            log_dbg("%s, idx:%d, state:%s, step:%d, aps <= 0 detected, %d, set dev aps = 0, wait and chk",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);    
        }else if( pws1_is_aps_changed(idx) ){ // new aps, set devaps and chk
            log_dbg("%s, idx:%d, state:%s, step:%d, new aps detected, %d -> %d, set dev aps",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), last_aps, aps);
            pws1_set_aps(idx, aps); // update last aps
            pws1_comm_set_active_power(idx, (double)(-aps));
        }else if( pws1_is_bsytikchk_timeout(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, bsytik timeout detected, set dev aps to 0", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_set_aps(idx, 0);             
        }else if( pws1_get_runstat(idx) != PWS1_RUNSTAT_DHG ){
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != dhg detected, set dev aps to 0, goto err", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_active_power(idx, 0);
            sm_set_state(sm, SMST_ERR, PWS1ERR_DHG_STATE_NOT_RUN_DETECTED); 
        }
    }else if( sm_get_step(sm) == 30 ){ // chk dev aps = 0 for readycmd
        if( pws1_get_dev_aps(idx) == 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk dev aps = 0 ok, goto chk dhg state",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,31);
            sm_set_count(sm,0);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_DHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev aps = 0 for readycmd timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev aps = 0 for readycmd, count: %d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 31){
        if(pws1_get_runstat(idx) == PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk in idle state, goto ready",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm,SMST_READY,PWS1ERR_NONE); 
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_DHG_WAIT_DEV_NO_DHG_TIME_OUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev no dhg timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting no dhg for readycmd, count: %d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }
}

static void pws1_sm_chg( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct comm_t* comm = &pws1[idx].comm;
    struct statemachine_t* sm = &pws1[idx].sm;
    int aps = pws1_get_aps(idx);
    int last_aps = pws1_get_last_aps(idx);
    int ap = pws1_get_ap(idx);

    /* chk comm state */ 
    if( comm_get_state(comm) != COMMST_NORMAL){ 
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, PWS1ERR_CHG_COMMERR);
        return;
    }  

    if( sm_get_step(sm) == 0 ){ // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));   
        sm_set_step(sm, 10);
        pws1_reset_bsytikchk(idx);
    }else if( sm_get_step(sm) == 10 ){ // wait and chk
        if( pws1_get_cmd(idx) == CMD_SM_READY ){
            log_dbg("%s, idx:%d, state:%s, step:%d, got ready cmd, set dev aps = 0", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));            
            pws1_reset_cmd(idx);
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);      
        }else if( aps >= 0 ){ // aps => 0
            log_dbg("%s, idx:%d, state:%s, step:%d, aps >= 0 detected, %d, set dev aps = 0, wait and chk",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            pws1_comm_set_active_power(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);    
        }else if( pws1_is_aps_changed(idx) ){ /* new aps, set devaps and chk */
            log_dbg("%s, idx:%d, state:%s, step:%d, new aps detected, %d -> %d, set dev aps",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), last_aps, aps);
            pws1_set_aps(idx, aps); // update last aps
            pws1_comm_set_active_power(idx, (-aps));
        }else if( pws1_is_bsytikchk_timeout(idx) ){
            log_dbg("%s, idx:%d, state:%s, step:%d, bsytik timeout detected, set dev aps to 0", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_set_aps(idx, 0);             
        }else if( pws1_get_runstat(idx) != PWS1_RUNSTAT_CHG ){
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != chg detected, set dev aps to 0, goto err", 
                                            __func__,idx, sm_get_szstate(sm), sm_get_step(sm));
            pws1_comm_set_active_power(idx, 0);
            sm_set_state(sm, SMST_ERR, PWS1ERR_CHG_STATE_NOT_RUN_DETECTED); 
        }
    }else if( sm_get_step(sm) == 30 ){ // chk dev aps = 0 for readycmd
        if( pws1_get_dev_aps(idx) == 0 ){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk dev aps = 0 ok, goto chk chg state",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm,31);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_CHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev aps = 0 for readycmd timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else{
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev aps = 0 for readycmd, count: %d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }else if( sm_get_step(sm) == 31){
        if(pws1_get_runstat(idx) == PWS1_RUNSTAT_IDLE){
            log_dbg("%s, idx:%d, state:%s, step:%d, chk in idle state, goto ready",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm,SMST_READY,PWS1ERR_NONE);
        }else{
            sm_inc_count(sm);
            if( sm_get_count(sm) >= 100 ){
                sm_set_state(sm, SMST_ERR, PWS1ERR_CHG_WAIT_DEV_NO_CHG_TIME_OUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev no chg timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }else if(sm_get_count(sm) % 10 == 0){
                log_dbg("%s, idx:%d, state:%s, step:%d, waiting no chg for readycmd, count: %d",
                                                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
            }
        }
    }
}

void pws1_sm( int idx )
{
    struct pws1_t* dev = &pws1[idx];
    struct statemachine_t* sm = &dev->sm;

    sm_cal_timing(&dev->sm);

    switch( sm_get_state( sm ) ){
        case SMST_LAUNCH:
            pws1_sm_launch( idx );
            break;

        case SMST_STDBY:
            pws1_sm_stdby( idx );
            break;

        case SMST_STOP:
            pws1_sm_stop( idx );
            break;

        case SMST_OFFGRID:
            pws1_sm_offgrid( idx );
            break;            

        case SMST_READY:
            pws1_sm_ready( idx );
            break;

        case SMST_DHG:
            pws1_sm_dhg( idx );
            break;

        case SMST_CHG:
            pws1_sm_chg( idx );
            break;

        case SMST_ERR:
            pws1_sm_err( idx );
            break;            

        default:
            log_dbg("%s, unknown state, %d",__func__, pws1_get_state( idx ));
            break;
    }
}