#include "NJointKneeHipController.h"
#include <algorithm>
#include <cmath>

#include <SimoxUtility/math/distance/angle_between.h>

#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include "RobotAPI/components/units/RobotUnit/ControlModes.h"
#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h"


// #include <armar7/rt/units/njoint_controller/common/controller_types.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron/ControllerConfig.aron.generated.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron_conversions.h>




namespace armarx

{
    const armarx::NJointControllerRegistration<NJointKneeHipController>
        registrationControllerNJointKneeHipController(
            "NJointKneeHipController");

    NJointKneeHipController::NJointKneeHipController(RobotUnit* robotUnit,
                                         const armarx::NJointControllerConfigPtr& config,
                                         const VirtualRobot::RobotPtr&)
    {
        ARMARX_RT_LOGF("Creating knee hip controller");

        ARMARX_CHECK_EXPRESSION(robotUnit);

        // config
        // ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        // ARMARX_CHECK_EXPRESSION(cfg);

        // const arondto::Config configData = arondto::Config::FromAron(cfg->config);

        // robot
        ARMARX_RT_LOGF("Setting up nodes");
        {
            _rtRobot = useSynchronizedRtRobot();
            _rtRobot->setThreadsafe(false);
            ARMARX_CHECK_NOT_NULL(_rtRobot);

            // _rtKneeNode = _rtRobot->getRobotNode(configData.params.kneeJointName);
            // _rtHipNode = _rtRobot->getRobotNode(configData.params.hipJointName);
            // _rtHipBodyNode = _rtRobot->getRobotNode(configData.params.hipBodyName);
            const auto *kneeJointName = "Knee";
            const auto *hipJointName = "Hip";
            const auto *hipBodyName = "Hip_body";


            _rtKneeNode = _rtRobot->getRobotNode(kneeJointName);
            _rtHipNode = _rtRobot->getRobotNode(hipJointName);
            _rtHipBodyNode = _rtRobot->getRobotNode(hipBodyName);
            ARMARX_CHECK_NOT_NULL(_rtKneeNode);
            ARMARX_CHECK_NOT_NULL(_rtHipNode);
            ARMARX_CHECK_NOT_NULL(_rtHipBodyNode);

            // auto kneeBase = useControlTarget(kneeJointName, armarx::ControlModes::Position1DoF);
            // _rtKneeCtrlTarget = kneeBase->asA<ControlTarget1DoFActuatorPosition>();
            // ARMARX_CHECK_NOT_NULL(_rtKneeCtrlTarget);

            // _rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
            //     hipJointName, armarx::ControlModes::Position1DoF);
            auto hipBase = useControlTarget(hipJointName, armarx::ControlModes::Velocity1DoF);
            _rtHipCtrlTarget = hipBase->asA<ControlTarget1DoFActuatorVelocity>();
            ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
        }
        const SensorValueBase* sv = useSensorValue(_rtKneeNode->getName());
        velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
        // positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();

        ARMARX_RT_LOGF("Nodes set up successfully");
    }

    NJointKneeHipController::~NJointKneeHipController() = default;

    // void
    // KneeHipController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
    //                                 const Ice::Current& /*iceCurrent*/)
    // {
    //     // const auto updateConfigDto = arondto::Config::FromAron(dto);

    //     // Config config;
    //     // fromAron(updateConfigDto, config);

    //     // setControlStruct(config);
    // }

    void
    NJointKneeHipController::onInitNJointController()
    {
    }

    void
    NJointKneeHipController::onConnectNJointController()
    {
    }

    std::string
    NJointKneeHipController::getClassName(const Ice::Current&) const
    {
        return "NJointKneeHipController";
    }

