#include "MotorCtrl.h"
#include "hwctrl.h"
#include "scm_canmatrix-binutil.h"
/*******************************************************************************
 * the defines
 ******************************************************************************/
#define START_FLAG  0x55aa
#define STOP_FLAG   0xaa55

#define AUTOCAL_STOP    0
#define AUTOCAL_START   1
#define AUTOCAL_ACT1    2
#define AUTOCAL_ACT2    3
#define AUTOCAL_END     4

#define OC_10A  100
#define OC_30A  300
#define OC_500mS   500
#define OC_50mS   50

#define OC_TIME_200MS 200

#define HALLDELAYMAX 2
/*******************************************************************************
 * the typedefs
 ******************************************************************************/

typedef struct 
{
    uint16_t MotorStopLoc1;
    uint16_t MotorStopLoc2;
    uint16_t MotorNowLoc;
}MOTOR_DATA;

typedef struct 
{
    uint16_t start_flag;
    MOTOR_DATA MotorData[6];
    uint16_t MemoryLoc[3][6];
    uint16_t checksum;
    uint16_t stop_flag;
}MEMORY_DATA;

/*******************************************************************************
 * the globals
 ******************************************************************************/

MOTOR_DATA MotorData[6];
uint16_t MemoryLoc[3][6];
MEMORY_DATA MemoryData;
uint8_t OC1flag,OC2flag,OC3flag;
uint16_t OC1Count,OC2Count,OC3Count;
uint8_t MotorState[6],MotorStateReal[6];
uint8_t MotorHallIO[6];
uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
uint8_t MotorLearnState[6];
uint8_t MotorErr[6];
uint16_t MotorTarget[6] = {0};
uint16_t current1,current2,current3;
uint8_t stopflag = 0;
static uint8_t AutoCalState;
uint16_t HallErrorCount[6];

/*******************************************************************************
 * the const
 ******************************************************************************/

/*******************************************************************************
 * the local function 
 ******************************************************************************/
static void MotorCtrl(void);
static void AutoCalCtrl(void);
static void MotorValueInit(void);

static uint16_t getOverCurrentTh(uint8_t ch);

/*******************************************************************************
 * the local functions
 ******************************************************************************/
static void MotorValueInit(void)
{
    uint8_t i;
    for (i = 0; i < 6; i++)
    {
        MotorHallLoc[i] = 0x8000;
        MotorErr[i] = 0;
    }
    //ReadMotorMemory();
}

