//
// Created by bastian on 28.03.22.
//

#include "MoveConnection.h"

MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {}

void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
                                    const std::function<void(bool)> &callback) {
    std::cout<<"planAndExecute."<<std::endl;
    lock.lock();
    state = RUNNING;
    lastTarget = pose;
    lastCallback = callback;

    moveGroup.setMaxVelocityScalingFactor(currentSpeed);
    moveGroup.setMaxAccelerationScalingFactor(1);
    
    moveGroup.setPoseTarget(*lastTarget);
    moveGroup.setGoalJointTolerance(0.01);
    moveGroup.setGoalOrientationTolerance(0.01);

    std::cout<<"Starting planning"<<std::endl;

    std::thread([&](){
        std::cout<<"Parameters set."<<std::endl;
        
        auto finished = moveGroup.move() == moveit::core::MoveItErrorCode::SUCCESS;
        
        if(state!=SPEED_CHANGE){
            lastCallback( finished );
            lastTarget = nullptr;
        }
        
        state = IDLE;
        lock.unlock();
    }).detach();
}

void MoveConnection::setSpeedMultiplier(const double speed){
    if(currentSpeed!=speed) {
        std::cout<<"Changing speed."<<std::endl;
        currentSpeed = speed;
        auto target = lastTarget;
        auto callback = lastCallback;
        if (target != nullptr) {
            state = SPEED_CHANGE;
            stop();
            std::cout<<"Restarting"<<std::endl;
            planAndExecute(target, callback);
        }
    }
}

void MoveConnection::stop() {
    std::cout<<"Stopping MoveConnection"<<std::endl;
    while (rclcpp::ok() && state!=IDLE){
        moveGroup.stop();
        usleep(1000);
    }
    std::cout<<"Stopped MoveConnection"<<std::endl;
}
