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


#include "service/grpc_touch_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "controller/touch_controller.h"


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

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

grpc::Status gRPCTouchServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {

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

    try {
        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
        ctrl::TouchController touch_controller;
        touch_controller.touch(robot,request->pose(),request->offset());
        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;
}