SimplePeriodicSpecializedSkill.h

#pragma once

#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include "PeriodicSkill.h"
#include "SimpleSpecializedSkill.h"

namespace armarx
{
    namespace skills
    {
        template <class AronT>
        class SimplePeriodicSpecializedSkill : public SimpleSpecializedSkill<AronT>
        {

        public:
            using Base = SimpleSpecializedSkill<AronT>;
            using ParamType = AronT;

            using Base::Base;

            using StepResult = PeriodicSkill::StepResult;

            SimplePeriodicSpecializedSkill(const SkillDescription& skillDescription,
                                           const armarx::Frequency& frequency) :
                Base(skillDescription), frequency(frequency)
            {
            }

        protected:
            /// Do not use anymore
            Skill::MainResult
            main(const typename Base::SpecializedMainInput& in) final
            {
                armarx::core::time::Metronome metronome(frequency);

                while (true)
                {
                    this->throwIfSkillShouldTerminate();

                    const auto res = step(in);
                    switch (res.status)
                    {
                        case ActiveOrTerminatedSkillStatus::Running:
                            // nothing to do here. break switch
                            break;
                        case ActiveOrTerminatedSkillStatus::Aborted:
                            return Skill::MakeAbortedResult();
                        case ActiveOrTerminatedSkillStatus::Succeeded:
                            return Skill::MakeSucceededResult(res.data);
                        case ActiveOrTerminatedSkillStatus::Failed:
                            return Skill::MakeFailedResult();
                    }

                    const auto sleepDuration = metronome.waitForNextTick();
                    if (not sleepDuration.isPositive())
                    {
                        ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
                                    << ": execution took too long (" << -sleepDuration
                                    << " too long. Expected " << frequency.toCycleDuration() << ")";
                    }
                }

                // never happens
                throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
            }

            /// Override this method with your own step function
            virtual StepResult
            step(const typename Base::SpecializedMainInput& in)
            {
                ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillId
                                 << "'. Please overwrite this method!";
                return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
            }

        protected:
            const armarx::Frequency frequency;
        };
    } // namespace skills
} // namespace armarx