#include "MotorCtrl.h"

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


uint8_t MotorState[6],MotorStateReal[6];
uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
uint8_t MotorLearnState[6];
uint8_t MotorErr[6],HallNotChange[6];
uint16_t current1,current2,current3;
uint16_t MotorTarget[6] = {0};
static uint8_t AutoCalState;

uint8_t EEL_SAVE_REQUIRE_FLAG;
uint16_t EEL_SAVE_Counter;
#define OVMOTORKB   1
#define OVMOTORZY   2

uint8_t OC1flag,OC2flag,OC3flag;

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

MOTOR_DATA MotorData[6];
uint16_t MemoryLoc[3][6];

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

uint16_t GetCheckSum(uint16_t * addr,uint8_t len)
{
    uint16_t sum=0;
    while (len --)
    {
        sum += *addr;
    }
    sum ^= 0xffff;

    return sum;
}
#define START_FLAG  0x55aa
#define STOP_FLAG   0xaa55
void WriteMotorMemory(void)
{
    uint8_t i;
    if (EEL_SAVE_REQUIRE_FLAG == 0 || EEL_SAVE_Counter > 0)
    {
        return;
    }

    MemoryData.start_flag = START_FLAG;
    MemoryData.stop_flag = STOP_FLAG;
    for (i = 0; i < 6; i++)
    {
        MemoryData.MotorData[i].MotorNowLoc = MotorHallLoc[i];
        MemoryData.MotorData[i].MotorStopLoc1 = MotorHardStop1[i];
        MemoryData.MotorData[i].MotorStopLoc2 = MotorHardStop2[i];
        MemoryData.MemoryLoc[0][i] = MemoryLoc[0][i];
        MemoryData.MemoryLoc[1][i] = MemoryLoc[1][i];
        MemoryData.MemoryLoc[2][i] = MemoryLoc[2][i];
    }
   

    MemoryData.checksum = GetCheckSum(&MemoryData.MotorData[0].MotorStopLoc1,36);
    if(WriteDataflash(&MemoryData,0x000F1000,sizeof(MemoryData)) == 0x11)
    {
        EEL_SAVE_REQUIRE_FLAG=0;
    }
}


void ReadMotorMemory(void)
{
    uint16_t checksum;
    uint8_t i;
    ReadFlashData(&MemoryData,0x000F1000,sizeof(MemoryData));
    checksum = GetCheckSum(&MemoryData.MotorData[0].MotorStopLoc1,36);
    if (MemoryData.start_flag == START_FLAG && MemoryData.stop_flag == STOP_FLAG && checksum == MemoryData.checksum)//
    {
        for (i = 0; i < 6; i++)
        {
            MotorHallLoc[i] = MemoryData.MotorData[i].MotorNowLoc;
            MotorHardStop1[i] = MemoryData.MotorData[i].MotorStopLoc1;
            MotorHardStop2[i] = MemoryData.MotorData[i].MotorStopLoc2;
            MemoryLoc[0][i] = MemoryData.MemoryLoc[0][i];
            MemoryLoc[1][i] = MemoryData.MemoryLoc[1][i];
            MemoryLoc[2][i] = MemoryData.MemoryLoc[2][i];
        }
        
    }
    else
    {
        for (i = 0; i < 6; i++)
        {
            MotorHallLoc[i] = 0x8000;
            MotorHardStop1[i] = 0;
            MotorHardStop2[i] = 0;
            MemoryLoc[0][i] = 0;
            MemoryLoc[1][i] = 0;
            MemoryLoc[2][i] = 0;
        }
        
    }
}


void MotorValueInit(void)
{
    uint8_t i;
    for (i = 0; i < 6; i++)
    {
        MotorHallLoc[i] = 0x8000;
        MotorErr[i] = 0;
    }
    ReadMotorMemory();
}

void setMotorState(uint8_t motorid,uint8_t act)
{
    //motorid-=1;
    uint8_t i;
    if (act <= ACT_XH && motorid < 6)
    {
        MotorState[motorid] = act;
        if (act!=ACT_NOACT)
        {
            for (i = 0; i < 3; i++)
            {
                MotorTarget[i] = 0;
            }
            
        }
    }
}
uint8_t getMotorState(uint8_t motorid)
{
    return MotorState[motorid];
}
void setMotorTarget(uint8_t motorid,uint16_t target)
{
    
    MotorTarget[motorid] = target;
}
 
 uint16_t getMotorTarget(uint8_t motorid)
 {
    return MotorTarget[motorid];
 }



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

