/*
 * 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"

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


    NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
        const RobotUnitPtr&,
        const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
        const VirtualRobot::RobotPtr&) :
        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)

    {
        const SensorValueBase* sv = useSensorValue(cfg->platformName);
        this->sv = sv->asA<SensorValueHolonomicPlatform>();
        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity);

        pid.threadSafe = false;

        oriCtrl.maxV = cfg->maxRotationVelocity;
        oriCtrl.acceleration = cfg->maxRotationAcceleration;
        oriCtrl.deceleration = cfg->maxRotationAcceleration;
        oriCtrl.maxDt = 0.1;
        oriCtrl.pid->Kp = cfg->p;
        oriCtrl.positionPeriodLo = -M_PI;
        oriCtrl.positionPeriodHi = M_PI;
        pid.preallocate(2);

    }

    void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
    {
        currentPosition << sv->relativePositionX, sv->relativePositionY;
        currentOrientation = sv->relativePositionRotation;


        if (rtGetControlStruct().newTargetSet)
        {
            pid.reset();
            *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
        }

        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;

            return;
        }

        float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
        Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;

        Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
        float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
        updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));

        pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);

        oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
        oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
        oriCtrl.currentPosition = updatedOrientation;
        oriCtrl.targetPosition =  getWriterControlStruct().targetOrientation;
        oriCtrl.currentV = sv->velocityRotation;

        Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
        target->velocityX = localTargetVelocity(0);
        target->velocityY = localTargetVelocity(1);
        target->velocityRotation = oriCtrl.run();

        Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
        if (posError.norm() < rtGetControlStruct().translationAccuracy)
        {
            // target->velocityX = 0;
            // target->velocityY = 0;
        }

        float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
        orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
        if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
        {
            //target->velocityRotation = 0;
        }
    }

    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 = std::atan2(std::sin(yaw), std::cos(yaw));
        getWriterControlStruct().translationAccuracy = translationAccuracy;
        getWriterControlStruct().rotationAccuracy = rotationAccuracy;
        getWriterControlStruct().newTargetSet = true;
        writeControlStruct();
    }


    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
    {
        // ..todo:: lock :)

        getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
        getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;

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

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

} // namespace armarx