static void AutoCalCtrl(void)
{
    static uint16_t autocalcounter[3];
    static uint8_t MotorArr1state,MotorArr2state,MotorArr3state;
    static uint8_t wait1,wait2,wait3;
    uint8_t i;
    uint32_t temp;
    switch (AutoCalState)
    {
    case AUTOCAL_STOP:
        MotorArr1state = AUTOCAL_STOP;
        MotorArr2state = AUTOCAL_STOP;
        MotorArr3state = AUTOCAL_STOP;
        return;
    case AUTOCAL_START:
        MotorArr1state = 1;
        MotorArr2state = 1;
        MotorArr3state = 1;
        setMotorState(MotorHG,Motor_ACT_CW);
        setMotorState(MotorTT,Motor_ACT_CW);
        setMotorState(Motor5,Motor_ACT_CW);
        autocalcounter[0] = 0;
        autocalcounter[1] = 0;
        autocalcounter[2] = 0;
        AutoCalState++;
        for (i = 0; i < 6; i++)
        {
            MotorHardStop1[i] = 0;
            MotorHardStop2[i] = 0;
            MotorHallLoc[i] = 0x8000;
        }
        break;
    case AUTOCAL_ACT1:
        if (MotorArr1state == 0 && MotorArr2state == 0 && MotorArr3state == 0)
        {
            for (i = 0; i < 6; i++)
            {
                temp = MotorHardStop1[i];
                temp += MotorHardStop2[i];
                setMotorTarget(i,temp/2);
            }
            
        }
        
        
        
        break;
    case AUTOCAL_ACT2:

        break;
    
    default:
        break;
    }
    //1
    switch (MotorArr1state)
    {
    case 1://Motor1 xq
        autocalcounter[0]++;
        if (MotorHardStop1[MotorHG] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorHG] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        break;
    case 2://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorHG,Motor_ACT_CCW);
            autocalcounter[0] = 0;
        }
        break;
    case 3://Motor1 xh 
        autocalcounter[0]++;
        if (MotorHardStop2[MotorHG] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorHG] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        break;
    case 4://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorKB,Motor_ACT_CW);
            autocalcounter[0] = 0;
        }
        break;
    case 5://Motor2 xq
        autocalcounter[0]++;
        if (MotorHardStop1[MotorKB] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorKB] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        break;
    case 6://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorKB,Motor_ACT_CCW);
            autocalcounter[0] = 0;
        }
        break;
    case 7:
        autocalcounter[0]++;
        if (MotorHardStop2[MotorKB] != 0)
        {
            MotorArr1state=0;
            wait1 = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorKB] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        break;
    default:
        break;
    }
    
    //2
     switch (MotorArr2state)
    {
    case 1://Motor3 xq
        autocalcounter[1]++;
        if (MotorHardStop1[MotorTT] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
	    setMotorState(MotorTT,Motor_ACT_NOACT);	
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorTT] != 0)
        {
            MotorArr2state = 4;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        break;
    case 2://wait
        wait2++;
        if (wait2 > 50)
        {
            MotorArr2state++;
            setMotorState(MotorTT,Motor_ACT_CCW);
            autocalcounter[1] = 0;
        }
        break;
    case 3://Motor3 xh 
        autocalcounter[1]++;
        if (MotorHardStop2[MotorTT] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorTT] != 0)
        {
            MotorArr2state = 4;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        break;
    case 4://wait
        wait2++;
        if (wait2 > 50)
        {
	        MotorArr2state++;
            setMotorState(MotorZY,Motor_ACT_CW);
            autocalcounter[1] = 0;
        }
        break;
    case 5://Motor4 xq
        autocalcounter[1]++;
        if (MotorHardStop1[MotorZY] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorZY] != 0)
        {
            MotorArr2state = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        break;
    case 6://wait
        wait2++;
        if (wait2 > 50)
        {
            MotorArr2state++;
            setMotorState(MotorZY,Motor_ACT_CCW);
            autocalcounter[1] = 0;
        }
        break;
    case 7:
        autocalcounter[1]++;
        if (MotorHardStop2[MotorZY] != 0)
        {
            MotorArr2state=0;
            wait2 = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorZY] != 0)
        {
            MotorArr2state = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        break;
    default:
        break;
    }
    switch (MotorArr3state)
    {
    case 1://Motor5 xq
        autocalcounter[2]++;
        if (MotorHardStop1[Motor5] != 0)
        {
            MotorArr3state++;
            wait3 = 0;
	    setMotorState(Motor5,Motor_ACT_NOACT);
        }
        if (autocalcounter[2] > 3000 || MotorErr[Motor5] != 0)
        {
            MotorArr3state = 4;
            wait3 = 0;
            setMotorState(Motor5,Motor_ACT_NOACT);
        }
        break;
    case 2://wait
        wait3++;
        if (wait3 > 50)
        {
            MotorArr3state++;
            setMotorState(Motor5,Motor_ACT_CCW);
            autocalcounter[2] = 0;
        }
        break;
    case 3://Motor5 xh 
        autocalcounter[2]++;
        if (MotorHardStop2[Motor5] != 0)
        {
            MotorArr3state++;
            wait3 = 0;
            setMotorState(Motor5,Motor_ACT_NOACT);
        }
        if (autocalcounter[2] > 3000 || MotorErr[Motor5] != 0)
        {
            MotorArr3state = 4;
            wait3 = 0;
            setMotorState(Motor5,Motor_ACT_NOACT);
        }
        break;
    case 4://wait
        wait3++;
        if (wait3 > 50)
        {
            MotorArr3state++;
            setMotorState(Motor6,Motor_ACT_CW);
            autocalcounter[2] = 0;
        }
        break;
    case 5://Motor6 xq
        autocalcounter[2]++;
        if (MotorHardStop1[Motor6] != 0)
        {
            MotorArr3state++;
            wait3 = 0;
            setMotorState(Motor6,Motor_ACT_NOACT);
        }
        if (autocalcounter[2] > 3000 || MotorErr[Motor6] != 0)
        {
            MotorArr3state = 0;
            setMotorState(Motor6,Motor_ACT_NOACT);
        }
        break;
    case 6://wait
        wait3++;
        if (wait3 > 50)
        {
            MotorArr3state++;
            setMotorState(Motor6,Motor_ACT_CCW);
            autocalcounter[2] = 0;
        }
        break;
    case 7:
        autocalcounter[2]++;
        if (MotorHardStop2[Motor6] != 0)
        {
            MotorArr3state=0;
            wait3 = 0;
            setMotorState(Motor6,Motor_ACT_NOACT);
        }
        if (autocalcounter[2] > 3000 || MotorErr[Motor6] != 0)
        {
            MotorArr3state = 0;
            setMotorState(Motor6,Motor_ACT_NOACT);
        }
        break;
    default:
        break;
    }
}