void StartAutoCal(void)
{
    //AutoCalState = AUTOCAL_START;
    uint8_t i;
    for (i = 0; i < 4; i++)
    {
        MotorHardStop1[i] = 0;
        MotorHardStop2[i] = 0;
        
    }
    
    
}
void StopAutoCal(void)
{
    if (AutoCalState != AUTOCAL_STOP)
    {
        AutoCalState = AUTOCAL_STOP;
        setMotorState(MOTOR1,ACT_NOACT);
        setMotorState(MOTOR2,ACT_NOACT);
        setMotorState(MOTOR3,ACT_NOACT);
        setMotorState(MOTOR4,ACT_NOACT);
        setMotorState(MOTOR5,ACT_NOACT);
        setMotorState(MOTOR6,ACT_NOACT);
    }
}

void FW_act(void)
{
    setMotorTarget(MOTOR1,MotorHardStop1[MOTOR1]-100);
    setMotorTarget(MOTOR2,MotorHardStop1[MOTOR2]-100);
    setMotorTarget(MOTOR3,MotorHardStop1[MOTOR3]-100);
}

void TP_act(void)
{
    setMotorTarget(MOTOR1,MotorHardStop2[MOTOR1]+500);//kb
    setMotorTarget(MOTOR2,MotorHardStop2[MOTOR2]+100);//hg
    setMotorTarget(MOTOR3,MotorHardStop2[MOTOR3]+100);//tt
}




uint8_t flagMotorMemoryKeyStart,flagMotorMemoryKeyLongPress,flagKEYM1press;
uint16_t countMotorMemoryKeyStart;
uint16_t countMotorMemoryKeyLongPress;
void MotorMemoryKeyMMPress(void)
{
    
    flagMotorMemoryKeyLongPress = 1;
    
    countMotorMemoryKeyLongPress = 0;
}

void MotorMemoryKeyMMRelease(void)
{
    if (countMotorMemoryKeyLongPress >= 300)
    {
        flagMotorMemoryKeyLongPress = 2;
        countMotorMemoryKeyLongPress = 500;
        flagKEYM1press = 0;
    }
    else
    {
        flagMotorMemoryKeyStart = 1;
        countMotorMemoryKeyStart = 0;
        flagMotorMemoryKeyLongPress = 0;
        countMotorMemoryKeyLongPress = 0;
        
    }
}

void MotorMemoryKeyM1Press(void)
{
    
}
void MotorMemoryKeyM1Release(void)
{
    uint8_t i;
    if (flagMotorMemoryKeyLongPress == 2)
    {
        flagKEYM1press++;
        if (flagKEYM1press >= 3)
        {
            StartAutoCal();
        }
        
    }
    if (flagMotorMemoryKeyStart == 1)
    {
        flagMotorMemoryKeyStart = 0;
        for (i = 0; i < 3; i++)
        {
            buzzer_start();
            if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
            {
                MemoryLoc[0][i] = MotorHallLoc[i];
            }
            EEL_SAVE_Counter = 1000;
            EEL_SAVE_REQUIRE_FLAG = 1;
        }
    }
    else
    {
        for (i = 0; i < 3; i++)
        {
            setMotorTarget(i,MemoryLoc[0][i]);
        }
    }
}

void MotorMemoryKeyM2Press(void)
{

}
void MotorMemoryKeyM2Release(void)
{
    uint8_t i;
    if (flagMotorMemoryKeyStart == 1)
    {
        flagMotorMemoryKeyStart = 0;
        buzzer_start();
        for (i = 0; i < 3; i++)
        {
            if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
            {
                MemoryLoc[1][i] = MotorHallLoc[i];
            }
        }
        EEL_SAVE_Counter = 1000;
        EEL_SAVE_REQUIRE_FLAG = 1;
    }
    else
    {
        for (i = 0; i < 3; i++)
        {
            setMotorTarget(i,MemoryLoc[1][i]);
        }
    }
}