    double
    NJointKneeHipController::calculate_dAnkle_dKnee(double kneeAngle, double shank, double p1, double p2, double p3)
    {
        constexpr auto squared = [](const double t) { return t * t; };
        const double k1 = shank / p1;
        const double k2 = shank / p3;
        const double k3 = (squared(shank) + squared(p1) + squared(p3) - squared(p2)) / (2 * p1 * p3);
        const double theta = kneeAngle;

        double T_fkt = sin(theta) * (((squared(k1)+k1+1)*k2-k1*k3)*cos(theta)-squared(k3)+(k1-1)*k2*k3+k1*squared(k2)+k1+1)
        +((k3+k2)*cos(theta)+k1+1)*sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1);


        double B_fkt = sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1)
                * (sin(theta) * sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1)
                + k1*squared(cos(theta))+(k3+(k1+2)*k2)*cos(theta)+k2*k3+squared(k2)+1);


        double dAnkle_dKnee =  T_fkt / B_fkt;

        return dAnkle_dKnee;
    }


    // double NJointKneeHipController::calculate_psi(double kneeAngle, double shank, double p1, double p2, double p3)
    // {
    //     constexpr auto squared = [](const double t) { return t * t; };
    //     const double k1 = shank / p1;
    //     const double k2 = shank / p3;
    //     const double k3 = (squared(shank) + squared(p1) + squared(p3) - squared(p2)) / (2 * p1 * p3);
    //     const double theta = kneeAngle;

    //     /* Solution of Freudenstein Equation*/
    //     double psi = 2 * atan((cos(theta) + sqrt(cos(theta) * cos(theta) -
    //                                             4 * (k1 * cos(theta) + cos(theta) + k2 + k3) *
    //                                                 (k1 * cos(theta) - cos(theta) - k2 + k3))) /
    //                          (k1 * cos(theta) + cos(theta) + k2 + k3));

    //     return psi;
    // }


    void
    NJointKneeHipController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                             const IceUtil::Time& timeSinceLastIteration)
    {

        auto& debugData = bufferRtToOnPublish_.getWriteBuffer();
        


        double kneeVelocity = velocitySensor->velocity;
        debugData.kneeVelocity = kneeVelocity;
        
        double kneeAngle = _rtKneeNode->getJointValue();
        debugData.kneeAngle = kneeAngle;


        double deltaKnee = kneeVelocity;
        double dHip_dKnee = 1 - calculate_dAnkle_dKnee(kneeAngle, 280, 84.372, 269.964, 45);

        double deltaHip =  dHip_dKnee * deltaKnee;
        double hipVelocity = deltaHip;
        debugData.hipVelocity = hipVelocity;

        std::clamp(hipVelocity, -0.5, 0.5);
        _rtHipCtrlTarget->velocity = hipVelocity;
        


        /*  Position approach  */

        // double theta = _rtKneeNode->getJointValue();
        // double psi = calculate_psi(theta, 280, 84.372, 269.964, 45);
        // double target = theta - psi + M_PI/5;

        // _rtHipCtrlTarget->position = target;

        bufferRtToOnPublish_.commitWrite();
    }

    void
    NJointKneeHipController::rtPreActivateController()
    {
    }

    void
    NJointKneeHipController::rtPostDeactivateController()
    {
    }

    void
    NJointKneeHipController::onPublish(const SensorAndControl&,
                                       const DebugDrawerInterfacePrx&,
                                       const DebugObserverInterfacePrx& debugObservers)
    {

        StringVariantBaseMap datafields;

        const auto& debugData = bufferRtToOnPublish_.getUpToDateReadBuffer();

        datafields["localTimestamp"] = new Variant(debugData.localTimestamp);

        datafields["kneeAngle"] = new Variant(debugData.kneeAngle);
        datafields["kneeVelocity"] = new Variant(debugData.kneeVelocity);
        datafields["hipAngle"] = new Variant(debugData.hipAngle);
        datafields["hipVelocity"] = new Variant(debugData.hipVelocity);

        debugObservers->setDebugChannel("NJointKneeHipController",
                                        datafields);

    }


} // namespace armarx