/*
 * 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 <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>

#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <cmath>

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(cfg->platformName);
        this->sv = sv->asA<SensorValueHolonomicPlatform>();
        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)
    {
        currentPosition << sv->relativePositionX, sv->relativePositionY;
        currentOrientation = sv->relativePositionRotation;


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

        getWriterControlStruct().newTargetSet = false;
        writeControlStruct();
	
            isAborted = false;
        }
        if (isAborted)
        {
            return;
        }
        else if ((rtGetControlStruct().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;
        }

        const float measuredOrientation = rtGetControlStruct().globalOrientation;

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

        const Eigen::Rotation2Df global_R_local(-measuredOrientation);

        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()
    {
    }

    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_PIf32, M_PIf32);
        getWriterControlStruct().translationAccuracy = translationAccuracy;
        getWriterControlStruct().rotationAccuracy = rotationAccuracy;
        getWriterControlStruct().newTargetSet = true;
        writeControlStruct();
    }


    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
    {
        // ..todo: check if norm is too large

        getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
        getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);

        getWriterControlStruct().startPosition = currentPosition;
        getWriterControlStruct().startOrientation = currentOrientation;

        getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
        writeControlStruct();


    }

} // namespace armarx