//
// Created by zx on 23-5-8.
//

#include "navigation_main.h"
#include "TimerRecord.h"


NavigationMain::NavigationMain() {
    is_master_AGV = true;
    move_mode_ = eSingle;
    wheelBase_ = 0;
}

NavigationMain::~NavigationMain() {

}

void NavigationMain::ResetPose(const Pose2d &pose) {
    if (move_mode_ == eDouble) {
        if (timedBrotherPose_.timeout() == true) {
            std::cout << "Brother pose is timeout can not set MainAGV pose" << std::endl;
            return;
        }
        //Pose2d transform(-wheelBase_/2.0,0,0);
        //Navigation::ResetPose(pose * transform);
        Pose2d brother = timedBrotherPose_.Get();
        Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));

        if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.5 || diff.theta() > 10 * M_PI / 180.0) {
            std::cout << " distance with two agv is too far diff: " << diff << std::endl;
            return;
        }
        Pose2d abs_diff = pose - brother;
        //计算两车朝向的方向
        float theta = pose.theta();//Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
        Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
        Pose2d abs_diff1 = pose - agv;
        Pose2d abs_diff2 = brother - agv;
        timeYawDiff1_.reset(pose.theta());

        timeYawDiff2_.reset((brother-pose).theta());
        Pose2d brotherInPose=Pose2d::relativePose(brother,pose);
        timeYawDiff1_.reset(brotherInPose.y());
        //timeYawDiff2_.reset(abs_diff2.theta());
        Navigation::ResetPose(pose);
//        timedPose_().m_diffYaw1=pose.theta()-theta;
//        timedPose_().m_diffYaw2=brother.theta()-theta;
//        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);

    } else
        Navigation::ResetPose(pose);
}

void NavigationMain::publish_statu(NavMessage::NavStatu &statu) {
    statu.set_main_agv(true);
    Navigation::publish_statu(statu);
}


void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
    /*if(move_mode_!=Monitor_emqx::eDouble)
    {
      printf(" navigation mode must set main,parameter:Pose2d\n");
      return false;
    }*/

    Navigation::Start(cmd, response);

}

void NavigationMain::SendMoveCmd(int clampLifterAction,int mode, ActionType type,
                                 double v[], double angular[]) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(clampLifterAction,mode, type, v, angular, wheelBase_);
    }
}

void NavigationMain::SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
                                 int space_id, double distance,double Y1,double Y2) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(clampLifterAction,mode, type, v, angular, wheelBase_,
                               space_id, distance,Y1,Y2);
        if (type == eRotation)
            RWheel_position_ = eR;
        if (type == eVertical)
            RWheel_position_ = eX;
        if (type == eHorizontal)
            RWheel_position_ = eY;
    }
}

bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
    if (Navigation::CreateRobotStatuMsg(robotStatu)) {
        robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
        robotStatu.mutable_agvstatu()->set_lifter_other(timed_other_lifter_.Get());
        return true;
    }
    //std::cout<<agvStatu.DebugString()<<std::endl;
    return false;
}

void NavigationMain::ResetOtherClamp(ClampStatu statu) {
    timed_other_clamp_.reset(statu, 1);
}

void NavigationMain::ResetOtherLifter(LifterStatus status) {
    timed_other_lifter_.reset(status, 1);
}

void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
    NavMessage::AgvStatu speed;
    if (IsMasterAGV()) {
        if (msg.toProtoMessage(speed)) {
            ResetStatu(speed.v(), speed.w());
            ResetClamp((ClampStatu) speed.clamp());
            ResetOtherClamp((ClampStatu) speed.clamp_other());
            ResetLifter((LifterStatus) speed.lifter());
            ResetOtherLifter((LifterStatus) speed.lifter_other());
            //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
        }
    } else {
        if (msg.toProtoMessage(speed)) {
            ResetStatu(speed.v(), speed.w());
            ResetClamp((ClampStatu) speed.clamp());
            ResetLifter((LifterStatus) speed.lifter());
        }
    }
    ResetDoor(speed.door());
    std::vector<int32_t> vecStatus;
    for(int i=0;i<speed.zcb_size();++i){
        vecStatus.push_back(speed.zcb(i));
    }
    ResetCarrier(vecStatus);
}

