//
// Created by lgv on 2025/8/15.
//


#include "service/grpc_humanoid_robot_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "cmvr/msgs/geometry.pb.h"

using namespace cmvr::service;
using namespace cmvr::device;
using namespace cmvr::api;
using google::protobuf::util::TimeUtil;


gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()){}


grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context,
    const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) {

    grpc::Status ret = grpc::Status::OK;

    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
        robot->torqueOff();
        response->set_success(true);
        response->set_error_message("");
    }catch (const std::exception& e) {
        response->set_success(false);
        response->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}


grpc::Status gRPCHumanoidRobotServiceImpl::torqueOn(grpc::ServerContext *context,
    const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) {

    grpc::Status ret = grpc::Status::OK;

    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
        robot->torqueOn();
        response->set_success(true);
        response->set_error_message("");
    }catch (const std::exception& e) {
        response->set_success(false);
        response->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}


grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context,
    const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) {
    grpc::Status ret = grpc::Status::OK;

    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
        std::vector<JointPoint> cmds{};
        for (const auto& jc : request->cmds()) {
            cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel());
        }
        robot->moveJ(cmds,request->vel(),request->acc());

        response->mutable_header()->set_success(true);
        response->mutable_header()->set_error_message("");
    }catch (const std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}

grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response)
{

    grpc::Status ret = grpc::Status::OK;
    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
        auto ee_link = request->ee_link();
        msgs::Pose3d pose;
        pose.mutable_position()->set_x(request->targetpose().x());
        pose.mutable_position()->set_y(request->targetpose().y());
        pose.mutable_position()->set_z(request->targetpose().z());
        pose.mutable_euler()->set_rx(request->targetpose().rx());
        pose.mutable_euler()->set_ry(request->targetpose().ry());
        pose.mutable_euler()->set_rz(request->targetpose().rz());

        robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc());
        response->mutable_header()->set_success(true);
        response->mutable_header()->set_error_message("");
    }catch (const std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();

    return ret;

}


grpc::Status gRPCHumanoidRobotServiceImpl::speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response)
{
    grpc::Status ret = grpc::Status::OK;
    try
    {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
        auto joint_name = request->joint_name();
        auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
        auto vel = request->vel();
        auto acc = request->acc();
        robot->speedJ(joint_name,dir,vel,acc);
    }catch (const std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}

grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response)
{
    grpc::Status ret = grpc::Status::OK;
    try
    {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
        auto ee_link = request->ee_link();
        auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
        auto vel = request->vel();
        auto acc = request->acc();
        auto cart = static_cast<device::RobotCartesian>(request->cart());

        robot->setToolFrame(ee_link);
        robot->speedL(cart,dir,vel,acc);
    }catch (const std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}

grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context,
    const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {

    grpc::Status ret = grpc::Status::OK;

    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());

        std::vector<cmvr::device::JointState> states{};
        robot->getJointsState(states);

        response->mutable_header()->set_success(true);
        response->mutable_header()->set_error_message("");

        for (const auto &js : states) {

            if (js.name != "WAIST_P" && js.name != "WAIST_Y") {
                auto joint_msg = response->add_state();
                joint_msg->add_name(js.name);
                joint_msg->add_position(js.position);
                joint_msg->add_velocity(js.velocity);
            }

        }
    }catch (const std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
    }
    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
    return ret;
}