void MotorMemoryKeyM3Press(void)
{

}
void MotorMemoryKeyM3Release(void)
{
    uint8_t i;
    if (flagMotorMemoryKeyStart == 1)
    {
        flagMotorMemoryKeyStart = 0;
        buzzer_start();
        for (i = 0; i < 3; i++)
        {
            if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
            {
                MemoryLoc[2][i] = MotorHallLoc[i];
            }
        }
        EEL_SAVE_Counter = 1000;
        EEL_SAVE_REQUIRE_FLAG = 1;
    }
    else
    {
        for (i = 0; i < 3; i++)
        {
            setMotorTarget(i,MemoryLoc[2][i]);
        }
    }
    
}

/// @brief 
/// @param  
void MotorCtrl(void)//10ms
{
    uint8_t i;
    WriteMotorMemory(); //TODO
    //AutoCalCtrl();
    if (flagMotorMemoryKeyStart == 1)
    {
        countMotorMemoryKeyStart++;
        if (countMotorMemoryKeyStart > 500)//5S
        {
            flagMotorMemoryKeyStart = 0;
            countMotorMemoryKeyStart = 0;
        }
    }
    if (flagMotorMemoryKeyLongPress == 1 && countMotorMemoryKeyLongPress < 1000)
    {
        countMotorMemoryKeyLongPress++;
    }
    if (flagMotorMemoryKeyLongPress == 2 && countMotorMemoryKeyLongPress > 0)
    {
        countMotorMemoryKeyLongPress--;
        if (countMotorMemoryKeyLongPress == 0)
        {
            flagMotorMemoryKeyLongPress = 0;
        }
        
    }
    
    
    
    
    if (OC1flag == 1)
    {
        OC1flag = 0;
        if (MotorState[0] == ACT_XQ && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
        {
            MotorHardStop1[0] = MotorHallLoc[0];
        }
        else if (MotorState[0] == ACT_XH && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
        {
            MotorHardStop2[0] = MotorHallLoc[0];
        }
        else if (MotorState[1] == ACT_XQ && MotorState[0] == ACT_NOACT  && MotorErr[1] == 0)
        {
            MotorHardStop1[1] = MotorHallLoc[1];
        }
        else if (MotorState[1] == ACT_XH && MotorState[0] == ACT_NOACT  && MotorErr[1] == 0)
        {
            MotorHardStop2[1] = MotorHallLoc[1];
        }
        
        if (MotorState[MOTOR1] != ACT_NOACT && MotorState[MOTOR2] != ACT_NOACT)
        {
            if (HallNotChange[MOTOR1] == 1)
            {
                MotorState[MOTOR1] = ACT_NOACT;
            }
            if (HallNotChange[MOTOR2] == 1)
            {
                MotorState[MOTOR2] = ACT_NOACT;
            }
            
            
        }
        else
        {
            MotorState[MOTOR1] = ACT_NOACT;
            MotorState[MOTOR2] = ACT_NOACT;
        }
        
        /*
        MotorState[MOTOR1] = ACT_NOACT;
        MotorState[MOTOR2] = ACT_NOACT;
        */
    }
    if (OC2flag == 1)
    {
        OC2flag = 0;
        if (MotorState[2] == ACT_XQ && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
        {
            MotorHardStop1[2] = MotorHallLoc[2];
        }
        else if (MotorState[2] == ACT_XH && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
        {
            MotorHardStop2[2] = MotorHallLoc[2];
        }
        else if (MotorState[3] == ACT_XQ && MotorState[2] == ACT_NOACT  && MotorErr[3] == 0)
        {
            MotorHardStop1[3] = MotorHallLoc[3];
        }
        else if (MotorState[3] == ACT_XH && MotorState[2] == ACT_NOACT  && MotorErr[3] == 0)
        {
            MotorHardStop2[3] = MotorHallLoc[3];
        }
        MotorState[MOTOR3] = ACT_NOACT;
        //MotorState[MOTOR4] = ACT_NOACT;
    }


    for (i = 0; i < 3; i++)
    {
        MotorStateReal[i] = MotorState[i];
        if (MotorState[i] == 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] = ACT_XQ;
                }
                else if (MotorTarget[i] < MotorHallLoc[i]-10)
                {
                    MotorStateReal[i] = ACT_XH;
                }
                else
                {
                    MotorTarget[i] = 0;
                }
            }
            
        }
        else if (MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0)
        {
            /*取消手动限制
            if (MotorHallLoc[i] > (MotorHardStop1[i]-20) && MotorStateReal[i] == ACT_XQ)
            {
                MotorStateReal[i] = ACT_NOACT;
            }
            if (MotorHallLoc[i] < (MotorHardStop2[i] + 20) && MotorStateReal[i] == ACT_XH)
            {
                MotorStateReal[i] = ACT_NOACT;
            }
            */
        }
        
        else
        {
            MotorTarget[i] = 0;
        }
    }
    
    MOTOR1Ctrl(MotorStateReal[MOTOR1]);
    MOTOR2Ctrl(MotorStateReal[MOTOR2]);
    MOTOR3Ctrl(MotorStateReal[MOTOR3]);


}



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


uint16_t getOCval(void)
{
    uint16_t OCval = 56U;
    if (MotorStateReal[MOTOR1] != ACT_NOACT)
    {
        OCval += 168U;
    }
    if (MotorStateReal[MOTOR2] != ACT_NOACT)
    {
        OCval += 168U;
    }
    return OCval;
}
void CurrentDetecte(void)
{
    
    static uint16_t OC_Count1=0,OC_Count2=0,OC_Count3 = 0;
    
    current1 = getAdval(ADCH_RLY2);
    current2 = getAdval(ADCH_RLY1);

    
    if (current1 > getOCval() && OC1flag == 0)//实测1A->0.14V->28AD
    {
        OC_Count1++;
        if (OC_Count1 >= 300)
        {
            OC_Count1 = 0;
            OC1flag = 1;
        }
    }
    else
    {
        OC_Count1 = 0;
    }

    if (current2 > 262U && OC2flag == 0)//实测1A->0.16V->32AD
    {
        OC_Count2++;
        if (OC_Count2 >= 300)
        {
            OC_Count2 = 0;
            OC2flag = 1;
        }
        
    }
    else
    {
        OC_Count2 = 0;
    }

    
}

void OverCurrentPro(uint8_t ovmotor)
{
    
}

#define HALLDELAYMAX 2

uint16_t HallErrorCount[6];
void HallDetecte(void)
{
    static uint8_t HallLastState[6],HallDelay[6];
    uint8_t i,hallstate;
    for (i = 0; i < 4; i++)
    {
        hallstate = GetSigState(SIGID_HALL_KB + i);
        if (hallstate != HallLastState[i])
        {
            HallDelay[i]++;
            if (HallDelay[i] > HALLDELAYMAX)
            {
                if (MotorStateReal[i] == ACT_XQ)
                {
                    MotorHallLoc[i]++;
                }
                else if (MotorStateReal[i] == ACT_XH)
                {
                    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] != ACT_NOACT)
            {
                HallErrorCount[i]++;
                if (HallErrorCount[i] > 500)
                {
                    HallErrorCount[i] = 500;
                    MotorErr[i] = 1;
                }
                
            }
            else
            {
                HallErrorCount[i] = 0;
            }
            
        }
        if (HallErrorCount[i] > 100)
        {
            HallNotChange[i] = 1;
        }
        else
        {
            HallNotChange[i] = 0;
        }
    }

    if (EEL_SAVE_Counter > 0)
    {
        EEL_SAVE_Counter--;
    }
    
}


void setJSMomory(void)
{
    buzzer_start();
    MemoryLoc[0][3] = MotorHallLoc[MOTOR1];
    MemoryLoc[1][3] = MotorHallLoc[MOTOR2];
    MemoryLoc[2][3] = MotorHallLoc[MOTOR3];
    EEL_SAVE_Counter = 1000;
    EEL_SAVE_REQUIRE_FLAG = 1;
}

void gotoJSLoc(void)
{
    setMotorTarget(MOTOR1,MemoryLoc[0][3]);
    setMotorTarget(MOTOR2,MemoryLoc[1][3]);
    setMotorTarget(MOTOR3,MemoryLoc[2][3]);
}