#include "MotorCtrl.h"

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


uint8_t MotorState[6],MotorStateReal[6];
uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
uint8_t MotorLearnState[6];
uint8_t MotorErr[6];
uint16_t current1,current2,current3;

static uint8_t AutoCalState;

uint8_t EEL_SAVE_REQUIRE_FLAG;
uint16_t EEL_SAVE_Counter;


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;

        MotorHardStop1[i] = 0;
        MotorHardStop2[i] = 0;

    }
    ReadMotorMemory();
}


uint16_t MotorTarget[6] = {0};
void setMotorTarget(uint8_t motorid,uint16_t target)
{
    if (MotorTarget[motorid] != 0)
    {
        MotorTarget[motorid] = 0;
        setMotorState(motorid,ACT_NOACT);
    }
    else
    {
        MotorTarget[motorid] = target;
    }
    
}
void setMotorAutoXQ(void)
{
    setMotorTarget(MOTOR1,MotorHardStop1[0] - 2500);
    //MotorTarget[0] = MotorHardStop1[0] - 2500;
}
void setMotorAutoXH(void)
{
    //MotorTarget[0] = MotorHardStop1[0] - 10;
    setMotorTarget(MOTOR1,MotorHardStop1[0] - 10);
}
void setMotorState(uint8_t motorid,uint8_t act)
{
    //motorid-=1;
    if (act <= ACT_XH && motorid < 6)
    {
        MotorState[motorid] = act;
        setMotorTarget(0,0);
    }
}
 



#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;
    
}
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 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(MOTOR1,ACT_XQ);
        setMotorState(MOTOR3,ACT_XQ);
        setMotorState(MOTOR5,ACT_XQ);
        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[MOTOR1] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MOTOR1,ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MOTOR1,ACT_NOACT);
        }
        break;
    case 2://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MOTOR1,ACT_XH);
            autocalcounter[0] = 0;
        }
        break;
    case 3://motor1 xh 
        autocalcounter[0]++;
        if (MotorHardStop2[MOTOR1] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MOTOR1,ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MOTOR1,ACT_NOACT);
        }
        break;
    case 4://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MOTOR2,ACT_XQ);
            autocalcounter[0] = 0;
        }
        break;
    case 5://motor2 xq
        autocalcounter[0]++;
        if (MotorHardStop1[MOTOR2] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MOTOR2,ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MOTOR2] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MOTOR2,ACT_NOACT);
        }
        break;
    case 6://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MOTOR2,ACT_XH);
            autocalcounter[0] = 0;
        }
        break;
    case 7:
        autocalcounter[0]++;
        if (MotorHardStop2[MOTOR2] != 0)
        {
            MotorArr1state=0;
            wait1 = 0;
            setMotorState(MOTOR2,ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MOTOR2] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MOTOR2,ACT_NOACT);
        }
        break;
    default:
        break;
    }
    
}


void MotorCtrl(void)//10ms
{
    uint8_t i;
    WriteMotorMemory(); //TODO
    //AutoCalCtrl();
    
    if (OC1flag == 1)
    {
        OC1flag = 0;
        if (MotorState[0] == ACT_XQ)
        {
            MotorHardStop1[0] = MotorHallLoc[0];//后停止点
        }
        else if (MotorState[0] == ACT_XH)
        {
            MotorHardStop2[0] = MotorHallLoc[0];//前停止点
        }
        MotorState[MOTOR1] = ACT_NOACT;
    }
    


    MotorStateReal[0] = MotorState[0];
    if (MotorState[0] == ACT_NOACT  && MotorHardStop1[0] != 0  )//&& MotorHardStop2[0] != 0
    {
        if (MotorTarget[0]!=0 && MotorHardStop1[0] > MotorTarget[0] )//&& MotorTarget[0] > MotorHardStop2[0]
        {
            if (MotorTarget[0] > MotorHallLoc[0]+10)
            {
                MotorStateReal[0] = ACT_XQ;
            }
            else if (MotorTarget[0] < MotorHallLoc[0]-10)
            {
                MotorStateReal[0] = ACT_XH;
            }
            else
            {
                MotorTarget[0] = 0;
            }
        }
        
    }
    else if (MotorHardStop1[0] != 0 && MotorHardStop2[0] != 0)
    {
        if (MotorHallLoc[0] > (MotorHardStop1[0]-20) && MotorStateReal[0] == ACT_XQ)
        {
            MotorStateReal[0] = ACT_NOACT;
        }
        if (MotorHallLoc[0] < (MotorHardStop2[0] + 20) && MotorStateReal[0] == ACT_XH)
        {
            MotorStateReal[0] = ACT_NOACT;
        }
    }
    else
    {
        MotorTarget[0] = 0;
    }

    
    MOTOR1Ctrl(MotorStateReal[MOTOR1]);
}



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

void CurrentDetecte(void)
{
    
    static uint16_t OC_Count1=0,OC_Count2=0,OC_Count3 = 0;
    
    current1 = getAdval(ADCH_RLY1);
    
    if (current1 > 100U && OC1flag == 0)
    {
        OC_Count1++;
        if (OC_Count1 >= 100)
        {
            OC_Count1 = 0;
            OC1flag = 1;
        }
    }
    else
    {
        OC_Count1 = 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 hallstate;

    hallstate = GetIOState(SIGID_HALL1);
    if (hallstate != HallLastState[0])
    {
        HallDelay[0]++;
        if (HallDelay[0] > HALLDELAYMAX)
        {
            if (MotorStateReal[0] == ACT_XQ)
            {
                MotorHallLoc[0]++;
            }
            else if (MotorStateReal[0] == ACT_XH)
            {
                MotorHallLoc[0]--;
            }
            HallLastState[0] = hallstate;
        }
        HallErrorCount[0] = 0;
        MotorErr[0] = 0;
        EEL_SAVE_Counter = 1000;
        EEL_SAVE_REQUIRE_FLAG = 1;
    }
    else
    {
        HallDelay[0] = 0;
        if (MotorStateReal[0] != ACT_NOACT)
        {
            HallErrorCount[0]++;
            if (HallErrorCount[0] > 500)
            {
                HallErrorCount[0] = 500;
                MotorErr[0] = 1;
            }
        }
    }

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

uint16_t getMotorLoc(void)
{
    return MotorHallLoc[0];
}