/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarX
 * @author     Markus Grotz (markus.grotz at kit dot edu)
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "NJointHolonomicPlatformGlobalPositionController.h"

#include <cmath>

#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>

#include "ArmarXCore/core/logging/Logging.h"

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

namespace armarx
{
    NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
        registrationNJointHolonomicPlatformGlobalPositionController(
            "NJointHolonomicPlatformGlobalPositionController");

    NJointHolonomicPlatformGlobalPositionController::
        NJointHolonomicPlatformGlobalPositionController(
            RobotUnit*,
            const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
            const VirtualRobot::RobotPtr&) :
        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
        opid(cfg->p_rot,
             cfg->i_rot,
             cfg->d_rot,
             cfg->maxRotationVelocity,
             cfg->maxRotationAcceleration,
             true)

    {
        const SensorValueBase* sv =
            useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
        this->sv = sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
                     ->asA<ControlTargetHolonomicPlatformVelocity>();


        pid.threadSafe = false;
        pid.preallocate(2);

        opid.threadSafe = false;
    }

    void
    NJointHolonomicPlatformGlobalPositionController::rtRun(
        const IceUtil::Time& currentTime,
        const IceUtil::Time& timeSinceLastIteration)
    {
        const auto global_T_robot = sv->global_T_root;

        const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
        const float global_orientation = rpy.z();
        const Eigen::Vector2f global_P_robot = global_T_robot.block<2, 1>(0, 3);

        if (rtGetControlStruct().newTargetSet)
        {
            pid.reset();
            opid.reset();

            getWriterControlStruct().newTargetSet = false;
            writeControlStruct();

            isTargetSet = true;
        }


        // if ((sv->lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
        // {
        //     ARMARX_RT_LOGF_WARNING  << deactivateSpam(0.5) << "Waiting for global pos";

        //     target->velocityX = 0;
        //     target->velocityY = 0;

        //     target->velocityRotation = 0;
        //     isAborted = true;

        //     return;
        // }

        if (not isTargetSet)
        {
            target->velocityX = 0;
            target->velocityY = 0;
            target->velocityRotation = 0;

            return;
        }

        if (not sv->isAvailable())
        {
            // ARMARX_RT_LOGF_INFO << deactivateSpam(1) << "global pose not available";
            target->velocityX = 0;
            target->velocityY = 0;
            target->velocityRotation = 0;

            return;
        }

        const float measuredOrientation = global_orientation;

        pid.update(
            timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
        opid.update(timeSinceLastIteration.toSecondsDouble(),
                    static_cast<double>(measuredOrientation),
                    rtGetControlStruct().targetOrientation);

        const Eigen::Rotation2Df global_R_local(-measuredOrientation);
        const Eigen::Vector2f velocities = global_R_local * pid.getControlValue();

        target->velocityX = velocities.x();
        target->velocityY = velocities.y();
        target->velocityRotation = static_cast<float>(opid.getControlValue());
    }

    void
    NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
    {
        target->velocityX = 0;
        target->velocityY = 0;
        target->velocityRotation = 0;
    }

    void
    NJointHolonomicPlatformGlobalPositionController::setTarget(float x,
                                                               float y,
                                                               float yaw,
                                                               float translationAccuracy,
                                                               float rotationAccuracy)
    {
        // todo do we really need a recursive mutex?

        std::lock_guard<std::recursive_mutex> lock(controlDataMutex);

        getWriterControlStruct().target << x, y;
        getWriterControlStruct().targetOrientation =
            simox::math::periodic_clamp(yaw, -M_PIf, M_PIf);

        getWriterControlStruct().translationAccuracy = translationAccuracy;
        getWriterControlStruct().rotationAccuracy = rotationAccuracy;

        getWriterControlStruct().newTargetSet = true;
        writeControlStruct();
    }

} // namespace armarx