#include "StateMachine.h"
#include <modules/my_task_creator/TaskCreator.h>
void StateMachine::TaskStateMachine(void *pvParameters)
{
    log_i("Task started");
    vTaskDelay(100);
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    StateMachine *obj=&(mCreator->mStateMachine);
    MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
    DefaultParametrs *mParams=&(mCreator->mParams);
    TrackerRxTx *mTracker=&(mCreator->mTracker);
    TickType_t mStartTick,mStopTick;//=xTaskGetTickCount();
    TickType_t mLastTick=xTaskGetTickCount();
    TickType_t mLastTickGuided=mLastTick;
    TickType_t mLastTickCalc=mLastTick;
    String ModeName;
    obj->mState=StateWait;
    while (1)
    {
        TickType_t mCurrentTick=xTaskGetTickCount();
        switch (obj->mState)
        {
        case StateWait:
            {
                ModeName="Wait";
                
                uint16_t n=obj->GetWpCount();
                if(obj->mSysID)
                {
                    if(obj->mSysID!=obj->mSysID_old)
                    {
                        mTransmitter->SendText(SOFT_VER,MAV_SEVERITY_NOTICE);
                        obj->mSysID_old=obj->mSysID;
                        uint8_t m=mParams->GetValue("CTRL_TEST_MODE");
                        float alt=mParams->GetValue("CTRL_TEST_ALT");
                        obj->SetTestMode(m,alt);
                        float ky=mParams->GetValue("CTRL_K_YAW");
                        float ki=mParams->GetValue("CTRL_I_YAW");
                        float kd=mParams->GetValue("CTRL_D_YAW");
                        float kimax=mParams->GetValue("CTRL_I_MAX");
                        float kp=mParams->GetValue("CTRL_K_PITCH");
                        bool use=mParams->GetValue("CTRL_USE_KPITCH");
                        float speed=mParams->GetValue("CTRL_ATTACK_SPD");
                        obj->SetCamSrvCH(mParams->GetValue("CAM_SRV_CH"));
                        obj->SetCTRLSrvCH(mParams->GetValue("CTRL_SRV_CH"));
                        obj->SetCamSrvMax(mParams->GetValue("CAM_SRV_MAX"));
                        obj->SetCamSrvMin(mParams->GetValue("CAM_SRV_MIN"));
                        obj->SetCamAngCruise(mParams->GetValue("CAM_ANG_CRUISE"));
                        obj->SetCamAngMax(mParams->GetValue("CAM_ANG_MAX"));
                        obj->SetCamAngMin(mParams->GetValue("CAM_ANG_MIN"));
                        obj->SetFlapRC(mParams->GetValue("CTRL_FLAP_RC"));
                        obj->SetFlapVAL(mParams->GetValue("CTRL_FLAP_VAL"));

                        obj->SetTestSrvCH(mParams->GetValue("TEST_SRV_CH"));
                        obj->SetTestSrvAmp(mParams->GetValue("TEST_SRV_AMP"));
                        obj->SetTestSrvT(mParams->GetValue("TEST_SRV_T"));
                        obj->SetTestSrvTrim(mParams->GetValue("TEST_SRV_TRIM"));
                        obj->SetCamReverse(mParams->GetValue("CAM_SRV_REVERSE"));

                        obj->SetK(ky);
                        obj->SetI(ki);
                        obj->SetD(kd);
                        obj->SetImax(kimax);
                        obj->SetKpitch(kp);
                        obj->SetUseKpitch(use);
                        obj->SetVideoParam(mParams->GetValue("VIDEO_FOVX"),mParams->GetValue("VIDEO_FOVY"),
                            mParams->GetValue("VIDEO_WIDTH"),mParams->GetValue("VIDEO_HEIGHT"));
                        obj->SetTrackerEnable(mParams->GetValue("VIDEO_ENABLE"));
                        obj->SetAttackSpeed(speed);
                        obj->SetLoiterAlt(mParams->GetValue("CTRL_LOITER_ALT"));
                        obj->SetSafeSpeed(mParams->GetValue("CTRL_SAFE_SPD"));
                        obj->SetSafePitch(mParams->GetValue("CTRL_SAFE_PITCH"));
                        obj->SetSafeTime(mParams->GetValue("CTRL_SAFE_TIME"));
                        obj->SetCamSrvKI(mParams->GetValue("CAM_SRV_KI"));
                        obj->SetCamSrvKP(mParams->GetValue("CAM_SRV_KP"));
                        obj->SetCamPitchDelta(mParams->GetValue("CAM_PITCH_DELTA"));
                        obj->SetCamRateMax(mParams->GetValue("CAM_RATE_MAX"));
                        obj->SetTecsDalt(mParams->GetValue("CTRL_TECS_DALT"));
                        if(mParams->GetValue("CTRL_TWP_NUM")==-1)
                        {    
                            mTransmitter->SendMissionReq();
                            mStartTick=xTaskGetTickCount();
                            log_i("Send MissionReq");
                            vTaskDelay(50);
                        }
                    }
                    else
                    {
                        if(mParams->GetValue("CTRL_TWP_NUM")==-1)
                        {
                            obj->mTargetWpNum=0;

                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
                            vTaskDelay(10);
                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
                            vTaskDelay(10);
                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,100000);
                            vTaskDelay(10);
                            if(obj->mTEST_SRV_CH)
                            {
                                obj->mState=StateTestSrv;
                                mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
                                log_i("State chnged to StateTestSrv");
                                vTaskDelay(1000);
                            }
                            else
                            {
                                obj->mState=StateNavigation;
                                log_i("State chnged to StateNavigation");
                            }
                            break;
                        }
                        mStopTick=xTaskGetTickCount();
                        if(mStopTick-mStartTick>500)
                        {
                            mTransmitter->SendMissionReq();
                            mStartTick=xTaskGetTickCount();
                            log_i("Resend MissionReq");
                        }
                        vTaskDelay(50);
                    }
                    if(n>0)
                    {
                        obj->mState=StateReadWps;
                        obj->mCurWpRead=0;
                        log_i("Change state to StateReadWps");
                        vTaskDelay(50);
                    }
                }
                else
                {
                    //log_i("Wait arm");
                    vTaskDelay(1000);
                }
                    
            }
            break;
        case StateReadWps:
            {
                ModeName="ReadWps";
                if(obj->mCurWpRead==obj->mWpReadedCount)
                {
                    mTransmitter->SendRequestMissionItem(obj->mCurWpRead);
                    log_i("Send req get wp num=%d",obj->mCurWpRead);
                    vTaskDelay(100);
                }
                else
                {
                    obj->mCurWpRead++;
                    if(obj->mCurWpRead>obj->mWpMaxNum)
                    {
                        obj->mCurWpRead--;
                        if(mParams->GetValue("CTRL_WP_REDY")>0)
                        {
                                     
                            obj->WpD=obj->mWpsMap[obj->mWpMaxNum-1];

                            obj->TargetWp=obj->mWpsMap[obj->mWpMaxNum];
                            obj->mTargetWpNum=obj->mWpMaxNum;

                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
                            vTaskDelay(10);
                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
                            vTaskDelay(10);
                            mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,100000);
                            vTaskDelay(10);
                            if(obj->mTEST_SRV_CH)
                            {
                                obj->mState=StateTestSrv;
                                mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
                                log_i("State chnged to StateTestSrv");
                                vTaskDelay(1000);
                            }
                            else
                            {
                                obj->mState=StateNavigation;
                                log_i("State chnged to StateNavigation");
                            }
                        }
                        else
                            obj->mState=StateCalcWp;
                            
                        log_i("All wp readed nums=%d",obj->mWpMaxNum);
                    }
                    vTaskDelay(10);
                }
            }
            break;
        case StateCalcWp:
            {
                ModeName="CalcWp";
                uint8_t error=obj->CalckTargetWp(pvParameters);
                if(error==StateMachine::CalcErrorNone)
                {
                    obj->mTargetWpNum=obj->TargetWp.seq;
                    mTransmitter->SendMissionCount();
                    obj->mState=StateSendWps;
                    log_i("State changed to StateSendWps");
                }
                else
                {
                    if(obj->mTEST_SRV_CH)
                    {
                        obj->mState=StateTestSrv;
                        mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
                        log_i("State chnged to StateTestSrv");
                        vTaskDelay(1000);
                    }
                    else
                    {
                        obj->mState=StateNavigation;
                        log_i("State chnged to StateNavigation");
                    }
                }
                vTaskDelay(10);
            }
            break;
        case StateSendWps:
            {
                ModeName="SendWps";
                if(obj->mWpsSended)
                {
                    
                    log_i("State chnged to StateNavigation");
                    mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
                    vTaskDelay(50);
                    mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
                    vTaskDelay(50);
                    mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,50000);
                    vTaskDelay(50);
                    obj->mState=StateNavigation;
                }
                vTaskDelay(50);
            }
            break;
        case StateNavigation:
            {
                ModeName="Navigation";
                static uint16_t cnt=0;
                if(mTracker->isNewData())
                {
                    uint16_t x,y,count;
                    uint8_t v,mode;
                    mTracker->GetTrackerData(x,y,v,mode,count);
                    obj->SetTrackerData(x,y,v,mode,count);
                }
                if(mTracker->isNewCamData())
                {
                    float val=mTracker->GetCamData();
                    obj->mCAM_TARGET=val;
                    obj->mNewCamData=true;
                }
                uint8_t res=obj->Correction();
                switch (res)
                {
                case ResContinue:
                    mTracker->SetMavlikBuf(obj->mBufferOutStream,obj->mSendLen);
                    break;
                case ResSwitchGuided:
                {
                    mTransmitter->SendGuidedMode(obj->mBaseMode,obj->GuidedWp);
                    log_i("Set guided mode");
                    vTaskDelay(50);
                    mTransmitter->SendGuidedSpeed(obj->mAttackSpeed);
                }
                    
                    break;    
                case ResSendControl:
                {
                   if(obj->mTECS_DALT>0)
                        mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,obj->mGuidedAlt,true);
                    else
                        mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,1,false);
                    mTracker->SetMavlikBuf(obj->mBufferOutStream,obj->mSendLen);
                    if(obj->mSendPWM)
                    {
                        obj->mSendPWM=false;
                        mTransmitter->SendCamPWM(obj->mCAM_SRV_CH,obj->mCamPWM);
                    }
                    if(obj->mFLAP_RC)
                    {
                        mTransmitter->SendFlapPWM(obj->mFLAP_RC,obj->mFLAP_PWM);
                    }
                    if(obj->mUnlock && obj->mCTRL_SRV_CH)
                    {
//Відправка PWM на відкриття замку                        
                        /*mTransmitter->SendCamPWM(obj->mCTRL_SRV_CH,2000);*/
                        
                        mTransmitter->SendCamPWM(obj->mCTRL_SRV_CH,1000);
                    }
                    if((mCurrentTick-mLastTickGuided)>5000)
                    {

                        String p(obj->mCam_pitch*180.0/M_PI,1);
                        String y(obj->mCam_yaw*180.0/M_PI,1);
                        String distans(obj->mDistans,1);
                        String use(obj->mUseTracker);
                        String tmp("Nav UseTracker=");
                        tmp+=use;
                        tmp+=String(" d=");
                        tmp+=distans;
                        tmp+=String(" TPitch=");
                        tmp+=p;
                        tmp+=String(" TYaw=");
                        tmp+=y;
                        mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);

                        /*String fovx(obj->mFovX,1);
                        String fovy(obj->mFovY,1);
                        String w(obj->mWidth,1);
                        String h(obj->mHeight,1);
                        String kx(obj->mKx,4);
                        String ky(obj->mKy,4);
                        String tmp1("test ");
                        tmp1+=fovx+String(" ")+fovy+String(" ")+w+String(" ")+h+String(" ")+kx+String(" ")+ky;
                        mTransmitter->SendText(tmp1,MAV_SEVERITY_INFO);*/


                        mLastTickGuided=mCurrentTick;

                    }
                }
                break;
                case ResSwitchRtl:
                {
                    mTransmitter->SendRtl();
                    String pich_error(obj->mPitchError,1);
                    String yaw_error(obj->mYawError,1);
                    String distans(obj->mDistans,1);
                    String tmp("NavEnd d=");
                    tmp+=distans;
                    tmp+=String(" pe=");
                    tmp+=pich_error;
                    tmp+=String(" ye=");
                    tmp+=yaw_error;
                    mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                    log_i("Set rtl mode");
                    vTaskDelay(1000);
                    mTransmitter->SendText("Set rtl mode",MAV_SEVERITY_INFO);
                }     
                    break;
                case ResSwitchCustomGuided:
                {
                    mTransmitter->SendGuidedMode(obj->mBaseMode,obj->CustomGuidedWp);
                    log_i("Set guided mode");
                    vTaskDelay(50);
                    mTransmitter->SendGuidedSpeed(obj->mAttackSpeed);
                }
                break;
                case ResSwitchLoiter:
                {
                    mTransmitter->SendLoiter(obj->mLoiterAlt);
                    if(obj->mFLAP_RC)
                    {
                        mTransmitter->SendFlapPWM(obj->mFLAP_RC,1000);
                    }
                    vTaskDelay(50);
                    //mTransmitter->SendLoiterSpeed(obj->mLoiterSpeed);
                    log_i("Set loiter mode");
                    String pich_error(obj->mPitchError,1);
                    String yaw_error(obj->mYawError,1);
                    String distans(obj->mDistans,1);
                    String tmp("NavEnd d=");
                    tmp+=distans;
                    tmp+=String(" pe=");
                    tmp+=pich_error;
                    tmp+=String(" ye=");
                    tmp+=yaw_error;
                    mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                    obj->mUnlock=false;
                    vTaskDelay(1000);
                }
                break; 
                case ResSendCamPWM:
                {
                    mTransmitter->SendCamPWM(obj->mCAM_SRV_CH,obj->mCamPWM);
                    log_i("SendCamPWM pwm=%d ang=%.2f",obj->mCamPWM,obj->mCAM_CURRENT);
                }
                break;
                case ResSwitchSafeMode:
                {
                    obj->mEnterSafeMode=true;
                    obj->mState=StateSafeMode;
                }
                break;                     
                default:
                    break;
                }
            
                
                uint32_t mCurrentTick1=xTaskGetTickCount();
                uint32_t dt=mCurrentTick1-mCurrentTick;
                cnt++;
                if(cnt==500)
                {
                    String tmp("Correction time=");
                    tmp+=dt;
                    mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                    log_i("Correction time=%i",dt);
                    cnt=0;
                }

                if(dt<20)
                {
                    vTaskDelay(20-dt);
                }
                
            }
            break;
        case StateTestSrv:
        {
            ModeName="StateTestSrv";
            static uint64_t cnt=0;
            if(obj->mTEST_SRV_T<0.1)
            {
                log_i("Error value TEST_SRV_T");
                obj->mTEST_SRV_T=3.0;
                
            }
            float om=2.0f*M_PI/obj->mTEST_SRV_T;
            float t=0.02f*cnt;
            if(om*t>2.0f*M_PI)
                cnt=0;
            cnt++;
            float val=sin(om*t)*obj->mTEST_SRV_AMP+obj->mTEST_SRV_TRIM;
            if(val>2000)
                val=2000;
            if(val<1000)
                val=1000;    
            mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,val);
            vTaskDelay(20);
            //log_i("SendCamPWM pwm=%d ang=%.2f",obj->mCamPWM,obj->mCAM_CURRENT);
        }
        break;
        case StateSafeMode:
        {
            ModeName="SafeMode";
            uint8_t res=obj->SafeMode();
            switch (res)
            {
                case ResContinue:
                {
                    vTaskDelay(20);
                }
                break;
                case ResSendWpAndControl:
                {
                    if(obj->mTECS_DALT>0)
                        mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,obj->mGuidedAlt,true);
                    else
                        mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,1,false);
                    vTaskDelay(10);
                    mTransmitter->SendGuidedMode(obj->mBaseMode,obj->CustomGuidedWp);
                    vTaskDelay(10);
                    mTransmitter->SendGuidedSpeed(obj->mSafeSpeed);
                    vTaskDelay(10);
                    if(obj->mFLAP_RC)
                    {
                        mTransmitter->SendFlapPWM(obj->mFLAP_RC,1000);
                    }
                    vTaskDelay(20);
                    String pich_error(obj->mPitchError,1);
                    String yaw_error(obj->mYawError,1);
                    String distans(obj->mDistans,1);
                    String tmp("NavEndSafe d=");
                    tmp+=distans;
                    tmp+=String(" pe=");
                    tmp+=pich_error;
                    tmp+=String(" ye=");
                    tmp+=yaw_error;
                    mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                    log_i("Set safe guided mode");
                }
                break;
                case ResSwitchNavigation:
                {
                    obj->mState=StateNavigation;
                    log_i("Switch form Safe to Navigation");
                }
                break;
            }
        }
        break;      
        default:
            break;
        }
        if((mCurrentTick-mLastTick)>12000)
        {
            String tmp("Navmode=");
            tmp+=ModeName;
            tmp+=String(" twp=");
            tmp+=String(obj->mTargetWpNum);
            tmp+=" TrMsgCnt=";
            tmp+=String(obj->mTrackerMsgCnt);
            mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
            mLastTick=mCurrentTick;
        }
    }
    
}
StateMachine::StateMachine():mState(StateWait),
mWpMaxNum(0),
mWpsSended(false)
{
    mYawPID=new AC_PID_Basic(3,0.1,0.01,0,5,0);
    mYawPID->reset_filter();
    mYawPID->reset_I();
}

