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

#include "ActorSystem.hpp"

IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate,
                    ignition::gazebo::ActorSystem::ISystemConfigure)

using namespace ignition::gazebo;

ActorSystem::ActorSystem() = default;

ActorSystem::~ActorSystem() = default;

void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationName) {
    igndbg << "Initial setup for new animation." << std::endl;

    auto actorComp = _ecm.Component<components::Actor>(entity);

    auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
    *animationNameComp = components::AnimationName(animationName);
    _ecm.SetChanged(entity, components::AnimationName::typeId, ComponentState::OneTimeChange);

    auto animationTimeComponent = _ecm.Component<components::AnimationTime>(entity);
    *animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero());
    _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);

    int foundAnimationIndex = -1;
    for (int animationIndex = 0; animationIndex < actorComp->Data().AnimationCount(); animationIndex++) {
        auto animation = actorComp->Data().AnimationByIndex(animationIndex);
        if (animation->Name() == animationName) {
            foundAnimationIndex = animationIndex;
            break;
        }
    }

    if (foundAnimationIndex != -1) {
        igndbg << "Found animation to play" << std::endl;
        auto fileName = actorComp->Data().AnimationByIndex(foundAnimationIndex)->Filename();

        currentSkeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();

        igndbg << "FileName: " << fileName << std::endl;
        igndbg << "AnimCnt: " << currentSkeleton->AnimationCount() << std::endl;

        auto rootNode = currentSkeleton->RootNode()->Name();

        currentRootNodeAnimation = currentSkeleton->Animation(0)->NodeAnimationByName(rootNode);

        if (currentSkeleton != nullptr) {
            duration = currentSkeleton->Animation(0)->Length();
            igndbg << "length: " << duration << " seconds." << std::endl;
        } else {
            ignerr << "No skeleton found!" << std::endl;
        }

        auto initialPose = currentRootNodeAnimation->FrameAt(0).Pose();
        auto poseComponent = _ecm.Component<components::Pose>(entity);

        auto newPose = initialPose;
        newPose.Pos().X(0);
        newPose.Pos().Y(0);
        *poseComponent = components::Pose(newPose);
    } else {
        ignerr << "No animation found!" << std::endl;
    }
}

double Angle(ignition::math::Quaterniond a,ignition::math::Quaterniond b){
    auto dot = fmin(abs(a.Normalized().Dot(b.Normalized())),1.0);
    return acos(dot);
}

