#include "MotorCtrl.h"

#include "hwCtrl.h"
#include "PINdef.h"
#include "appTask.h"

static uint8_t amCtrl_state = 0,am_state_machine = 0;
static uint16_t am_state_machine_counter = 0;
const uint8_t am_state_machine_arr[3][3] = {
   //0,1,2
    {1,2,0},
    {2,0,1},
    {1,2,0},
};


void amCtrl_Init(void)
{
    amCtrl_state = 0;
    am_state_machine_counter = 0;
}

void amCtrl_SetState(uint8_t state)
{
    amCtrl_state = state;
}

void amCtrl_Task(void)//50ms
{
    if (amCtrl_state == 0)
    {
        setPumpState(PUMP_OFF);
        setValveState(VALVE3,VALVE_OFF);
        setValveState(VALVE4,VALVE_OFF);
        setValveState(VALVE5,VALVE_OFF);
        am_state_machine = 0;
        am_state_machine_counter = 0;
    }
    else
    {
        setPumpState(PUMP_ON);
        switch (am_state_machine)
        {
        case 0:
            setValveState(VALVE3,VALVE_ON);
            setValveState(VALVE4,VALVE_OFF);
            setValveState(VALVE5,VALVE_OFF);
            am_state_machine_counter++;
            if (am_state_machine_counter >= 60)
            {
                am_state_machine_counter = 0;
                am_state_machine = am_state_machine_arr[amCtrl_state-1][am_state_machine];
            }
            break;
        
        case 1:
            setValveState(VALVE3,VALVE_OFF);
            setValveState(VALVE4,VALVE_ON);
            setValveState(VALVE5,VALVE_OFF);
            am_state_machine_counter++;
            if (am_state_machine_counter >= 60)
            {
                am_state_machine_counter = 0;
                am_state_machine = am_state_machine_arr[amCtrl_state-1][am_state_machine];
            }
            break;
            
        case 2:
            setValveState(VALVE3,VALVE_OFF);
            setValveState(VALVE4,VALVE_OFF);
            setValveState(VALVE5,VALVE_ON);
            am_state_machine_counter++;
            if (am_state_machine_counter >= 70)
            {
                am_state_machine_counter = 0;
                am_state_machine = am_state_machine_arr[amCtrl_state-1][am_state_machine];
            }
        default:
            break;
        }
    }
    
    
}