void StateMachine::SetWp(mavlink_mission_item_t wp)
{
    mMutex.Lock();
    mWpsMap[wp.seq]=wp;
    mWpReadedCount=wp.seq+1;
    mMutex.Unlock();
    //log_i("Readed wp num=%d",wp.seq);
}
mavlink_mission_item_t StateMachine::GetWp(uint16_t num)
{
    mavlink_mission_item_t wp={0};
    mMutex.Lock();
    if(num<mWpsMap.size())
        wp=mWpsMap[num];
    mMutex.Unlock();
    return wp;
}


void StateMachine::SetWpCount(uint16_t n)
{
    mWpMaxNum=n;
}
int8_t StateMachine::CalckTargetWp(void *pvParameters)
{
    #define  Rz 6371000.0f
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    StateMachine *obj=&(mCreator->mStateMachine);
    MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
    DefaultParametrs *mParams=&(mCreator->mParams);
    uint16_t MaxWpNum=obj->GetWpCount();
    if(!MaxWpNum)
    {
        log_i("Calc error wps nums=%d",MaxWpNum);
        return StateMachine::CalcErrorWpNums;
    }
    float TaggetWpNumF=mParams->GetValue("CTRL_TWP_NUM");
    uint16_t TaggetWpNum;
    if(TaggetWpNumF<0)
    {
        TaggetWpNum=MaxWpNum;
    }
    else
        TaggetWpNum=TaggetWpNumF;
    obj->TargetWp=obj->GetWp(TaggetWpNum);
    mavlink_mission_item_t Wp2Target=obj->GetWp(TaggetWpNum-1);


    float AttackAlt=mParams->GetValue("CTRL_ATTACK_ALT");
    float AttackDist=mParams->GetValue("CTRL_ATTACK_DIST");

    float Lat0=Wp2Target.x*M_PI/180.0;
    float Lon0=Wp2Target.y*M_PI/180.0;
    float Lat1=obj->TargetWp.x*M_PI/180.0;
    float Lon1=obj->TargetWp.y*M_PI/180.0;
    float Z1=obj->TargetWp.z;

    float X1=(Lat1-Lat0)*Rz;
    float Y1=(Lon1-Lon0)*Rz*cos(Lat0);
    log_i("Target X1=%f Y1=%f Z1=%f",X1,Y1,Z1);

    float lenVn=sqrt(X1*X1+Y1*Y1);
    float X1n=X1/lenVn;
    float Y1n=Y1/lenVn;
    float t1;
    if(fabs(X1)>0.1)
      t1=X1/X1n;
    else
      t1=Y1/Y1n;
    float td=t1-AttackDist;
    log_i("Target t1=%f td=%f ",t1,td);
    float Xd=X1n*td;
    float Yd=Y1n*td;
    float Zd=AttackAlt+Z1;
    log_i("Attack Xd=%f Yd=%f Zd=%f",Xd,Yd,Zd);

    float LatD=Lat0*180.0/M_PI+Xd*180.0/M_PI/Rz;
    float LonD=Lon0*180.0/M_PI+Yd*180.0/M_PI/Rz/cos(Lat0);

    obj->WpD=obj->TargetWp;
    obj->WpD.x=LatD;
    obj->WpD.y=LonD;
    obj->WpD.z=Zd;

    obj->TargetWp.seq=obj->TargetWp.seq+1;


    obj->SetWp(obj->WpD);

    obj->SetWp(obj->TargetWp);

    obj->mWpMaxNum=obj->TargetWp.seq;
    mTransmitter->SetWps(obj->mWpsMap);
    return StateMachine::CalcErrorNone;
}