bool NavigationMain::clamp_close() {
    if (move_mode_ == eSingle)
        return Navigation::clamp_close();
    printf("双车夹持\n");
    if (monitor_) {
        monitor_->clamp_close(move_mode_);
        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
                printf("双车夹持completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_close(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::clamp_half_open() {
    if (move_mode_ == eSingle)
        return Navigation::clamp_half_open();

    if (monitor_) {
        printf("双车松夹持\n");
        monitor_->clamp_half_open(move_mode_);
        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eHalfOpened && timed_other_clamp_.Get() == eHalfOpened) {
                printf("双车松夹持completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_half_open(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::lifter_rise() {
    if (move_mode_ == eSingle) {
        return Navigation::lifter_rise();
    }
    if (monitor_) {
        printf("双车提升机构提升\n");
        actionType_ = eLifterRise;
        if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get()==eRose)
            return true;
        else {
            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
                if(!clamp_half_open()){
                    printf(" clamp half open failed\n");
                    return false;
                }
            }
        }

        while (cancel_ == false) {
            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
                printf("双车提升机构提升completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_rise(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::lifter_down() {
    if (move_mode_ == eSingle) {
        return Navigation::lifter_down();
    }
    if (monitor_) {
        printf("双车提升机构下降\n");
        actionType_ = eLifterDown;
        if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned)
            return true;
        else {
            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
                if(!clamp_half_open()){
                    printf(" clamp half open failed\n");
                    return false;
                }
            }
        }

        while (cancel_ == false) {
            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned) {
                printf("双车提升机构下降completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_down(move_mode_);
        }
        return false;
    }
    return false;
}

Navigation::MpcResult
NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
                                       NavMessage::PathNode &target,int clampLifterAction) {
    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
//    if (timedPose_.timeout()) {
        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
        return eMpcFailed;
    }

    NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();

    stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
    double acc_angular = 25 * M_PI / 180.0;
    double dt = 0.1;

    Pose2d current = timedPose_.Get();
    double x = space.x();
    double y = space.y();
    Pose2d vec(x - current.x(), y - current.y(), 0);
    double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y());
    bool isFront=isFront_.Get();
    if(vecTheta<0.)
        isFront=!isFront;

    //前车先到,后车进入2点,保持与车位一致的朝向
    if (isFront==false ) {
        printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
        if (move_mode_ == eSingle) {
            x -= wheelbase * cos(vecTheta);
            y -= wheelbase * sin(vecTheta);
            printf("确定车位点：eSingle\n");
        }
    } else { //当后车先到,倒车入库
        printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
        //rotated.mutable_theta() = space.theta();

    }

    if (move_mode_ == eDouble){
        if((!isFront_.Get() && vecTheta>0) || (isFront_.Get()&&vecTheta<=0)){
            x -= wheelbase * cos(vecTheta);
            y -= wheelbase * sin(vecTheta);
        }

        printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
    }

    Pose2d spaceInCurrent=Pose2d::relativePose(Pose2d(space.x(),space.y(),0),current);

    target.set_x(x);
    target.set_y(y);
    target.set_theta(current.theta());
    target.set_l(0.02);
    target.set_w(0.5);
    target.set_id(space.id());
    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());

    if (move_mode_ == eDouble && space.id().find("S1101") != -1){
        Pose2d nt(x,y,0);
        Pose2d ntInCurrent=Pose2d::relativePose(nt,current);
        nt=current*Pose2d(ntInCurrent.x(),0,0);
        nt.set_theta(current.theta());

        target.set_x(nt.x());
        target.set_y(nt.y());
        target.set_theta(nt.theta());
        target.set_l(0.02);
        target.set_w(0.5);
        target.set_id(space.id());
        std::cout<< "整车进出口：" << nt <<std::endl;
        return eMpcSuccess;
    }

    double angular_pericision = 0.5;
    if(GetSequenceType() == eSequencePick && TargetIsExport(space)){
        angular_pericision = 5;
    }

    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        Pose2d current_now = timedPose_.Get();
        if(spaceInCurrent.x()<0.)
            current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
        double yawDiff = vecTheta - current_now.theta();
        if (move_mode_ == eDouble) {
            double yawDiff2 = vecTheta - (Pose2d(0, 0, M_PI) + current_now).theta();
            if (fabs(yawDiff) > fabs(yawDiff2)) {
                yawDiff=yawDiff2;
            }
        }

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
        }, "Rotation_mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }

        //下发速度
        if (fabs(yawDiff) < angular_pericision * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
            Stop();
            if(WaitClampLifterStatu(clampLifterAction)==false)
                return eMpcFailed;
            usleep(1000*1000);
            Pose2d current_2 = timedPose_.Get();
            if(spaceInCurrent.x()<0.)
                current_2 = current_2.rotate(current_2.x(), current_2.y(), M_PI);
            yawDiff = vecTheta - current_2.theta();
            if (move_mode_ == eDouble) {
                double yawDiff2 = vecTheta - (Pose2d(0, 0, M_PI) + current_now).theta();
                if (fabs(yawDiff) > fabs(yawDiff2)) {
                    yawDiff=yawDiff2;
                }
            }
            if(fabs(yawDiff) < 0.3){
                return eMpcSuccess;
            }else{
                continue;
            }
        } else{
            const int down_count = 3;
            double v[down_count] = {0,0,0};
            double w[down_count] = {out[0], out[1], out[2]};
            SendMoveCmd(clampLifterAction,move_mode_, eRotation, v, w);
            actionType_ = eRotation;
            /*
            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
                   */
        }
        continue;

    }
    return eMpcFailed;
}