//
// Created by ros on 2/6/23.
//

#include "Server.hpp"
#include <condition_variable>

using namespace ros_actor_message_queue_msgs;

static const char* feedbackName;
static const char* actionName;

static std::mutex feedbackMutex;
static std::mutex rosMutex;

static std::condition_variable stateCondition;

static ActionMessage cancelAction;

void sendAction(mqd_t queue, ActionMessage *message) { 
    std::cout << "Sending action..." << std::endl;
    mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); 
    std::cout << "Sent action" << std::endl;
}

void waitForState(FeedbackMessage *currentFeedback,ActorPluginState state){
    std::unique_lock lock(feedbackMutex);
    stateCondition.wait(lock,[&currentFeedback,state]{
        return currentFeedback->state==state;
    });
    
    return;
}

int main(int argc, char **argv) {
    cancelAction.state = IDLE;
    rclcpp::init(argc, argv);

    auto node = rclcpp::Node::make_shared("ActorPlugin", "ActorPlugin");

    node->get_node_options().arguments();

    feedbackName = "/ActorPluginFeedback";
    actionName = "/ActorPluginAction";

    mq_attr queueAttributes;
    queueAttributes.mq_flags = 0;
    queueAttributes.mq_curmsgs = 0;
    queueAttributes.mq_maxmsg = 1;

    mq_unlink(feedbackName);
    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
    mqd_t feedbackQueue = mq_open(feedbackName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
    if(feedbackQueue == (mqd_t) -1){
        perror(nullptr);
        return -1;
    }

    std::atexit([]{
        std::cout << "Shutting down " << feedbackName << std::endl;
        mq_unlink(feedbackName);
    });
    
    mq_unlink(actionName);
    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage);
    mqd_t actionQueue = mq_open(actionName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
    if(actionQueue == (mqd_t) -1){
        perror(nullptr);
        return -1;
    }

    std::atexit([]{
        std::cout << "Shutting down " << actionName << std::endl;
        mq_unlink(actionName);
    });

    std::cout << "All message queues online." << std::endl;

    AnimationServerGoalPtr currentAnimationGoalPtr;
    MovementServerGoalPtr currentMovementGoalPtr;

    FeedbackMessage currentFeedback;
    ActionMessage currentAction;

    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback] {
        while (true) {
            FeedbackMessage newFeedback;
            if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){
                perror(nullptr);
            }
            feedbackMutex.lock();
            if(newFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
                auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
                feedback->progress = newFeedback.progress;
                currentAnimationGoalPtr->publish_feedback(feedback);
            }
            if(newFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
                auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
                feedback->current = newFeedback.current;
                currentMovementGoalPtr->publish_feedback(feedback);
            }
            if (newFeedback.state==IDLE) {
                if(currentFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
                    std::cout << "Finished Animation. " << std::endl;
                    currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
                    currentAnimationGoalPtr = nullptr;
                }
                if(currentFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
                    std::cout << "Finished Movement. " << std::endl;
                    currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
                    currentMovementGoalPtr = nullptr;
                }
            }
            currentFeedback = newFeedback;
            feedbackMutex.unlock();
            stateCondition.notify_all();
        }
    }).detach();

    auto animationServer = rclcpp_action::create_server<action::Animation>(
        node, "animation",
        [&currentAction, &currentFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
            // igndbg << "goal" << shared_mmap->currentState << std::endl;
            rosMutex.lock();

            if (currentFeedback.state == IDLE) {
                std::cout << "Accepting Animation..." << std::endl;
                
                currentAction.animationSpeed = animationGoal->animation_speed;
                strcpy(currentAction.animationName, animationGoal->animation_name.data());

                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
            } else {
                std::cout << "Rejecting Animation..." << std::endl;
                rosMutex.unlock();
                return rclcpp_action::GoalResponse::REJECT;
            }
        },
        [actionQueue, &currentFeedback](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
            if (currentFeedback.state == IDLE) {
                return rclcpp_action::CancelResponse::REJECT;
            } else {
                rosMutex.lock();
                std::cout << "Cancelling Animation..." << std::endl;
                sendAction(actionQueue, &cancelAction);
                
                waitForState(&currentFeedback, IDLE);
                std::cout << "Cancelled Animation..." << std::endl;
                rosMutex.unlock();

                return rclcpp_action::CancelResponse::ACCEPT;
            }
        },
        [actionQueue, &currentAction, &currentAnimationGoalPtr, &currentFeedback](const AnimationServerGoalPtr &animationGoalPtr) {
            feedbackMutex.lock();
            currentAnimationGoalPtr = animationGoalPtr;
            feedbackMutex.unlock();
            currentAction.state = ANIMATION;
            sendAction(actionQueue, &currentAction);

            waitForState(&currentFeedback, ANIMATION);
            rosMutex.unlock();
        });

    auto movementServer = rclcpp_action::create_server<action::Movement>(
        node, "movement",
        [&currentAction, &currentFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
            rosMutex.lock();
            if (currentFeedback.state == IDLE) {
                std::cout << "Accepting Movement..." << std::endl;
                
                currentAction.target = movementGoal->target;
                currentAction.animationSpeed = movementGoal->animation_distance;
                strcpy(currentAction.animationName, movementGoal->animation_name.data());

                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
            } else {
                std::cout << "Rejecting Movement..." << std::endl;
                rosMutex.unlock();
                return rclcpp_action::GoalResponse::REJECT;
            }
        },
        [actionQueue, &currentFeedback](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
            if (currentFeedback.state == IDLE) {
                return rclcpp_action::CancelResponse::REJECT;
            } else {
                rosMutex.lock();
                std::cout << "Cancelling Movement..." << std::endl;
                sendAction(actionQueue, &cancelAction);
                
                waitForState(&currentFeedback, IDLE);
                std::cout << "Cancelled Movement..." << std::endl;
                rosMutex.unlock();
                
                return rclcpp_action::CancelResponse::ACCEPT;
            }
        },
        [actionQueue, &currentAction, &currentFeedback, &currentMovementGoalPtr](const MovementServerGoalPtr &movementGoalPtr) {
            feedbackMutex.lock();
            currentMovementGoalPtr = movementGoalPtr;
            feedbackMutex.unlock();
            currentAction.state = MOVEMENT;
            sendAction(actionQueue, &currentAction);
            
            waitForState(&currentFeedback, MOVEMENT);
            rosMutex.unlock();
        });
    
    while(rclcpp::ok()){
        rclcpp::spin(node);
    }
}