void StateMachine::SetRPY(float r, float p, float y)
{
    mMutex.Lock();
    mRoll=r;
    mPitch=p;
    mYaw=y;
    mMutex.Unlock();
}

void StateMachine::SetPos(float lat, float lon, float alt)
{
    mMutex.Lock();
    mLat=lat;
    mLon=lon;
    mAlt=alt;
    mMutex.Unlock();
}

uint8_t StateMachine::Correction()
{
    static uint32_t cwpn=0;
    static uint8_t cnt=0;
    static bool in_corr=false;
    float Lat0,Lon0,Lat1,Lon1,X1,Y1,D,Alt,yaw,roll,pitch,trackerX,trackerY;
    float yaw_t;
    float pitch_q;
    float roll_q;
    uint8_t trackerV=0;
    uint8_t trackerMode=0;
    bool trackerNew=false;
    mY=mZ=mR=mX=0;
    mMutex.Lock();
        Lat0=mLat*M_PI/180.0;
        Lon0=mLon*M_PI/180.0;
        Alt=mAlt;
        roll=mRoll;
        pitch=mPitch;
        yaw=mYaw;
        if(mNewTrackerData)
        {
            trackerX=mTrackerX;
            trackerY=mTrackerY;
            trackerV=mTrackerValid;
            trackerNew=true;
            trackerMode=mTrackerMode;
            
            mNewTrackerData=false;

        }
    mMutex.Unlock();
    if(mUseCustomTrackerMode)
    {
        if(mCustomMode==PLANE_MODE_RTL ||  mCustomMode==PLANE_MODE_LOITER)
        {
            mUseCustomTrackerMode=false;
            return ResContinue;
        }
        if(mTrackerMode==1)
        {
            if(mCustomMode!=PLANE_MODE_GUIDED)
            {
                mUseCustomTrackerMode=false;
                mUseTracker=false;
                return ResContinue;
            }
            if(mTestMode)
            {
                if(Alt<mTestAlt+20 && mCTRL_SRV_CH)
                {
                    mUnlock=true;
                }    
                if(Alt<mTestAlt)
                {
                    log_i("Switch to loiter test alt");
                    mUseCustomTrackerMode=false;
                    mUseTracker=false;
                    
                    if(Alt<mLoiterAltMin)
                        mLoiterAlt=mLoiterAltMin;
                    else
                        mLoiterAlt=Alt;
                    return ResSwitchLoiter;
                }
            }
            if(trackerV)
                mUseTracker=true;
            if(trackerV&&trackerNew)
            {
                float camYaw=(trackerX-mWidth/2.0)*mKx;
                float camPitch=(mHeight/2.0-trackerY)*mKy;
                Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
                Dcm<float> mCameraM;
                if(mCAM_SRV_CH)
                {
                    mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
                    CamYTrecker(camPitch,true);
                }
                else
                {
                    mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
                }
                Dcm<float> mToNED=mPlaneM*mCameraM;
                Euler<float> ang=Euler<float>(mToNED);
                mCam_pitch=ang.theta();
                mCam_yaw=ang.psi();
            }
            yaw_t=mCam_yaw;
            pitch_q=mCam_pitch;
            
            mMutex.Lock();    
            roll_q=mYawPID->update_all(yaw_t,yaw,0.02,false)*M_PI/180.0;
            mMutex.Unlock();
            mYawError=mYawPID->get_error();
            if(fabs(pitch_q-pitch)*180/M_PI>mCAM_PITCH_DELTA)
            {
                mRollMax=5;
                mYawPID->reset_I();
            }
            else
            {
                if(mCAM_CURRENT>0 && mCAM_CURRENT<1.0f && mCAM_SRV_CH)
                {
                    Ang2PWM(0);
                    mCAM_CURRENT=0;
                    mSendPWM=true;  
                }
                mRollMax=40;
            }
            if(roll_q>mRollMax*M_PI/180.0)
                roll_q=mRollMax*M_PI/180.0;
            if(roll_q<-mRollMax*M_PI/180.0)
                roll_q=-mRollMax*M_PI/180.0;    
            cnt++;
            if(cnt==1000)
            {
                log_i("Use mode custom tracker=%d target yaw=%f,pitch=%f",mUseTracker,yaw_t*180.0/M_PI,pitch_q*180.0/M_PI);
                cnt=0;
            }   
            float phi=roll_q;
            float theta=pitch_q;
            float psi=0;
            Eulerf et(phi,theta,psi);
            Quatf qt(et);
            mQ0=qt(0);
            mQ1=qt(1);
            mQ2=qt(2);
            mQ3=qt(3);
            mAttackMode=AttackTracker;
            CreateMavlink(roll_q,pitch_q,yaw_t,0,0,0,mAttackMode);
            mGuidedAlt=Alt-mTECS_DALT;
            return ResSendControl;
        }
        else
        {
            log_i("Switch to loiter mode");
            mUseCustomTrackerMode=false;
            if(Alt<mLoiterAltMin)
                mLoiterAlt=mLoiterAltMin;
            else
                mLoiterAlt=Alt;
            
            mUseTracker=false;    
            return ResSwitchLoiter;
        }
    }
    else
    {
        if(trackerMode==1 && trackerV && mCustomMode!=PLANE_MODE_RTL && mCustomMode!=PLANE_MODE_LOITER)
        {
            
            mUseTracker=true;
            mUseCustomTrackerMode=true;
            float camYaw=(trackerX-mWidth/2.0)*mKx;
            float camPitch=(mHeight/2.0-trackerY)*mKy;
            Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
            Dcm<float> mCameraM;
            if(mCAM_SRV_CH)
            {
                    mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
            }
            else
            {
                    mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
            }
            Dcm<float> mToNED=mPlaneM*mCameraM;
            Euler<float> ang=Euler<float>(mToNED);
            mCam_pitch=ang.theta();
            mCam_yaw=ang.psi();
            Vector3f n(1,0,0);
            Vector3f n1=mToNED*n;
            if(fabs(n1(2))<1e-5)
            {
                /*mUseCustomTrackerMode=false;
                log_i("Tracker erro guided point theta");
                if(trackerV)
                mUseTracker=false;
                return ResContinue;*/
                float X=cos(yaw)*1000.0f;
                float Y=sin(yaw)*1000.0f;
                float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
                float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
                CustomGuidedWp=GuidedWp;
                CustomGuidedWp.x=LatT;
                CustomGuidedWp.y=LonT;
                CustomGuidedWp.z=Alt;
                log_i("Tracker custom mode switch to Guided");
                mYawPID->reset_I();
                mYawPID->reset_filter();
                mUnlock=false;
                return ResSwitchCustomGuided;
            }
            if(n1(2)>1e-5)
            {
                float t=Alt/(n1(2));
                float X=n1(0)*t;
                float Y=n1(1)*t;
                float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
                float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
                D=sqrt(X*X+Y*Y);
                /*if(D>5000)
                {
                    mUseCustomTrackerMode=false;
                    log_i("Tracker custom mode error distans>5000 ");
                    if(trackerV)
                    mUseTracker=false;
                    return ResContinue;
                }*/
                CustomGuidedWp=GuidedWp;
                CustomGuidedWp.x=LatT;
                CustomGuidedWp.y=LonT;
                CustomGuidedWp.z=1;
                log_i("Tracker custom mode switch to Guided");
                mYawPID->reset_I();
                mYawPID->reset_filter();
                mUnlock=false;
                return ResSwitchCustomGuided;
            }
            if(n1(2)<-1e-5)
            {
                float t=(-Alt)/(n1(2));
                float X=n1(0)*t;
                float Y=n1(1)*t;
                float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
                float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
                D=sqrt(X*X+Y*Y);
                /*if(D>5000)
                {
                    mUseCustomTrackerMode=false;
                    log_i("Tracker custom mode error distans>5000 ");
                    if(trackerV)
                    mUseTracker=false;
                    return ResContinue;
                }*/
                CustomGuidedWp=GuidedWp;
                CustomGuidedWp.x=LatT;
                CustomGuidedWp.y=LonT;
                CustomGuidedWp.z=2*Alt;
                log_i("Tracker custom mode switch to Guided");
                mYawPID->reset_I();
                mYawPID->reset_filter();
                mUnlock=false;
                return ResSwitchCustomGuided;
            }

        }
    }

    if(mTargetWpNum>1 && cwpn!=mCurrWpNum && mCurrWpNum==TargetWp.seq && mCustomMode==PLANE_MODE_AUTO)
    {
        Lat0=WpD.x*M_PI/180.0;
        Lon0=WpD.y*M_PI/180.0;
        Lat1=TargetWp.x*M_PI/180.0;
        Lon1=TargetWp.y*M_PI/180.0;
        X1=(Lat1-Lat0)*Rz;
        Y1=(Lon1-Lon0)*Rz*cos(Lat0);
        D=sqrt(X1*X1+Y1*Y1);
        float dAlt=WpD.z-TargetWp.z;
        mTanWp=dAlt/D;
        mAngleWp=atan(mTanWp)*180.0/M_PI;
        cwpn=mCurrWpNum;
        GuidedWp=TargetWp;
        GuidedWp.seq=0;
        GuidedWp.current=2;
        mUseTracker=false;
        mKpitcKorr=mK_Pitch*M_PI/180.0/D;
        log_i("Current target wp nums=%d",mCurrWpNum);
        //WpD1.z=mAlt;
        //mX=1000;
        mYawPID->reset_I();
        mYawPID->reset_filter();
        mUnlock=false;
        return ResSwitchGuided;
    }

    if(mTargetWpNum>1 && mCurrWpNum==TargetWp.seq && mCustomMode==PLANE_MODE_GUIDED)
    {
        mAttackMode=AttackWp;
        if(trackerV)
        {
                mUseTracker=true;
                
        }
        Lat1=TargetWp.x*M_PI/180.0;
        Lon1=TargetWp.y*M_PI/180.0;
        X1=(Lat1-Lat0)*Rz;
        Y1=(Lon1-Lon0)*Rz*cos(Lat0);
        D=sqrt(X1*X1+Y1*Y1);
        if(mTestMode)
        {
            if(Alt<mTestAlt+20 && mCTRL_SRV_CH)
            {
                mUnlock=true;
            }    
            if(Alt<mTestAlt)
            {
                log_i("Switch to loiter test alt");
                cwpn=0;
                in_corr=false;
                mUseTracker=false;
                if(Alt<mLoiterAltMin)
                    mLoiterAlt=mLoiterAltMin;
                else
                    mLoiterAlt=Alt;
                return ResSwitchSafeMode;//ResSwitchLoiter;
                
            }
        }

        if(!in_corr)
        {
            mAngleWp=-atan2(Alt-TargetWp.z,D);
            mTanWp=(Alt-TargetWp.z)/D;
            in_corr=true;
        }
        float ErrorAlt=Alt-TargetWp.z;
        mAltError=ErrorAlt;
        mDistans=D;
        
        yaw_t=atan2(Y1,X1);
        
        if(mUseKpitch)
            pitch_q=-atan2(Alt-TargetWp.z,D)+mKpitcKorr*D;//mAngleWp-mK_Pitch*ErrorAlt*M_PI/180;
        else
            pitch_q=-atan2(Alt-TargetWp.z,D);


        if(mUseTracker&&mEnableTracker)
        {
            mAttackMode=AttackWpTracker;
            if(trackerV&&trackerNew)
            {
                float camYaw=(trackerX-mWidth/2.0)*mKx;
                float camPitch=(mHeight/2.0-trackerY)*mKy;
                Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
                Dcm<float> mCameraM;
                if(mCAM_SRV_CH)
                {
                    mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
                    CamYTrecker(camPitch,true);
                }
                else
                {
                    mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
                }
                Dcm<float> mToNED=mPlaneM*mCameraM;
                Euler<float> ang=Euler<float>(mToNED);
                mCam_pitch=ang.theta();
                mCam_yaw=ang.psi();
            }
           yaw_t=mCam_yaw;
           pitch_q=mCam_pitch; 
        }
        else
        {
            mCam_yaw=yaw_t;
            mCam_pitch=pitch_q;
        }     
        mPitchError=(pitch_q-pitch)*180.0/M_PI;
        /*if(yaw_t<0)
            yaw_t+=2*M_PI;
        float error=yaw_t-yaw;
        if(error>(M_PI+2.0*M_PI/180.0))
            error=-2*M_PI+error;
        if(error<(-M_PI-2.0*M_PI/180.0))
            error=2*M_PI+error;
        mYawError=error*180.0/M_PI;    
        float roll_q=mK_Yaw*(error);
        if(roll_q>30.0*M_PI/180.0)
            roll_q=30.0*M_PI/180.0;
        if(roll_q<-30.0*M_PI/180.0)
            roll_q=-30*M_PI/180.0;*/
        mMutex.Lock();    
        roll_q=mYawPID->update_all(yaw_t,yaw,0.02,false)*M_PI/180.0;
        mYawError=mYawPID->get_error();
        mMutex.Unlock();
        if(fabs(pitch_q-pitch)*180/M_PI>mCAM_PITCH_DELTA)
        {
            mRollMax=5;
            mYawPID->reset_I();
        }
        else
        {
            if(mCAM_CURRENT>0 && mCAM_CURRENT<1.0f && mCAM_SRV_CH)
            {
                Ang2PWM(0);
                mCAM_CURRENT=0;
                mSendPWM=true;  
            }
            mRollMax=40; 
        }   
        if(roll_q>mRollMax*M_PI/180.0)
            roll_q=mRollMax*M_PI/180.0;
        if(roll_q<-mRollMax*M_PI/180.0)
            roll_q=-mRollMax*M_PI/180.0;    

        cnt++;
        if(cnt==1000)
        {
            log_i("Use tracker=%d target yaw=%f,pitch=%f",mUseTracker,yaw_t*180.0/M_PI,pitch_q*180.0/M_PI);
            cnt=0;
        }   
        float phi=roll_q;
        float theta=pitch_q;
        float psi=0;
        Eulerf et(phi,theta,psi);
        Quatf qt(et);
        mQ0=qt(0);
        mQ1=qt(1);
        mQ2=qt(2);
        mQ3=qt(3);
        CreateMavlink(roll_q,pitch_q,yaw_t,X1,Y1,mAltError,mAttackMode);
        mGuidedAlt=Alt-mTECS_DALT;
        return ResSendControl;

    }
    
    if(mCAM_SRV_CH)
    {
        if(trackerNew&&mEnableTracker)
        {
            if(trackerV)
            {
                float camPitch=(mHeight/2.0-trackerY)*mKy;
                mUseTracker=true;
                /*mCAM_CURRENT-=camPitch*mCAM_SRV_KI;
                if(mCAM_CURRENT<mCAM_ANG_MIN)
                    mCAM_CURRENT=mCAM_ANG_MIN;
                if(mCAM_CURRENT>mCAM_ANG_MAX)
                    mCAM_CURRENT=mCAM_ANG_MAX;    
                Ang2PWM(mCAM_CURRENT);*/
                CamYTrecker(camPitch,false);
                
                return ResSendCamPWM;
            }
            else
            {
                mUseTracker=false;
            }    
        }
        CreateMavlink(0,mCAM_CURRENT*M_PI/180.0f,0,0,0,0,CameraManual);

        if(mNewCamData&&!mUseTracker)
        {
            mNewCamData=false;
            float ang;
            float target=100.0f-mCAM_TARGET;
            if(target<0)
                target=0;
            float dang=mCAM_ANG_MAX-mCAM_ANG_MIN;
            ang=mCAM_ANG_MIN+target*dang/100.0f;
            if(fabs(mCAM_CURRENT-ang)>0.1)
            {
                Ang2PWM(ang);
                mCAM_CURRENT=ang;
                return ResSendCamPWM;
            }
            else
            {
                return ResContinue;
            }

        }
        else
        {
            if(mCAM_CURRENT<0.0f)
            {
                mCAM_CURRENT=mCAM_ANG_CRUISE;
                Ang2PWM(mCAM_CURRENT);
                log_i("Set camera to CAM_ANG_CRUISE PWM=%d",mCamPWM);
                return ResSendCamPWM;
            }
        }
    }
    else
    {
        CreateMavlink(0,mCAM_CURRENT*M_PI/180.0f,0,0,0,0,mAttackMode);
    }
    cwpn=0;
    in_corr=false;
    //mUseTracker=false;
    mAttackMode=AttackNone;
    return ResContinue;
}
void StateMachine::CamYTrecker(float camPitch,bool on_attack)
{
    if(on_attack && camPitch<0)
        return;
    
    if(fabs(camPitch)>1e-5)
    {
        float errorI=camPitch*mCAM_SRV_KI;
        if(errorI>mCAM_RATE_MAX)
           errorI=mCAM_RATE_MAX;
        if(errorI<-mCAM_RATE_MAX)
           errorI=-mCAM_RATE_MAX;            
        mCAM_CURRENT-=errorI;

        float errorP=camPitch*mCAM_SRV_KP;
        if(errorI+errorP>mCAM_RATE_MAX)
            errorP=mCAM_RATE_MAX-errorI;
        if(errorI+errorP<-mCAM_RATE_MAX)
            errorP=-mCAM_RATE_MAX-errorI;
            
        float ang=mCAM_CURRENT-errorP;

        if(mCAM_CURRENT<mCAM_ANG_MIN)
            mCAM_CURRENT=mCAM_ANG_MIN;
        if(mCAM_CURRENT>mCAM_ANG_MAX)
            mCAM_CURRENT=mCAM_ANG_MAX;    
        if(ang>mCAM_ANG_MAX)
            ang=mCAM_ANG_MAX;
        if(ang<mCAM_ANG_MIN)
            ang=mCAM_ANG_MIN;
        

        Ang2PWM(ang);
        mSendPWM=true;    
    }
    
}