static void MotorCtrl(void)//10ms
{
    uint8_t i;
    //WriteMotorMemory(); //TODO
    AutoCalCtrl();
    
    
    if (OC1flag == 1)
    {
        OC1flag = 0;
        if (MotorState[0] == Motor_ACT_CW)
        {
            MotorHardStop1[0] = MotorHallLoc[0];
        }
        else if (MotorState[0] == Motor_ACT_CCW)
        {
            MotorHardStop2[0] = MotorHallLoc[0];
        }
        else if (MotorState[1] == Motor_ACT_CW)
        {
            MotorHardStop1[1] = MotorHallLoc[1];
        }
        else if (MotorState[1] == Motor_ACT_CCW)
        {
            MotorHardStop2[1] = MotorHallLoc[1];
        }
        setMotorTarget(MotorHG,0);
        setMotorTarget(MotorKB,0);
        MotorState[MotorHG] = Motor_ACT_NOACT;
        MotorState[MotorKB] = Motor_ACT_NOACT;
    }
    if (OC2flag == 1)
    {
        OC2flag = 0;
        if (MotorState[2] == Motor_ACT_CW)
        {
            MotorHardStop1[2] = MotorHallLoc[2];
        }
        else if (MotorState[2] == Motor_ACT_CCW)
        {
            MotorHardStop2[2] = MotorHallLoc[2];
        }
        else if (MotorState[3] == Motor_ACT_CW)
        {
            MotorHardStop1[3] = MotorHallLoc[3];
        }
        else if (MotorState[3] == Motor_ACT_CCW)
        {
            MotorHardStop2[3] = MotorHallLoc[3];
        }
        setMotorTarget(MotorTT,0);
        setMotorTarget(MotorZY,0);
        MotorState[MotorTT] = Motor_ACT_NOACT;
        MotorState[MotorZY] = Motor_ACT_NOACT;
    }
    if (OC3flag == 1)
    {
        OC3flag = 0;
        if (MotorState[4] == Motor_ACT_CW)
        {
            MotorHardStop1[4] = MotorHallLoc[4];
        }
        else if (MotorState[4] == Motor_ACT_CCW)
        {
            MotorHardStop2[4] = MotorHallLoc[4];
        }
        else if (MotorState[5] == Motor_ACT_CW)
        {
            MotorHardStop1[5] = MotorHallLoc[5];
        }
        else if (MotorState[5] == Motor_ACT_CCW)
        {
            MotorHardStop2[5] = MotorHallLoc[5];
        }
        setMotorTarget(Motor5,0);
        setMotorTarget(Motor6,0);
        MotorState[Motor5] = Motor_ACT_NOACT;
        MotorState[Motor6] = Motor_ACT_NOACT;
    }

    for (i = 0; i < 6; i++)
    {
        MotorStateReal[i] = MotorState[i];
        if (MotorState[i] == Motor_ACT_NOACT  && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 )
        {
            if (MotorTarget[i]!=0 && MotorHardStop1[i] > MotorTarget[i] && MotorTarget[i] > MotorHardStop2[i])
            {
                if (MotorTarget[i] > MotorHallLoc[i]+10)
                {
                    MotorStateReal[i] = Motor_ACT_CW;
                }
                else if (MotorTarget[i] < MotorHallLoc[i]-10)
                {
                    MotorStateReal[i] = Motor_ACT_CCW;
                }
                else
                {
                    MotorTarget[i] = 0;
                }
            }
            
        }
        else if (MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0)
        {
            if (MotorHallLoc[i] > (MotorHardStop1[i]-20) && MotorStateReal[i] == Motor_ACT_CW)
            {
                MotorStateReal[i] = Motor_ACT_NOACT;
            }
            if (MotorHallLoc[i] < (MotorHardStop2[i] + 20) && MotorStateReal[i] == Motor_ACT_CCW)
            {
                MotorStateReal[i] = Motor_ACT_NOACT;
            }
            
        }
        
        else
        {
            MotorTarget[i] = 0;
        }
    }
    
    
}