void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
    threadMutex.lock();

    if(nextState != currentState){
        igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
        currentState = nextState;
        feedback.progress = 0.0f;
        sendFeedback();
    }

    switch (currentState) {
    case SETUP: {
        igndbg << "Setting up actor plugin" << std::endl;

        auto actorComponent = _ecm.Component<components::Actor>(entity);

        auto animationNameComponent = _ecm.Component<components::AnimationName>(entity);
        if (nullptr == animationNameComponent) {
            _ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name()));
        }

        auto animTimeComponent = _ecm.Component<components::AnimationTime>(entity);
        if (nullptr == animTimeComponent) {
            _ecm.CreateComponent(entity, components::AnimationTime());
        }

        auto poseComponent = _ecm.Component<components::Pose>(entity);
        if (nullptr == poseComponent) {
            _ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero));
        }

        auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
        if (nullptr == trajectoryPoseComponent) {
            _ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
        }

        addPoseToFeedback(startPose);

        nextState = IDLE;
        break;
    }
    case ANIMATION: {
        if (lastState == IDLE) {
            switchAnimation(_ecm, animation_name);
        }
        auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
        auto newTime = animationTimeComponent->Data() + _info.dt;
        auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
        
        auto progress = newTimeSeconds / (duration/animation_speed);
        feedback.progress = progress;
        sendFeedback();

        if (progress >= 1.0) {
            igndbg << "Animation " << animation_name << " finished." << std::endl;
            nextState = IDLE;
            feedback.progress = 0.0f;
            sendFeedback();
            break;
        }

        *animationTimeComponent = components::AnimationTime(newTime);
        _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);

        break;
    }
    case MOVEMENT: {
        auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
        auto actorPose = trajectoryPoseComp->Data();

        auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
        auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
        auto deltaTimeSeconds = std::chrono::duration<double>(_info.dt).count();
        auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
        auto newTime = animationTimeComponent->Data() + _info.dt;

        auto turnSpeed = 1;

        if (lastState == IDLE) {
            igndbg << "Starting Movement..." << std::endl;
            switchAnimation(_ecm, animation_name);
            
            movementDetails.state = movementDetails.ROTATE_TO_TARGET;
            movementDetails.time = 0.0;
            movementDetails.stepTime = 0.0;
            movementDetails.start = actorPose;
            movementDetails.targetDiff.Pos() = movementDetails.target.Pos() - movementDetails.start.Pos();

            if(movementDetails.targetDiff.Pos().Length() >= 0.001){
                movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
                movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_speed * duration;
            }else{
                movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
                movementDetails.rotateToTargetDuration = 0.0;
                movementDetails.moveDuration = 0.0;
            }
            
            if( 
                movementDetails.target.Rot().W() == 0 &&
                movementDetails.target.Rot().X() == 0 &&
                movementDetails.target.Rot().Y() == 0 &&
                movementDetails.target.Rot().Z() == 0
            ){
                movementDetails.target.Rot() = movementDetails.targetDiff.Rot();
            }

            movementDetails.rotateToEndDuration = Angle(movementDetails.targetDiff.Rot(),movementDetails.target.Rot()) / turnSpeed;
            
            igndbg << movementDetails.rotateToTargetDuration << " " <<movementDetails.moveDuration << " " << movementDetails.rotateToEndDuration << std::endl;
        }
        
        movementDetails.time += deltaTimeSeconds;
        movementDetails.stepTime += deltaTimeSeconds;

        if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
            if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
                newTime = newTime.zero();
                actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
            }else{
                actorPose.Rot() = movementDetails.targetDiff.Rot();
                movementDetails.state = movementDetails.MOVE;
                movementDetails.stepTime -= movementDetails.rotateToTargetDuration;
            }
        }

        if (movementDetails.state == movementDetails.MOVE){
            if(movementDetails.stepTime<=movementDetails.moveDuration){
                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(fmod(movementDetails.stepTime,duration)));
                actorPose.Pos()=movementDetails.start.Pos()+(movementDetails.targetDiff.Pos()*(movementDetails.stepTime/movementDetails.moveDuration));
            }else{
                actorPose.Pos()=movementDetails.target.Pos();
                movementDetails.state = movementDetails.ROTATE_TO_END;
                movementDetails.stepTime -= movementDetails.moveDuration;
            }
        }

        if (movementDetails.state == movementDetails.ROTATE_TO_END){
            if(movementDetails.stepTime<=movementDetails.rotateToEndDuration){
                actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToEndDuration,movementDetails.targetDiff.Rot(),movementDetails.target.Rot(),true);
            }else{
                actorPose.Rot() = movementDetails.target.Rot();
                movementDetails.state = movementDetails.END;
                movementDetails.stepTime -= movementDetails.rotateToEndDuration;
            }
        }

        if (movementDetails.state == movementDetails.END || movementDetails.state  == movementDetails.ROTATE_TO_END){
            if (newTime.count() < duration / 2){
                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds-(deltaTimeSeconds)));
                if(newTime.count()<=0){
                    newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(0.0));
                    if(movementDetails.state==movementDetails.END){
                        nextState = IDLE;
                    }
                }
            }else{
                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds+(deltaTimeSeconds)));
                if(newTime.count()>=duration){
                    newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(duration));
                    if(movementDetails.state == movementDetails.END){
                        nextState = IDLE;
                    }
                }
            }
        }

        addPoseToFeedback(actorPose);
        feedback.progress = movementDetails.time / (movementDetails.rotateToTargetDuration + movementDetails.moveDuration + movementDetails.rotateToEndDuration);
        sendFeedback();

        *trajectoryPoseComp = components::TrajectoryPose(actorPose);
        _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);

        *animationTimeComponent = components::AnimationTime(newTime);
        _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);

        break;
    }
    case IDLE:
        break;
    }

    lastState = currentState;

    threadMutex.unlock();
}