void StateMachine::Ang2PWM(float v)
{
    float K=(mCAM_SRV_MAX-mCAM_SRV_MIN)/(mCAM_ANG_MAX-mCAM_ANG_MIN);
    float pwm;
    if(mCAM_SRV_REVERSE)
    {
        pwm=mCAM_SRV_MAX-K*(v-mCAM_ANG_MIN);
    }
    else
    {
        pwm=mCAM_SRV_MIN+K*(v-mCAM_ANG_MIN);
    }
    mCamPWM=roundf(pwm);
    if(mCamPWM>mCAM_SRV_MAX)
        mCamPWM=mCAM_SRV_MAX;
    if(mCamPWM<mCAM_SRV_MIN)
        mCamPWM=mCAM_SRV_MIN;    
}

void StateMachine::SetTrackerData(float x,float y,uint8_t v,uint8_t mode,uint16_t cnt)
{
    mMutex.Lock();
    mTrackerX=x;
    mTrackerY=y;
    mTrackerValid=v;
    mTrackerMode=mode;
    mTrackerMsgCnt=cnt;
    mNewTrackerData=true;
    mMutex.Unlock();
}

void StateMachine::CreateMavlink(float r,float p,float y,float X,float Y,float Z,float mode)
{
    mavlink_message_t msg;
    mavlink_vision_position_delta_t pos;
    pos.angle_delta[0]=r;
    pos.angle_delta[1]=p;
    pos.angle_delta[2]=y;
    pos.position_delta[0]=X;
    pos.position_delta[1]=Y;
    pos.position_delta[2]=Z;
    pos.confidence=mode;
    mavlink_msg_vision_position_delta_encode(1,1,&msg,&pos);
    mSendLen = mavlink_msg_to_send_buffer(mBufferOutStream, &msg);
    mMavlinkNew=true;

}

