//
// Created by bastian on 31.08.22.
//

#ifndef BUILD_ACTORSYSTEM_H
#define BUILD_ACTORSYSTEM_H

#include <cstddef>
#include <fcntl.h>
#include <mqueue.h>
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <unistd.h>
#include <optional>
#include <queue>
#include <string>
#include <cmath>

#include <sys/stat.h>
#include <sys/mman.h>

#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/common/Console.hh>
#include <ignition/common/Mesh.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/NodeAnimation.hh>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Types.hh>
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <sdf/Actor.hh>

#include <ros_actor_message_queue_msgs/MessageTypes.hpp>

using namespace ros_actor_message_queue_msgs;

#define AnimationActionServer rclcpp_action::Server<action::Animation>
#define MovementActionServer rclcpp_action::Server<action::Movement>

#define AnimationGoalPtr std::shared_ptr<const action::Animation::Goal>
#define AnimationServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Animation>>

#define MovementGoalPtr std::shared_ptr<const action::Movement::Goal>
#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>

namespace ignition {
namespace gazebo {

class ActorSystem : public System,
                    public ISystemPreUpdate,
                    public ISystemConfigure {

    private:
        ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
        ignition::math::Pose3d target_pose;
        double animation_speed;
        char animation_name[256];
        ActorPluginState nextState,currentState,lastState;
        double duration{};
        Entity entity{kNullEntity};
        std::mutex threadMutex;
        std::shared_ptr<common::Skeleton> currentSkeleton;
        common::NodeAnimation* currentRootNodeAnimation;
        mqd_t feedbackQueue;
        mqd_t actionQueue;
        FeedbackMessage feedback;

        struct {
            enum {ROTATE_TO_TARGET,MOVE,ROTATE_TO_END,END} state;
            ignition::math::Pose3d start;
            ignition::math::Pose3d targetDiff;
            ignition::math::Pose3d target;
            double rotateToTargetDuration,moveDuration,rotateToEndDuration,time,stepTime;
        } movementDetails;

    public:
        ActorSystem();
        ~ActorSystem() override;
        void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) override;
        void Configure(const ignition::gazebo::Entity &animationGoalPtr,
                       const std::shared_ptr<const sdf::Element> &_sdf,
                       EntityComponentManager &_ecm,
                       EventManager &/*_eventMgr*/) override;

    private:
        void switchAnimation(EntityComponentManager &_ecm, char *animationName);
        void messageQueueInterface(const char name[256]);
        void sendFeedback();
        void addPoseToFeedback(ignition::math::Pose3<double> pose);

};
}
}

#endif //BUILD_ACTORSYSTEM_H