void ActorSystem::messageQueueInterface(const char name[256]) {
    char generatedName[256];

    strcpy(generatedName,name);
    strcat(generatedName,"Feedback");

    igndbg << "Waiting for " << generatedName << std::endl;
    feedbackQueue = (mqd_t) -1;
    while(feedbackQueue == (mqd_t) -1){
        feedbackQueue = mq_open(generatedName, O_RDWR);

        if (feedbackQueue == (mqd_t) -1) {
            if(errno!=ENOENT){
                perror(nullptr);
                return;
            }
            mq_close(feedbackQueue);
            sleep(100);
        }
    }

    strcpy(generatedName,name);
    strcat(generatedName,"Action");

    igndbg << "Waiting for " << generatedName << std::endl;
    actionQueue = (mqd_t) -1;
    while(actionQueue == (mqd_t) -1){
        actionQueue = mq_open(generatedName, O_RDWR);

        if (actionQueue == (mqd_t) -1) {
            if(errno!=ENOENT){
                perror(nullptr);
                return;
            }
            mq_close(actionQueue);
            sleep(100);
        }
    }

    igndbg << "Queues online, starting listener" << std::endl;

    std::thread([this] {
        ActionMessage receivedAction;
        while (true) {
            igndbg << "Waiting for action" << std::endl;

            mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr);

            threadMutex.lock();

            strcpy(animation_name, receivedAction.animationName);
            animation_speed = receivedAction.animationSpeed;
            movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
            movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
            nextState = receivedAction.state;
            
            igndbg << "Received Action; State: " << nextState << " Target: " << movementDetails.target.Pos() << " | " << movementDetails.target.Rot().Z() << std::endl;
            
            threadMutex.unlock();
        }
    }).detach();
}

void ActorSystem::addPoseToFeedback(ignition::math::Pose3<double> pose){
    feedback.current.position.x=pose.X();
    feedback.current.position.y=pose.Y();
    feedback.current.position.z=pose.Z();
    feedback.current.orientation.w=pose.Rot().W();
    feedback.current.orientation.x=pose.Rot().X();
    feedback.current.orientation.y=pose.Rot().Y();
    feedback.current.orientation.z=pose.Rot().Z();
}

void ActorSystem::sendFeedback(){
    feedback.state = currentState;
    if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)!=0){
        ignerr << "Error " << errno << " sending feedback." << std::endl;
    }
}

// Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
// https://github.com/gazebosim/gz-sim/blob/ign-gazebo6/src/systems/follow_actor/FollowActor.cc
void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &_ecm,
                            EventManager &) {
    
    igndbg << "actor plugin configuring..." << std::endl;

    auto actorComp = _ecm.Component<components::Actor>(_entity);
    if (nullptr == actorComp) {
        ignerr << "Plugin not attached to actor, terminating." << std::endl;
        return;
    }

    char topic[256] = "/ActorPlugin";
    if (_sdf->HasElement("topic")) {
        auto topicString = _sdf->Get<std::string>("topic");
        if (topicString.size() >= 256) {
            ignerr << "queue name too long, not starting plugin!";
            return;
        }
        strcpy(topic, topicString.c_str());
    }

    if (_sdf->HasElement("x")) {
        auto x = _sdf->Get<float>("x");
        startPose.SetX(x);
    }

    if (_sdf->HasElement("y")) {
        auto y = _sdf->Get<float>("y");
        startPose.SetY(y);
    }

    if (_sdf->HasElement("rot")) {
        auto rot = _sdf->Get<float>("rot");
        startPose.Rot().Euler(0,0,rot);
    }

    this->entity = _entity;

    messageQueueInterface(topic);

    nextState = SETUP;
    currentState = SETUP;
    lastState = SETUP;
}