void StateMachine::SetFlapVAL(float val)
{
    mMutex.Lock();
    mFLAP_VAL=val;
    if(val<0)
        val=fabs(val);
    if(val>1000)
        val=1000;
    mFLAP_PWM=1000+val;        
    mMutex.Unlock();
}

uint8_t StateMachine::SafeMode()
{
    float Lat0,Lon0,Lat1,Lon1,X1,Y1,D,Alt,yaw,roll,pitch;
    
    mY=mZ=mR=mX=0;
    mMutex.Lock();
    Lat0=mLat*M_PI/180.0;
    Lon0=mLon*M_PI/180.0;
    Alt=mAlt;
    roll=mRoll;
    pitch=mPitch;
    yaw=mYaw;
    mMutex.Unlock();
    if(mEnterSafeMode)
    {
        mEnterSafeMode=false;

        float phi=0;
        float theta=mSafePitch*M_PI/180.0f;
        float psi=0;
        Eulerf et(phi,theta,psi);
        Quatf qt(et);
        mQ0=qt(0);
        mQ1=qt(1);
        mQ2=qt(2);
        mQ3=qt(3);

        float d=mSafeSpeed*mSafeTime*cos(theta);
        float dAlt=mSafeSpeed*mSafeTime*sin(theta);

        float X=cos(yaw)*d;
        float Y=sin(yaw)*d;
        float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
        float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
        CustomGuidedWp=GuidedWp;
        CustomGuidedWp.x=LatT;
        CustomGuidedWp.y=LonT;
        CustomGuidedWp.z=Alt+dAlt;
        mGuidedAlt=Alt+dAlt;
        return ResSendWpAndControl;
    }
    if(mTargetWpNum>1  && mCurrWpNum!=TargetWp.seq && mCustomMode!=PLANE_MODE_GUIDED)
    {
        return ResSwitchNavigation;
    }
    return ResContinue;
}