#define OC_DELAY_TIME   200
void CurrentDetect(void)
{
    uint32_t oc_th,current;
    oc_th = getOverCurrentTh(0);
    current = GetAdcmv(ADCH_RLY1)/10;
    if (current > oc_th)
    {
        OC1Count++;
        if (OC1Count > OC_DELAY_TIME)
        {
            OC1Count = 0;
            OC1flag = 1;
        }
    }
    else
    {
        OC1Count = 0;
    }

    oc_th = getOverCurrentTh(1);
    current = GetAdcmv(ADCH_RLY3)/10;
    if (current > oc_th)
    {
        OC2Count++;
        if (OC2Count > OC_DELAY_TIME)
        {
            OC2Count = 0;
            OC2flag = 1;
        }
    }
    else
    {
        OC2Count = 0;
    }
    
}
/*******************************************************************************
 * the global functions
 ******************************************************************************/
void MotorCtrl_Init(void)
{
    for (uint8_t i = 0; i < 6; i++)
    {
        hw_MotorCtrl(i, Motor_ACT_NOACT);
    }
    MotorValueInit();
}

static void SetMotorMsg(void)
{
    scm_canmatrix_tx.SCM_STATE.MOTOR_HG_STATE = MotorState[MotorHG];
    scm_canmatrix_tx.SCM_STATE.MOTOR_KB_STATE = MotorState[MotorKB];
    scm_canmatrix_tx.SCM_STATE.MOTOR_TT_STATE = MotorState[MotorTT];
    scm_canmatrix_tx.SCM_STATE.MOTOR_ZY_STATE = MotorState[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STATUS  = MotorStateReal[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STATUS  = MotorStateReal[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STATUS  = MotorStateReal[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STATUS  = MotorStateReal[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_LOC = MotorHallLoc[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_LOC = MotorHallLoc[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_LOC = MotorHallLoc[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_LOC = MotorHallLoc[MotorZY];
    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STOP_1 = MotorHardStop1[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STOP_2 = MotorHardStop2[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STOP_1 = MotorHardStop1[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STOP_2 = MotorHardStop2[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STOP_1 = MotorHardStop1[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STOP_2 = MotorHardStop2[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STOP_1 = MotorHardStop1[MotorZY];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STOP_2 = MotorHardStop2[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_HALLIO = MotorHallIO[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_HALLIO = MotorHallIO[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_HALLIO = MotorHallIO[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_HALLIO = MotorHallIO[MotorZY];
}
void MotorCtrl_Maintask(void)//10ms task
{
    
    MotorCtrl();
    
    for (Motor_ID_Type i = 0; i < MOTOR_NUM; i++)
    {
        hw_MotorCtrl( i, MotorStateReal[i]);
    }
    SetMotorMsg();
}

void setMotorState(Motor_ID_Type motorid,Motor_ACT_Type act)
{
    if (act < Motor_ACT_NUM && motorid < MOTOR_NUM)
    {
        MotorState[motorid] = act;
    }
}

void setMotorTarget(uint8_t motorid,uint16_t target)
{
    if (MotorHardStop1[motorid]!=0 && MotorHardStop2[motorid]!=0)
    {
        MotorTarget[motorid] = target;
    }
    else
    {
        MotorTarget[motorid] = 0;
    }
}

void StartAutoCal(void)
{
    AutoCalState = AUTOCAL_START;
    
}
void StopAutoCal(void)
{
    uint8_t i;
    if (AutoCalState != AUTOCAL_STOP)
    {
        AutoCalState = AUTOCAL_STOP;
        setMotorState(MotorHG,Motor_ACT_NOACT);
        setMotorState(MotorKB,Motor_ACT_NOACT);
        setMotorState(MotorTT,Motor_ACT_NOACT);
        setMotorState(MotorZY,Motor_ACT_NOACT);
        setMotorState(Motor5,Motor_ACT_NOACT);
        setMotorState(Motor6,Motor_ACT_NOACT);
    }
    for (i = 0; i < 6; i++)
    {
        if (MotorTarget[i] != 0)
        {
            MotorTarget[i] = 0;
            stopflag = 1;
        }
        
    }
    
}






static uint16_t getOverCurrentTh(uint8_t ch)
{
    uint16_t th = 50;
    switch (ch)
    {
    case 0:
        if (MotorStateReal[MotorHG]!=Motor_ACT_NOACT)
        {
            th+=50;
        }
        if (MotorStateReal[MotorKB]!=Motor_ACT_NOACT)
        {
            th+=50;
        }
        break;
    case 1:
        if (MotorStateReal[MotorTT]!=Motor_ACT_NOACT)
        {
            th+=50;
        }
        if (MotorStateReal[MotorZY]!=Motor_ACT_NOACT)
        {
            th+=50;
        }
        break;
    case 2:
        if (MotorStateReal[Motor5]!=Motor_ACT_NOACT)
        {
            th+=50;
        }
        if (MotorStateReal[Motor6]!=Motor_ACT_NOACT)
        {
            th+=20;
        }
        break;
    default:
        break;
    }
    return th;
}

void HallDetecte(void)
{
    static uint8_t HallLastState[6],HallDelay[6];
    uint8_t i,hallstate;
    for (i = 0; i < 6; i++)
    {
        hallstate = GetHallIO(i);
        MotorHallIO[i] = hallstate;
        if (hallstate != HallLastState[i])
        {
            HallDelay[i]++;
            if (HallDelay[i] > HALLDELAYMAX)
            {
                if (MotorStateReal[i] == Motor_ACT_CW)
                {
                    MotorHallLoc[i]++;
                }
                else if (MotorStateReal[i] == Motor_ACT_CCW)
                {
                    MotorHallLoc[i]--;
                }
                HallLastState[i] = hallstate;
            }
            HallErrorCount[i] = 0;
            MotorErr[i] = 0;
            //EEL_SAVE_Counter = 1000;
            //EEL_SAVE_REQUIRE_FLAG = 1;
        }
        else
        {
            HallDelay[i] = 0;
            if (MotorStateReal[i] != Motor_ACT_NOACT)
            {
                HallErrorCount[i]++;
                if (HallErrorCount[i] > 500)
                {
                    HallErrorCount[i] = 500;
                    MotorErr[i] = 1;
                }
                
            }
            
        }
    }
    /*
    if (EEL_SAVE_Counter > 0)
    {
        EEL_SAVE_Counter--;
    }
    */
}