/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "NJointHolonomicPlatformVelocityControllerWithRamp.h"

#include <ArmarXCore/core/exceptions/local/ExpressionException.h>

#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>

namespace armarx
{
    NJointHolonomicPlatformVelocityControllerWithRamp::
        NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
                                                          ConfigPtrT cfg,
                                                          const VirtualRobot::RobotPtr&) :
        ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
    {
        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
                     ->asA<ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(target)
            << "The actuator " << cfg->platformName << " has no control mode "
            << ControlModes::HolonomicPlatformVelocity;

        const auto sensor = useSensorValue(cfg->platformName);
        ARMARX_CHECK_EXPRESSION(sensor) << "No sensor value for " << cfg->platformName;
        velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(velocitySensor)
            << "Sensor value for " << cfg->platformName << " has invalid type";
    }

    void
    NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
    {
        activationVelocity(0) = velocitySensor->velocityX;
        activationVelocity(1) = velocitySensor->velocityY;
        activationVelocity(2) = velocitySensor->velocityRotation;
        activationTime = IceUtil::Time::now();

        // init velocity ramp
        ramp.init(activationVelocity);
    }

    void
    NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;

        if (commandAge > maxCommandDelay && // command must be recent
            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
             rtGetControlStruct().velocityRotation !=
                 0.0f)) // only throw error if any command is not zero
        {
            throw LocalException(
                "Platform target velocity was not set for a too long time: delay: ")
                << commandAge.toSecondsDouble()
                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
        }
        else
        {
            Eigen::Vector3f result;
            if (activationTime > rtGetControlStruct().commandTimestamp)
            {
                // No valid command has been specified. Sane default: come to rest.
                const Eigen::Vector3f targetVelocity = Eigen::Vector3f::Zero();
                result = ramp.update(targetVelocity, timeSinceLastIteration.toSecondsDouble());
            }
            else
            {
                Eigen::Vector3f x(rtGetControlStruct().velocityX,
                                  rtGetControlStruct().velocityY,
                                  rtGetControlStruct().velocityRotation);
                result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
            }

            target->velocityX = result(0);
            target->velocityY = result(1);
            target->velocityRotation = result(2);
        }
    }

    void
    NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
        float maxPositionAcceleration,
        float maxOrientationAcceleration)
    {
        ramp.setMaxPositionAcceleration(maxPositionAcceleration);
        ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
    }

    NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
        registrationNJointHolonomicPlatformVelocityControllerWithRamp(
            "NJointHolonomicPlatformVelocityControllerWithRamp");

    Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
                                                          float maxOrientationAcceleration) :
        maxPositionAcceleration(maxPositionAcceleration),
        maxOrientationAcceleration(maxOrientationAcceleration)
    {
    }

    void
    Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
    {
        this->state = state;
    }

    Eigen::Vector3f
    Cartesian2DimVelocityRamp::update(const Eigen::Vector3f& target, float dt)
    {
        if (dt <= 0)
        {
            return state;
        }
        Eigen::Vector3f delta = target - state;
        float factor = 1;

        Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
        float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
        factor = std::max(factor, posFactor);

        float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
        factor = std::max(factor, oriFactor);

        state += delta / factor;
        return state;
    }

    void
    Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
    {
        this->maxPositionAcceleration = maxPositionAcceleration;
    }

    void
    Cartesian2DimVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
    {
        this->maxOrientationAcceleration = maxOrientationAcceleration;
    }

} // namespace armarx