#include "task/touch_screen_task/include/touch_screen_task.h"

#include <algorithm>
#include <cmath>
#include <exception>
#include <limits>
#include <sstream>
#include <unordered_map>

#include "common/base/logging/logger.h"
#include "common/math/proto_geometry.h"
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
#include "cmvr/config/touch_screen_algorithm_config.pb.h"
#include "manager/device_manager/include/device_manager.h"
#include "service/grpc/server/include/camera_operational_activity_registry.h"
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
#include <visp3/core/vpRotationMatrix.h>

namespace cmvr::task {

namespace {

device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix<double, 6, 1>& twist) {
    return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]};
}

Eigen::Matrix<double, 6, 1> toEigen6(const device::CartesianVelocity& twist) {
    Eigen::Matrix<double, 6, 1> out;
    out << twist.vx, twist.vy, twist.vz, twist.wx, twist.wy, twist.wz;
    return out;
}

bool computeLinearMoveDeltaTool(const Eigen::Matrix<double, 6, 1>& twist_base,
                                const double move_length,
                                Eigen::Vector3d& delta_out) {
    if (!std::isfinite(move_length) || move_length <= 0.0) {
        return false;
    }

    const Eigen::Vector3d linear = twist_base.head<3>();
    if (!linear.allFinite()) {
        return false;
    }

    const double linear_norm = linear.norm();
    if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) {
        return false;
    }

    delta_out = linear / linear_norm * move_length;
    return delta_out.allFinite();
}

using TouchScreenTaskConfig = cmvr::config::TouchScreenTaskConfig;

std::array<double, 6> toArray6(const Eigen::Matrix<double, 6, 1>& value) {
    return {{value[0], value[1], value[2], value[3], value[4], value[5]}};
}



std::vector<std::string> controlJointNames(const TouchScreenTaskConfig& config) {
    const auto& names = config.alignment().ibvs().control_joint_names();
    return {names.begin(), names.end()};
}

bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config,
                                       std::vector<double>& positions_out) {
    std::unordered_map<std::string, double> q_map;
    q_map.reserve(static_cast<size_t>(config.initialization().joint_positions_size()));
    for (const auto& joint : config.initialization().joint_positions()) {
        if (!joint.has_joint_name() || joint.joint_name().empty() || !joint.has_rad() ||
            !std::isfinite(joint.rad())) {
            return false;
        }
        q_map[joint.joint_name()] = joint.rad();
    }

    positions_out.clear();
    positions_out.reserve(static_cast<size_t>(
        config.alignment().ibvs().control_joint_names_size()));
    for (const auto& name : config.alignment().ibvs().control_joint_names()) {
        const auto it = q_map.find(name);
        if (it == q_map.end()) {
            return false;
        }
        positions_out.push_back(it->second);
    }
    return !positions_out.empty();
}

bool isTouchTriggered(const TouchScreenTaskConfig& config,
                      const double resultant_force_value) {
    return resultant_force_value >= config.touch().tactile().force_threshold();
}

double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
                         const cmvr::config::TouchScreenTactileCriterion criterion) {
    switch (criterion) {
        case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ:
            return static_cast<double>(point.fz);
        case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE:
            return point.magnitude();
    }
    return static_cast<double>(point.fz);
}

Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) {
    vpRotationMatrix R_visp;
    R_visp.buildFrom(rx, ry, rz);

    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
    for (int r = 0; r < 3; ++r) {
        for (int c = 0; c < 3; ++c) {
            R(r, c) = R_visp[r][c];
        }
    }
    return R;
}

double rotationErrorRad(const Eigen::Matrix3d& R_current,
                        const Eigen::Matrix3d& R_target) {
    if (!R_current.allFinite() || !R_target.allFinite()) {
        return std::numeric_limits<double>::infinity();
    }

    const Eigen::Matrix3d R_err = R_current * R_target.transpose();
    const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0);
    return std::acos(cos_angle);
}

Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
    const Eigen::AngleAxisd aa(R);
    if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) {
        return Eigen::Vector3d::Zero();
    }
    return aa.axis() * aa.angle();
}

bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
                                          const Eigen::Matrix3d& R_current,
                                          double& yaw_rad_out) {
    if (!R_target.allFinite() || !R_current.allFinite()) {
        return false;
    }

    const Eigen::Vector3d z_ref = R_target.col(2);
    const Eigen::Vector3d x_ref = R_target.col(0);
    const Eigen::Vector3d y_ref = R_target.col(1);

    Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0));
    if (in_plane.norm() <= 1e-9) {
        in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1));
    }
    const double in_plane_norm = in_plane.norm();
    if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) {
        return false;
    }

    in_plane /= in_plane_norm;
    yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane));
    return std::isfinite(yaw_rad_out);
}

bool appendRequestedTactileRegions(
    const device::AbstractDexHand::FingerType finger,
    const device::AbstractDexHand::TactileRegion region,
    std::vector<device::AbstractDexHand::TactileRegionKey>& regions_out) {
    using DeviceTactileRegion = device::AbstractDexHand::TactileRegion;

    switch (region) {
        case device::AbstractDexHand::TactileRegion::TIP:
            regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
            return true;
        case device::AbstractDexHand::TactileRegion::FINGER:
            regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
            return true;
        case device::AbstractDexHand::TactileRegion::PAD:
            regions_out.emplace_back(finger, DeviceTactileRegion::PAD);
            return true;
        case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE:
            if (finger != device::AbstractDexHand::FingerType::THUMB) {
                return false;
            }
            regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE);
            return true;
    }
    return false;
}

perception::AprilTagPerception::DepthPolicy toDepthPolicy(
    const cmvr::config::TouchScreenDepthPolicy policy) {
    switch (policy) {
        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE:
            return perception::AprilTagPerception::DepthPolicy::NONE;
        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER:
            return perception::AprilTagPerception::DepthPolicy::PREFER;
        case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE:
            return perception::AprilTagPerception::DepthPolicy::REQUIRE;
    }
    return perception::AprilTagPerception::DepthPolicy::NONE;
}

perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod(
    const cmvr::config::TouchScreenTargetPointMethod method) {
    switch (method) {
        case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE:
            return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
        case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE:
            return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE;
    }
    return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
}

device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) {
    switch (finger) {
        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY:
            return device::AbstractDexHand::FingerType::PINKY;
        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING:
            return device::AbstractDexHand::FingerType::RING;
        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE:
            return device::AbstractDexHand::FingerType::MIDDLE;
        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX:
            return device::AbstractDexHand::FingerType::INDEX;
        case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB:
            return device::AbstractDexHand::FingerType::THUMB;
    }
    return device::AbstractDexHand::FingerType::INDEX;
}

const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) {
    switch (finger) {
        case device::AbstractDexHand::FingerType::PINKY: return "PINKY";
        case device::AbstractDexHand::FingerType::RING: return "RING";
        case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE";
        case device::AbstractDexHand::FingerType::INDEX: return "INDEX";
        case device::AbstractDexHand::FingerType::THUMB: return "THUMB";
        case device::AbstractDexHand::FingerType::PALM: return "PALM";
    }
    return "UNKNOWN";
}

const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) {
    switch (region) {
        case device::AbstractDexHand::TactileRegion::TIP: return "TIP";
        case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER";
        case device::AbstractDexHand::TactileRegion::PAD: return "PAD";
        case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
        case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
    }
    return "UNKNOWN";
}

device::AbstractDexHand::TactileRegion toTactileRegion(
    const cmvr::config::TouchScreenTactileRegion region) {
    switch (region) {
        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP:
            return device::AbstractDexHand::TactileRegion::TIP;
        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER:
            return device::AbstractDexHand::TactileRegion::FINGER;
        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD:
            return device::AbstractDexHand::TactileRegion::PAD;
        case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE:
            return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE;
    }
    return device::AbstractDexHand::TactileRegion::TIP;
}

} // namespace

TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg)
    : id_(cfg.id()),
      tracker_(nullptr),
      config_(cfg) {
    config_valid_ = validateConfig(config_);
    if (!config_valid_) {
        last_status_ = Status::INVALID_CONFIG;
    }
}

bool TouchScreenTask::init() {
    auto& admission_gate = service::globalStopAllAdmissionGate();
    std::uint64_t admission_generation = 0U;
    {
        auto admission = admission_gate.lockAdmission();
        if (!admission.accepting()) {
            last_status_ = Status::NOT_INITIALIZED;
            return false;
        }
        admission_generation = admission.generation();
    }

    if (!config_valid_) {
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    const auto& devices = config_.devices();
    if (!devices.has_arm_id() || devices.arm_id().empty() ||
        !devices.has_camera_id() || devices.camera_id().empty() ||
        !devices.has_dexhand_id() || devices.dexhand_id().empty()) {
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    auto& dm = device::DeviceManager::getInstance();
    auto arm = dm.getDevice<device::RobotArm>(devices.arm_id());
    auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id());
    auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id());
    if (!camera) {
        CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id();
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }

    auto& camera_registry =
        service::globalCameraOperationalActivityRegistry();
    service::CameraOperationalActivityRegistry::ActivityToken camera_token;
    service::CameraOperationalActivityRegistry::DispatchResult camera_start;
    try {
        camera_start = camera_registry.start(
            devices.camera_id(), camera, &camera_token);
    } catch (...) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }
    if (camera_start != service::CameraOperationalActivityRegistry::
                            DispatchResult::Success) {
        CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: "
                        << devices.camera_id();
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }

    const auto admission_current = [&] {
        auto admission = admission_gate.lockAdmission();
        return admission.accepting() &&
            admission.generation() == admission_generation;
    };
    const auto rollback_camera = [&] {
        if (camera_registry.stopIfCurrent(camera_token)) {
            return;
        }
        const auto ticket = admission_gate.beginStopAll();
        (void)admission_gate.finishStopAll(ticket, false);
    };
    const auto mark_interrupted = [this] {
        std::lock_guard lock(mutex_);
        initialized_ = false;
        last_status_ = Status::NOT_INITIALIZED;
    };

    if (!admission_current()) {
        rollback_camera();
        mark_interrupted();
        return false;
    }

    bool initialized = false;
    try {
        initialized = init(arm, dexhand, camera);
    } catch (...) {
        rollback_camera();
        throw;
    }
    if (!initialized) {
        rollback_camera();
        return false;
    }
    if (admission_current()) {
        return true;
    }

    rollback_camera();
    mark_interrupted();
    return false;
}

bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
                          const std::shared_ptr<device::AbstractDexHand>& dexhand,
                          const std::shared_ptr<device::AbstractCamera>& camera) {
    std::lock_guard<std::mutex> lock(mutex_);
    arm_ = arm;
    {
        std::lock_guard arm_lock(activity_arm_mutex_);
        activity_arm_ = arm;
    }
    dexhand_ = dexhand;
    camera_ = camera;

    if (!config_valid_ || !arm_ || !camera_) {
        initialized_ = false;
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    if ((config_.initialization().before_start() || config_.initialization().after_finish()) &&
        config_.initialization().joint_positions().empty()) {
        initialized_ = false;
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    if (config_.alignment().ibvs().camera_link().empty() ||
        config_.alignment().ibvs().control_joint_names().empty()) {
        initialized_ = false;
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
    perception_->setTagSize(config_.perception().apriltag().tag_size_m());

    tracker_.setPerception(perception_);
    tracker_.setTargetPointMethod(toTargetPointMethod(
        config_.perception().apriltag().target_point_method()));

    auto pinocchio_solver =
        std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(arm_->kinematicsSolver());
    if (!pinocchio_solver ||
        !ibvs_.init(pinocchio_solver,
                    config_.alignment().ibvs().camera_link())) {
        initialized_ = false;
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    ibvs_.setPerception(perception_);
    if (!validateControlJointNames()) {
        initialized_ = false;
        last_status_ = Status::CONTROL_JOINT_MISMATCH;
        return false;
    }

    initialized_ = applyConfig();
    if (initialized_) {
        tracker_.clear();
        tracker_.resetActiveTagTracking();
        ibvs_.reset();
        phase_ = Phase::IDLE;
        phase_after_retract_ = Phase::DONE;
        final_status_after_retract_ = Status::DONE;
        target_locked_ = false;
        ibvs_target_initialized_ = false;
        touch_command_started_ = false;
        retract_command_started_ = false;
        align_stable_count_ = 0;
        last_active_tag_id_ = -1;
        last_touch_pressure_sum_ = 0.0;
        last_touch_nonzero_count_ = 0;
        last_align_error_camera_.setZero();
        touch_start_position_valid_ = false;
        touch_start_position_base_.setZero();
        retract_start_position_valid_ = false;
        retract_start_position_base_.setZero();
        last_status_ = Status::IDLE;
    } else {
        last_status_ = Status::INVALID_CONFIG;
    }
    return initialized_;
}

bool TouchScreenTask::touch(const int u, const int v) {
    return touchIfCurrent(u, v, [] { return true; });
}

bool TouchScreenTask::touchIfCurrent(
    const int u,
    const int v,
    const std::function<bool()>& still_admitted,
    SafetyHooks safety_hooks)
{
    auto& admission_gate = service::globalStopAllAdmissionGate();
    std::uint64_t admission_generation = 0U;
    {
        auto admission = admission_gate.lockAdmission();
        if (!admission.accepting()) {
            return false;
        }
        admission_generation = admission.generation();
    }
    if (stop_requested_.load(std::memory_order_acquire)) {
        return false;
    }
    const auto activity_generation =
        activity_generation_.load(std::memory_order_acquire);
    std::lock_guard<std::mutex> lock(mutex_);
    if (!still_admitted || !still_admitted()) {
        return false;
    }
    if (!initialized_ || !camera_ || camera_->id().empty()) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }

    auto& camera_registry =
        service::globalCameraOperationalActivityRegistry();
    service::CameraOperationalActivityRegistry::ActivityToken camera_token;
    service::CameraOperationalActivityRegistry::DispatchResult camera_start;
    try {
        camera_start = camera_registry.start(
            camera_->id(), camera_, &camera_token);
    } catch (...) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }
    if (camera_start != service::CameraOperationalActivityRegistry::
                            DispatchResult::Success) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }

    const auto rollback_camera = [&] {
        if (camera_registry.stopIfCurrent(camera_token)) {
            return;
        }
        const auto ticket = admission_gate.beginStopAll();
        (void)admission_gate.finishStopAll(ticket, false);
    };

    bool admission_current = false;
    bool admitted = false;
    {
        // The arm lease and activity marker are the publication point. Holding
        // admission here makes that point linearizable with beginStopAll().
        auto admission = admission_gate.lockAdmission();
        admission_current = admission.accepting() &&
            admission.generation() == admission_generation;
        if (admission_current) {
            admitted = beginActivityIfCurrent(activity_generation);
        }
    }
    if (!admission_current || !admitted) {
        rollback_camera();
        return false;
    }

    resetActivityUnlocked();
    activity_safety_hooks_ = std::move(safety_hooks);
    if (!activitySafetyCurrent()) {
        last_status_ = Status::SAFETY_ADMISSION_REVOKED;
        activity_active_.store(false, std::memory_order_release);
        clearActivitySafetyHooksUnlocked();
        releaseActivityControlUnlocked();
        rollback_camera();
        return false;
    }

    const bool started = startFromPixelUnlocked(u, v);
    if (!started) {
        activity_active_.store(false, std::memory_order_release);
        clearActivitySafetyHooksUnlocked();
        releaseActivityControlUnlocked();
        rollback_camera();
        return false;
    }

    // startFromPixelUnlocked() may perform an interruptible initialization
    // move. Do not retain the global gate across device work; reject and roll
    // back if StopAll changed the generation while that work was in flight.
    admission_current = false;
    {
        auto admission = admission_gate.lockAdmission();
        admission_current = admission.accepting() &&
            admission.generation() == admission_generation;
    }
    if (admission_current) {
        return true;
    }
    activity_active_.store(false, std::memory_order_release);
    resetActivityUnlocked();
    clearActivitySafetyHooksUnlocked();
    releaseActivityControlUnlocked();
    rollback_camera();
    return false;
}

bool TouchScreenTask::beginActivityIfCurrent(
    const std::uint64_t activity_generation)
{
    if (stop_requested_.load(std::memory_order_acquire) ||
        activity_generation_.load(std::memory_order_acquire) !=
            activity_generation) {
        return false;
    }
    if (isBusyUnlocked()) {
        last_status_ = Status::TASK_BUSY;
        return false;
    }
    if (!acquireActivityControlUnlocked()) {
        last_status_ = Status::TASK_BUSY;
        return false;
    }
    if (stop_requested_.load(std::memory_order_acquire) ||
        activity_generation_.load(std::memory_order_acquire) !=
            activity_generation) {
        releaseActivityControlUnlocked();
        return false;
    }
    activity_active_.store(true, std::memory_order_release);
    return true;
}

bool TouchScreenTask::acquireActivityControlUnlocked()
{
    if (!arm_ || arm_->id().empty() || activityControlToken().valid()) {
        return false;
    }
    static std::atomic<std::uint64_t> sequence{0U};
    const auto acquired = control::ControlAuthorityManager::instance()
        .tryAcquire(
            arm_->id(),
            "touch-screen:" + id_ + ":" +
                std::to_string(
                    sequence.fetch_add(1U, std::memory_order_relaxed) + 1U),
            std::chrono::duration_cast<
                control::ControlAuthorityManager::Duration>(
                    std::chrono::hours(24)));
    if (!acquired.acquired) {
        return false;
    }
    {
        std::lock_guard lock(activity_control_mutex_);
        activity_control_token_ = acquired.token;
    }
    return true;
}

control::ControlLeaseToken TouchScreenTask::activityControlToken() const
{
    std::lock_guard lock(activity_control_mutex_);
    return activity_control_token_;
}

bool TouchScreenTask::activityControlCurrent() const
{
    const auto token = activityControlToken();
    return token.valid() &&
        control::ControlAuthorityManager::instance().validate(
            token);
}

std::function<bool()> TouchScreenTask::activityCancellationRequested() const
{
    const auto token = activityControlToken();
    return [this, token] {
        return stop_requested_.load(std::memory_order_acquire) ||
            !control::ControlAuthorityManager::instance().validate(token);
    };
}

control::ControlDispatchGuard
TouchScreenTask::tryBeginActivityDispatch() const
{
    const auto token = activityControlToken();
    return control::ControlAuthorityManager::instance().tryBeginDispatch(
        token);
}

bool TouchScreenTask::activitySafetyCurrent() const
{
    if (!activity_safety_hooks_.revalidate) {
        return true;
    }
    try {
        return activity_safety_hooks_.revalidate();
    } catch (...) {
        return false;
    }
}

bool TouchScreenTask::runArmActuationIfCurrent(
    const SafetyHooks::HardwareOperation& operation) const
{
    if (!operation || !activitySafetyCurrent()) {
        return false;
    }
    auto authority_dispatch = tryBeginActivityDispatch();
    if (!authority_dispatch.acquired()) {
        return false;
    }
    try {
        return activity_safety_hooks_.dispatch_actuation
            ? activity_safety_hooks_.dispatch_actuation(operation)
            : operation();
    } catch (...) {
        return false;
    }
}

bool TouchScreenTask::runArmStopIfCurrent(
    const SafetyHooks::HardwareOperation& operation) const
{
    if (!operation) {
        return false;
    }
    auto authority_dispatch = tryBeginActivityDispatch();
    if (!authority_dispatch.acquired()) {
        return false;
    }
    try {
        return activity_safety_hooks_.dispatch_stop
            ? activity_safety_hooks_.dispatch_stop(operation)
            : operation();
    } catch (...) {
        return false;
    }
}

void TouchScreenTask::clearActivitySafetyHooksUnlocked() noexcept
{
    activity_safety_hooks_ = {};
}

void TouchScreenTask::releaseActivityControlUnlocked() noexcept
{
    control::ControlLeaseToken token;
    {
        std::lock_guard lock(activity_control_mutex_);
        token = std::move(activity_control_token_);
        activity_control_token_ = {};
    }
    control::ControlAuthorityManager::instance().release(token);
}

void TouchScreenTask::finishActivityUnlocked(
    const Phase phase,
    const Status status) noexcept
{
    phase_ = phase;
    last_status_ = status;
    touch_command_started_ = false;
    retract_command_started_ = false;
    activity_active_.store(false, std::memory_order_release);
    clearActivitySafetyHooksUnlocked();
    releaseActivityControlUnlocked();
}

bool TouchScreenTask::startFromPixel(const int u, const int v) {
    return touchIfCurrent(u, v, [] { return true; });
}

bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
    if (!initialized_) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }
    if (u < 0 || v < 0) {
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    if (!moveToInitPositionBeforeStartIfEnabled()) {
        return false;
    }

    tracker_.clear();
    tracker_.resetActiveTagTracking();
    ibvs_.reset();

    target_u_ = u;
    target_v_ = v;
    target_locked_ = false;
    ibvs_target_initialized_ = false;
    touch_command_started_ = false;
    retract_command_started_ = false;
    align_stable_count_ = 0;
    align_debug_count_ = 0;
    last_touch_pressure_sum_ = 0.0;
    last_touch_nonzero_count_ = 0;
    last_active_tag_id_ = -1;
    last_align_error_camera_.setZero();
    locked_target_rotation_valid_ = false;
    locked_target_rotation_.setIdentity();
    touch_start_position_valid_ = false;
    touch_start_position_base_.setZero();
    retract_start_position_valid_ = false;
    retract_start_position_base_.setZero();
    phase_ = Phase::ALIGNING;
    phase_after_retract_ = Phase::DONE;
    final_status_after_retract_ = Status::DONE;
    phase_start_time_ = Clock::now();
    last_status_ = Status::ALIGN_WAITING_TRACK;
    return true;
}

bool TouchScreenTask::step(const double dt) {
    std::lock_guard<std::mutex> lock(mutex_);
    if (stop_requested_.load(std::memory_order_acquire)) {
        return true;
    }
    if (activity_active_.load(std::memory_order_acquire) &&
        !activityControlCurrent()) {
        activity_active_.store(false, std::memory_order_release);
        resetActivityUnlocked();
        clearActivitySafetyHooksUnlocked();
        releaseActivityControlUnlocked();
        return true;
    }
    if (activity_active_.load(std::memory_order_acquire) &&
        !activitySafetyCurrent()) {
        (void)runArmStopIfCurrent([this] {
            return arm_ && arm_->stopMotion().ok();
        });
        finishActivityUnlocked(
            Phase::FAILED, Status::SAFETY_ADMISSION_REVOKED);
        return false;
    }
    if (!initialized_) {
        last_status_ = Status::NOT_INITIALIZED;
        return false;
    }
    if (!std::isfinite(dt) || dt <= 0.0) {
        enterFailed(Status::INVALID_CONFIG);
        return false;
    }

    if (dexhand_) {
        const bool tactile_ok = updateTouchPressure();
        // std::cout << "[TouchScreenTask][TACTILE] finger="
        //           << fingerTypeToString(toFingerType(config_.touch().tactile().finger()))
        //           << ", region=" << tactileRegionToString(toTactileRegion(config_.touch().tactile().region()))
        //           << ", ok=" << (tactile_ok ? 1 : 0)
        //           << ", nonzero_count=" << last_touch_nonzero_count_
        //           << ", pressure_sum=" << last_touch_pressure_sum_
        //           << std::endl;
    }

    switch (phase_) {
        case Phase::IDLE:
            last_status_ = Status::IDLE;
            return true;
        case Phase::ALIGNING:
            return stepAligning(dt);
        case Phase::ALIGN_REACHED:
            last_status_ = Status::ALIGN_REACHED;
            if (config_.alignment().pause_when_reached()) {
                return true;
            }
            if (!startTouchPhase()) {
                enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
                                    last_status_ == Status::INVALID_CONFIG ||
                                    last_status_ == Status::ROBOT_STATE_FAILED
                                ? last_status_
                                : Status::ROBOT_COMMAND_FAILED);
                return false;
            }
            return true;
        case Phase::TOUCHING:
            return stepTouching();
        case Phase::DWELLING:
            return stepDwelling();
        case Phase::RETRACTING:
            return stepRetracting();
        case Phase::DONE:
            last_status_ = Status::DONE;
            return true;
        case Phase::FAILED:
            return false;
    }
    enterFailed(Status::INVALID_CONFIG);
    return false;
}

void TouchScreenTask::stop() {
    (void)stopActivity();
}

bool TouchScreenTask::stopActivity() {
    activity_generation_.fetch_add(1U, std::memory_order_acq_rel);
    stop_requested_.store(true, std::memory_order_release);

    std::shared_ptr<device::RobotArm> arm;
    {
        std::lock_guard lock(activity_arm_mutex_);
        arm = activity_arm_;
    }

    static std::atomic<std::uint64_t> stop_sequence{0U};
    auto& authority = control::ControlAuthorityManager::instance();
    const auto expected_token = activityControlToken();
    control::ControlAcquireResult stop_barrier;
    bool barrier_error = false;
    if (arm && expected_token.valid()) {
        try {
            stop_barrier = authority.preemptAcquireIfCurrent(
                expected_token,
                "touch-screen-stop:" + id_ + ":" +
                    std::to_string(
                        stop_sequence.fetch_add(
                            1U, std::memory_order_relaxed) + 1U),
                std::chrono::duration_cast<
                    control::ControlAuthorityManager::Duration>(
                        std::chrono::hours(24)));
        } catch (...) {
            barrier_error = true;
            (void)authority.quarantineIfCurrent(expected_token);
        }
    }

    // Only the caller which atomically converted this task's exact lease may
    // touch the driver. If StopAll already owns the safety barrier, its arm
    // stop runs independently while this task only drains its old step.
    if (stop_barrier.acquired) {
        try {
            (void)arm->stopMotion();
        } catch (...) {
        }
    }

    bool was_active = false;
    {
        // A step holds this mutex through all of its arm submissions. Taking
        // it here proves that the old step has exited before state is reset.
        std::lock_guard<std::mutex> lock(mutex_);
        was_active = activity_active_.exchange(
            false, std::memory_order_acq_rel);
        if (was_active || activityControlToken().valid() ||
            isBusyUnlocked()) {
            resetActivityUnlocked();
        }
        clearActivitySafetyHooksUnlocked();
        releaseActivityControlUnlocked();
    }

    bool stopped = !barrier_error;
    if (stop_barrier.acquired) {
        const bool handler_released = authority.waitForPreemptedRelease(
            stop_barrier.token,
            control::ControlAuthorityManager::Duration::zero());
        if (handler_released) {
            // A backend may have allowed the cancellation request to return
            // without fully quiescing. Confirm once more after the old task
            // step and every guarded dispatch have drained.
            try {
                stopped = arm->stopMotion().ok();
            } catch (...) {
                stopped = false;
            }
        } else {
            stopped = false;
        }

        if (stopped) {
            authority.release(stop_barrier.token);
        } else {
            (void)authority.retireSafetyHolder(stop_barrier.token);
        }
    }

    stop_requested_.store(false, std::memory_order_release);
    return stopped;
}

void TouchScreenTask::resetActivityUnlocked() {
    ibvs_.resetTwistCommandState();

    phase_ = Phase::IDLE;
    phase_after_retract_ = Phase::DONE;
    final_status_after_retract_ = Status::DONE;
    target_locked_ = false;
    ibvs_target_initialized_ = false;
    touch_command_started_ = false;
    retract_command_started_ = false;
    align_stable_count_ = 0;
    last_active_tag_id_ = -1;
    last_touch_pressure_sum_ = 0.0;
    last_touch_nonzero_count_ = 0;
    last_align_error_camera_.setZero();
    locked_target_rotation_valid_ = false;
    locked_target_rotation_.setIdentity();
    touch_start_position_valid_ = false;
    touch_start_position_base_.setZero();
    retract_start_position_valid_ = false;
    retract_start_position_base_.setZero();
    last_status_ = Status::STOPPED;
}

TouchScreenTask::Phase TouchScreenTask::phase() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return phase_;
}

TouchScreenTask::Status TouchScreenTask::lastStatus() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return last_status_;
}

bool TouchScreenTask::isBusyUnlocked() const {
    return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
           phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
           phase_ == Phase::RETRACTING;
}

bool TouchScreenTask::isBusy() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return isBusyUnlocked();
}

TaskState TouchScreenTask::state() const {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!initialized_) {
        return TaskState::UNINITIALIZED;
    }
    if (isBusyUnlocked()) {
        return TaskState::RUNNING;
    }
    if (phase_ == Phase::DONE) {
        return TaskState::SUCCEEDED;
    }
    if (phase_ == Phase::FAILED) {
        return TaskState::FAILED;
    }
    if (last_status_ == Status::STOPPED) {
        return TaskState::STOPPED;
    }
    return TaskState::IDLE;
}

bool TouchScreenTask::isFinished() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return phase_ == Phase::DONE;
}

bool TouchScreenTask::isFailed() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return phase_ == Phase::FAILED;
}

int TouchScreenTask::targetU() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return target_u_;
}

int TouchScreenTask::targetV() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return target_v_;
}

double TouchScreenTask::lastTouchPressureSum() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return last_touch_pressure_sum_;
}

int TouchScreenTask::lastTouchNonzeroCount() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return last_touch_nonzero_count_;
}

int TouchScreenTask::lastActiveTagId() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return last_active_tag_id_;
}

Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return last_align_error_camera_;
}

std::string TouchScreenTask::controlDeviceId() const {
    std::lock_guard<std::mutex> lock(mutex_);
    if (arm_ && !arm_->id().empty()) {
        return arm_->id();
    }
    return config_.devices().arm_id();
}

std::string TouchScreenTask::stateString() const {
    return taskStateToString(state());
}

std::string TouchScreenTask::detailStatusString() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return std::string(phaseToString(phase_)) + "/" + statusToString(last_status_);
}

const char* TouchScreenTask::phaseToString(const Phase phase) {
    switch (phase) {
        case Phase::IDLE: return "IDLE";
        case Phase::ALIGNING: return "ALIGNING";
        case Phase::ALIGN_REACHED: return "ALIGN_REACHED";
        case Phase::TOUCHING: return "TOUCHING";
        case Phase::DWELLING: return "DWELLING";
        case Phase::RETRACTING: return "RETRACTING";
        case Phase::DONE: return "DONE";
        case Phase::FAILED: return "FAILED";
    }
    return "UNKNOWN";
}

const char* TouchScreenTask::statusToString(const Status status) {
    switch (status) {
        case Status::IDLE: return "IDLE";
        case Status::NOT_INITIALIZED: return "NOT_INITIALIZED";
        case Status::INVALID_CONFIG: return "INVALID_CONFIG";
        case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH";
        case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION";
        case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK";
        case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED";
        case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED";
        case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT";
        case Status::ALIGNING: return "ALIGNING";
        case Status::ALIGN_REACHED: return "ALIGN_REACHED";
        case Status::TOUCHING: return "TOUCHING";
        case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE";
        case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED";
        case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT";
        case Status::RETRACTING: return "RETRACTING";
        case Status::DONE: return "DONE";
        case Status::STOPPED: return "STOPPED";
        case Status::SAFETY_ADMISSION_REVOKED: return "SAFETY_ADMISSION_REVOKED";
        case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED";
        case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED";
        case Status::TASK_BUSY: return "TASK_BUSY";
    }
    return "UNKNOWN";
}

bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& config) {
    if (!config.has_id() || config.id().empty() ||
        !config.has_devices() ||
        !config.devices().has_arm_id() || config.devices().arm_id().empty() ||
        !config.devices().has_dexhand_id() || config.devices().dexhand_id().empty() ||
        !config.devices().has_camera_id() || config.devices().camera_id().empty() ||
        !config.has_initialization() ||
        !config.initialization().has_before_start() ||
        !config.initialization().has_after_finish() ||
        !config.initialization().has_velocity() ||
        !config.initialization().has_acceleration() ||
        !config.has_perception() ||
        !config.perception().has_apriltag() ||
        !config.has_alignment() ||
        !config.alignment().has_ibvs() ||
        !config.alignment().has_target() ||
        !config.alignment().has_error_threshold() ||
        !config.alignment().has_stable_frames() ||
        !config.alignment().has_timeout_s() ||
        !config.alignment().has_pause_when_reached() ||
        !config.has_touch() ||
        !config.touch().has_tactile() ||
        !config.touch().has_dwell_time_s() ||
        !config.has_retract() ||
        !config.retract().has_twist_tool() ||
        !config.retract().has_acceleration() ||
        !config.retract().has_duration_s()) {
        return false;
    }

    const auto& apriltag = config.perception().apriltag();
    const auto& alignment = config.alignment();
    const auto& ibvs = alignment.ibvs();
    const auto& target = alignment.target();
    const auto& touch = config.touch();
    const auto& tactile = touch.tactile();
    const auto& retract = config.retract();

    if (!apriltag.has_tag_size_m() ||
        !apriltag.has_depth_policy() ||
        !apriltag.has_target_point_method() ||
        !target.has_position_in_camera() ||
        !hasVec3(target.position_in_camera()) ||
        !target.has_rotation_vector() ||
        !hasVec3(target.rotation_vector()) ||
        !target.has_mode() ||
        !hasVec6(alignment.error_threshold()) ||
        !ibvs.has_camera_link() ||
        !ibvs.has_lambda() ||
        !ibvs.has_mu() ||
        !ibvs.has_qdot_max() ||
        !ibvs.has_vmax6() ||
        !hasVec6(ibvs.vmax6()) ||
        !ibvs.has_amax6() ||
        !hasVec6(ibvs.amax6()) ||
        !ibvs.has_twist_filter_alpha() ||
        !ibvs.has_r_camera_to_visp() ||
        !hasMat3(ibvs.r_camera_to_visp()) ||
        !ibvs.has_r_camera_to_urdf() ||
        !hasMat3(ibvs.r_camera_to_urdf()) ||
        !tactile.has_finger() ||
        !tactile.has_region() ||
        !tactile.has_criterion() ||
        !tactile.has_force_threshold() ||
        !hasVec6(retract.twist_tool())) {
        return false;
    }

    if (ibvs.camera_link().empty() ||
        ibvs.control_joint_names().empty()) {
        return false;
    }

    const Eigen::Vector3d target_position =
        cmvr::common::math::toEigenVec3(alignment.target().position_in_camera());
    const Eigen::Vector3d target_rotation =
        cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
    const Eigen::Matrix<double, 6, 1> error_threshold =
        cmvr::common::math::toEigenVec6(alignment.error_threshold());
    const Eigen::Matrix3d camera_to_visp =
        cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp());
    const Eigen::Matrix3d camera_to_urdf =
        cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf());

    if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 ||
        !target_position.allFinite() ||
        !target_rotation.allFinite() ||
        alignment.stable_frames() <= 0 ||
        !std::isfinite(alignment.timeout_s()) || alignment.timeout_s() <= 0.0 ||
        !std::isfinite(tactile.force_threshold()) || tactile.force_threshold() < 0.0 ||
        !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
        !camera_to_visp.allFinite() || !camera_to_urdf.allFinite() ||
        !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
        !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
        !std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) {
        return false;
    }
    for (int i = 0; i < error_threshold.size(); ++i) {
        if (!std::isfinite(error_threshold[i]) || error_threshold[i] < 0.0) {
            return false;
        }
    }

    if (!std::isfinite(config.initialization().velocity()) ||
        config.initialization().velocity() <= 0.0 ||
        !std::isfinite(config.initialization().acceleration()) ||
        config.initialization().acceleration() <= 0.0) {
        return false;
    }

    if (config.initialization().before_start() || config.initialization().after_finish()) {
        std::vector<double> init_positions;
        if (!buildInitJointPositionsFromConfig(config, init_positions)) {
            return false;
        }
    }

    std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
    if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
                                       toTactileRegion(tactile.region()),
                                       tactile_regions)) {
        return false;
    }

    switch (touch.motion_case()) {
        case cmvr::config::TouchScreenTaskTouchConfig::kSpeedL: {
            const auto& speed_l = touch.speed_l();
            if (!speed_l.has_twist_tool() ||
                !hasVec6(speed_l.twist_tool()) ||
                !speed_l.has_acceleration() ||
                !speed_l.has_max_distance_m()) {
                return false;
            }
            const Eigen::Matrix<double, 6, 1> twist =
                cmvr::common::math::toEigenVec6(speed_l.twist_tool());
            return twist.allFinite() &&
                   std::isfinite(speed_l.acceleration()) &&
                   speed_l.acceleration() > 0.0 &&
                   std::isfinite(speed_l.max_distance_m()) &&
                   speed_l.max_distance_m() >= 0.0;
        }
        case cmvr::config::TouchScreenTaskTouchConfig::kMoveL: {
            const auto& move_l = touch.move_l();
            if (!move_l.has_direction_tool() ||
                !hasVec6(move_l.direction_tool()) ||
                !move_l.has_distance_m() ||
                !move_l.has_velocity() ||
                !move_l.has_acceleration() ||
                !move_l.has_jerk()) {
                return false;
            }
            const Eigen::Matrix<double, 6, 1> direction =
                cmvr::common::math::toEigenVec6(move_l.direction_tool());
            Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
            if (!computeLinearMoveDeltaTool(direction, move_l.distance_m(), touch_forward_delta) ||
                !std::isfinite(move_l.velocity()) || move_l.velocity() <= 0.0 ||
                !std::isfinite(move_l.acceleration()) || move_l.acceleration() <= 0.0 ||
                !std::isfinite(move_l.jerk()) || move_l.jerk() <= 0.0 ||
                move_l.joint_velocity_limits_size() != ibvs.control_joint_names_size()) {
                return false;
            }
            for (const double qd_max_i : move_l.joint_velocity_limits()) {
                if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) {
                    return false;
                }
            }
            return true;
        }
        case cmvr::config::TouchScreenTaskTouchConfig::MOTION_NOT_SET:
        default:
            return false;
    }
}

bool TouchScreenTask::applyConfig() {
    if (!perception_) {
        return false;
    }
    if (!validateConfig(config_)) {
        return false;
    }

    if (dexhand_) {
        const auto& tactile = config_.touch().tactile();
        std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
        if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
                                           toTactileRegion(tactile.region()),
                                           tactile_regions)) {
            return false;
        }
        try {
            dexhand_->setTactilePollingRegions(tactile_regions);
        } catch (...) {
            return false;
        }
    }

    const auto& apriltag = config_.perception().apriltag();
    const auto& ibvs = config_.alignment().ibvs();
    const auto target_position_in_camera =
        cmvr::common::math::toEigenVec3(config_.alignment().target().position_in_camera());
    const Eigen::Matrix3d r_camera_to_visp =
        cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp());
    const Eigen::Matrix3d r_camera_to_urdf =
        cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf());
    perception_->setTagSize(apriltag.tag_size_m());
    tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method()));
    ibvs_.setLambda(ibvs.lambda());
    ibvs_.setMu(ibvs.mu());
    ibvs_.setQdotMax(ibvs.qdot_max());
    ibvs_.setVelocityLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.vmax6())));
    ibvs_.setAccelerationLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.amax6())));
    ibvs_.setTwistFilterAlpha(ibvs.twist_filter_alpha());
    ibvs_.setAlignCameraToVisp(r_camera_to_visp);
    ibvs_.setAlignCameraToUrdf(r_camera_to_urdf);
    CMVR_LOG(DEBUG) << "[TouchScreenTask] Apply config id=" << config_.id()
                    << ", arm_id=" << config_.devices().arm_id()
                    << ", camera_id=" << config_.devices().camera_id()
                    << ", tag_size_m=" << apriltag.tag_size_m()
                    << ", target_position_in_camera=[" << target_position_in_camera.x()
                    << ", " << target_position_in_camera.y()
                    << ", " << target_position_in_camera.z() << "]"
                    << ", r_camera_to_visp=[" << r_camera_to_visp(0, 0)
                    << ", " << r_camera_to_visp(0, 1)
                    << ", " << r_camera_to_visp(0, 2)
                    << "; " << r_camera_to_visp(1, 0)
                    << ", " << r_camera_to_visp(1, 1)
                    << ", " << r_camera_to_visp(1, 2)
                    << "; " << r_camera_to_visp(2, 0)
                    << ", " << r_camera_to_visp(2, 1)
                    << ", " << r_camera_to_visp(2, 2) << "]"
                    << ", r_camera_to_urdf=[" << r_camera_to_urdf(0, 0)
                    << ", " << r_camera_to_urdf(0, 1)
                    << ", " << r_camera_to_urdf(0, 2)
                    << "; " << r_camera_to_urdf(1, 0)
                    << ", " << r_camera_to_urdf(1, 1)
                    << ", " << r_camera_to_urdf(1, 2)
                    << "; " << r_camera_to_urdf(2, 0)
                    << ", " << r_camera_to_urdf(2, 1)
                    << ", " << r_camera_to_urdf(2, 2) << "]";
    return true;
}

bool TouchScreenTask::validateControlJointNames() const {
    std::vector<std::string> solver_joint_names;
    if (!ibvs_.getChainJointNames(solver_joint_names)) {
        return false;
    }
    const std::vector<std::string> task_joint_names = controlJointNames(config_);
    if (solver_joint_names == task_joint_names) {
        return true;
    }

    std::ostringstream mismatch;
    mismatch << "[TouchScreenTask] control_joint_names mismatch with IbvsController IK chain"
             << ", task joints=[";
    for (const auto& name : task_joint_names) {
        mismatch << name << ' ';
    }
    mismatch << "], solver joints=[";
    for (const auto& name : solver_joint_names) {
        mismatch << name << ' ';
    }
    mismatch << ']';
    CMVR_LOG(ERROR) << mismatch.str();
    return false;
}

bool TouchScreenTask::stepAligning(const double dt) {
    const auto& alignment = config_.alignment();
    const auto target_position_in_camera =
        cmvr::common::math::toEigenVec3(alignment.target().position_in_camera());
    const auto target_rotation_vector =
        cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
    const auto error_threshold =
        cmvr::common::math::toEigenVec6(alignment.error_threshold());

    const auto now = Clock::now();
    const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
    if (elapsed > alignment.timeout_s()) {
        enterFailed(Status::ALIGN_TIMEOUT);
        return false;
    }
    const double ibvs_dt = std::clamp(dt, 0.005, 0.05);

    perception::AprilTagPerception::Options perception_options;
    perception_options.depth_policy =
        toDepthPolicy(config_.perception().apriltag().depth_policy());
    perception_options.detect_tags = true;
    perception_options.fetch_encoded = false;
    if (!perception_->update(perception_options)) {
        hardStopIbvsMotion();
        last_status_ = Status::ALIGN_WAITING_PERCEPTION;
        return true;
    }

    bool tracking_ok = false;
    if (!target_locked_) {
        tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_);
        if (tracking_ok) {
            target_locked_ = true;
            ibvs_target_initialized_ = false;
            align_stable_count_ = 0;
        }
    } else {
        tracking_ok = tracker_.track();
    }

    if (!tracking_ok) {
        hardStopIbvsMotion();
        align_stable_count_ = 0;
        last_status_ = Status::ALIGN_WAITING_TRACK;
        return true;
    }

    const int tag_id = tracker_.activeTagId();
    last_active_tag_id_ = tag_id;
    if (tag_id < 0) {
        hardStopIbvsMotion();
        align_stable_count_ = 0;
        last_status_ = Status::ALIGN_WAITING_TRACK;
        return true;
    }

    Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
    if (!tracker_.getAnchorInTag(tag_id, p_t_target)) {
        hardStopIbvsMotion();
        align_stable_count_ = 0;
        last_status_ = Status::ALIGN_WAITING_TRACK;
        return true;
    }

    const auto* current_tag = perception_->findTag(tag_id);
    if (!current_tag) {
        hardStopIbvsMotion();
        align_stable_count_ = 0;
        last_status_ = Status::ALIGN_WAITING_TRACK;
        return true;
    }

    Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(),
                                                        target_rotation_vector.y(),
                                                        target_rotation_vector.z());
    const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
    switch (alignment.target().mode()) {
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
            break;
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
            if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
                double locked_yaw_rad = 0.0;
                if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) {
                    enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
                    return false;
                }
                const Eigen::Matrix3d Rz_locked =
                    Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix();
                locked_target_rotation_ = R_target * Rz_locked;
                locked_target_rotation_valid_ = true;
            }
            R_target = locked_target_rotation_;
            break;
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY:
            // True position-only mode: keep the desired orientation equal to the
            // current tag orientation every frame so IBVS does not actively try to
            // correct rotational error.
            R_target = R_current;
            break;
    }
    const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target);

    ibvs_.setTrackedTagId(tag_id);
    const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() ||
                                alignment.target().mode() ==
                                    cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY;
    if (refresh_target) {
        if (!ibvs_.setTargetFromPointInTag(p_t_target,
                                           target_position_in_camera,
                                           target_rotvec.x(),
                                           target_rotvec.y(),
                                           target_rotvec.z())) {
            enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
            return false;
        }
        ibvs_target_initialized_ = true;
    }

    std::vector<double> q_now;
    if (!readControlledJointPositions(q_now)) {
        enterFailed(Status::ROBOT_STATE_FAILED);
        return false;
    }

    std::vector<double> qdot_cmd;
    if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) {
        switch (ibvs_.lastComputeStatus()) {
            case IbvsController::ComputeStatus::NO_NEW_FRAME:
            case IbvsController::ComputeStatus::NO_TAG:
            case IbvsController::ComputeStatus::TAG_MISMATCH:
            case IbvsController::ComputeStatus::NO_DEPTH:
                hardStopIbvsMotion();
                align_stable_count_ = 0;
                last_status_ = Status::ALIGN_WAITING_TRACK;
                return true;
            case IbvsController::ComputeStatus::OK:
            case IbvsController::ComputeStatus::NOT_READY:
            case IbvsController::ComputeStatus::BAD_IMAGE:
            case IbvsController::ComputeStatus::INVALID_INPUT:
            case IbvsController::ComputeStatus::IK_FAILED:
            default:
                enterFailed(Status::ALIGN_COMPUTE_FAILED);
                return false;
        }
    }

    if ((align_debug_count_++ % 20) == 0) {
        Eigen::Matrix<double, 6, 1> achieved_twist_base =
            Eigen::Matrix<double, 6, 1>::Zero();
        bool achieved_ok = false;
        auto pinocchio_solver =
            arm_ ? std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(arm_->kinematicsSolver()) : nullptr;
        if (pinocchio_solver) {
            achieved_ok = pinocchio_solver->computeTwistBaseAtQ(
                q_now,
                qdot_cmd,
                config_.alignment().ibvs().camera_link(),
                achieved_twist_base);
        }

        const auto& target_c = tracker_.lastTargetInCamera();
        const Eigen::Vector3d err_c = target_c - target_position_in_camera;
        const auto& v_visp = ibvs_.lastCameraTwistVisp();
        const double qdot_norm =
            qdot_cmd.empty()
                ? 0.0
                : Eigen::Map<const Eigen::VectorXd>(
                      qdot_cmd.data(),
                      static_cast<Eigen::Index>(qdot_cmd.size())).norm();
        CMVR_LOG(DEBUG) << "[TouchScreenTask][ALIGN_DEBUG]"
                        << " target_c=[" << target_c.x() << ", " << target_c.y()
                        << ", " << target_c.z() << "]"
                        << ", err_c=[" << err_c.x() << ", " << err_c.y()
                        << ", " << err_c.z() << "]"
                        << ", v_visp=[" << v_visp[0] << ", " << v_visp[1]
                        << ", " << v_visp[2] << ", " << v_visp[3]
                        << ", " << v_visp[4] << ", " << v_visp[5] << "]"
                        << ", qdot0=" << (qdot_cmd.empty() ? 0.0 : qdot_cmd.front())
                        << ", qdot_norm=" << qdot_norm
                        << ", achieved_ok=" << achieved_ok
                        << ", achieved_twist_base=[" << achieved_twist_base[0]
                        << ", " << achieved_twist_base[1]
                        << ", " << achieved_twist_base[2]
                        << ", " << achieved_twist_base[3]
                        << ", " << achieved_twist_base[4]
                        << ", " << achieved_twist_base[5] << "]";
    }

    if (!sendJointVelocity(qdot_cmd)) {
        enterFailed(Status::ROBOT_COMMAND_FAILED);
        return false;
    }

    last_align_error_camera_ = tracker_.lastTargetInCamera() - target_position_in_camera;
    const Eigen::Vector3d rot_error_vec =
        rotvecFromRotationMatrix(R_target.transpose() * R_current);
    bool align_ok =
        std::abs(last_align_error_camera_.x()) <= error_threshold[0] &&
        std::abs(last_align_error_camera_.y()) <= error_threshold[1] &&
        std::abs(last_align_error_camera_.z()) <= error_threshold[2];
    switch (alignment.target().mode()) {
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
            align_ok = align_ok &&
                       std::abs(rot_error_vec.x()) <= error_threshold[3] &&
                       std::abs(rot_error_vec.y()) <= error_threshold[4] &&
                       std::abs(rot_error_vec.z()) <= error_threshold[5];
            break;
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
            align_ok = align_ok &&
                       std::abs(rot_error_vec.x()) <= error_threshold[3] &&
                       std::abs(rot_error_vec.y()) <= error_threshold[4];
            break;
        case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY:
            break;
    }
    if (align_ok) {
        ++align_stable_count_;
    } else {
        align_stable_count_ = 0;
    }

    if (align_stable_count_ >= alignment.stable_frames()) {
        CMVR_LOG(INFO) << "[TouchScreenTask][ALIGN_REACHED] tag_id=" << tag_id
                  << ", err_xyz=[" << last_align_error_camera_.x() << ", "
                  << last_align_error_camera_.y() << ", "
                  << last_align_error_camera_.z() << "]"
                  << ", err_rxyz=[" << rot_error_vec.x() << ", "
                  << rot_error_vec.y() << ", "
                  << rot_error_vec.z() << "]";
        hardStopIbvsMotion();
        phase_ = Phase::ALIGN_REACHED;
        phase_start_time_ = Clock::now();
        touch_command_started_ = false;
        last_status_ = Status::ALIGN_REACHED;
        return true;
    }

    last_status_ = Status::ALIGNING;
    return true;
}

bool TouchScreenTask::stepTouching() {
    if (!touch_command_started_) {
        if (!startTouchPhase()) {
            enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
                                    last_status_ == Status::INVALID_CONFIG ||
                                    last_status_ == Status::ROBOT_STATE_FAILED
                                ? last_status_
                                : Status::ROBOT_COMMAND_FAILED);
            return false;
        }
    }

    if (!dexhand_) {
        enterFailed(Status::TACTILE_UNAVAILABLE);
        return false;
    }

    if (isTouchTriggered(config_, last_touch_pressure_sum_)) {
        if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) {
            logTouchingSpeedLState();
        }
        if (!handleTouchTriggered(true)) {
            enterFailed(Status::ROBOT_COMMAND_FAILED);
            return false;
        }
        return true;
    }

    if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kMoveL) {
        last_status_ = Status::TOUCHING;
        return true;
    }

    const double speed_l_max_distance_m = config_.touch().speed_l().max_distance_m();
    if (speed_l_max_distance_m > 0.0) {
        if (!touch_start_position_valid_) {
            enterFailed(Status::ROBOT_STATE_FAILED);
            return false;
        }

        Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
        if (!readCurrentTouchPointPositionBase(current_position_base)) {
            enterFailed(Status::ROBOT_STATE_FAILED);
            return false;
        }

        const double traveled_distance =
            (current_position_base - touch_start_position_base_).norm();
        if (traveled_distance >= speed_l_max_distance_m) {
            logTouchingSpeedLState();
            if (!updateTouchPressure()) {
                enterFailed(Status::TACTILE_UNAVAILABLE);
                return false;
            }
            if (isTouchTriggered(config_, last_touch_pressure_sum_)) {
                if (!handleTouchTriggered(true)) {
                    enterFailed(Status::ROBOT_COMMAND_FAILED);
                    return false;
                }
                return true;
            }
            if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) {
                enterFailed(Status::ROBOT_COMMAND_FAILED);
                return false;
            }
            return true;
        }
    }

    last_status_ = Status::TOUCHING;
    return true;
}

bool TouchScreenTask::stepDwelling() {
    const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
    if (elapsed < config_.touch().dwell_time_s()) {
        last_status_ = Status::TOUCH_TRIGGERED;
        return true;
    }

    if (!startRetractPhase(Phase::DONE, Status::DONE)) {
        enterFailed(Status::ROBOT_COMMAND_FAILED);
        return false;
    }
    return true;
}

bool TouchScreenTask::stepRetracting() {
    if (!retract_command_started_) {
        if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) {
            enterFailed(Status::ROBOT_COMMAND_FAILED);
            return false;
        }
    }

    const auto now = Clock::now();
    const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
    const double retract_duration_s = config_.retract().duration_s();
    if (elapsed < retract_duration_s) {
        if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
            last_retract_log_time_ = now;
            const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
            Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
            const bool have_current = readCurrentTouchPointPositionBase(current_position_base);
            const bool have_delta = retract_start_position_valid_ && have_current;
            Eigen::Vector3d delta_base = Eigen::Vector3d::Zero();
            if (have_delta) {
                delta_base = current_position_base - retract_start_position_base_;
            }
            if (have_delta) {
                CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
                               << "/" << retract_duration_s
                               << ", cmd_base=[" << cmd_base.vx << ", "
                               << cmd_base.vy << ", " << cmd_base.vz << ", "
                               << cmd_base.wx << ", " << cmd_base.wy << ", "
                               << cmd_base.wz << "]"
                               << ", tcp_delta_base=[" << delta_base.x() << ", "
                               << delta_base.y() << ", " << delta_base.z()
                               << "], tcp_dist=" << delta_base.norm();
            } else {
                CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
                               << "/" << retract_duration_s
                               << ", cmd_base=[" << cmd_base.vx << ", "
                               << cmd_base.vy << ", " << cmd_base.vz << ", "
                               << cmd_base.wx << ", " << cmd_base.wy << ", "
                               << cmd_base.wz << "]"
                               << ", tcp_delta_base=unavailable";
            }
        }
        last_status_ = Status::RETRACTING;
        return true;
    }

    Eigen::Vector3d final_position_base = Eigen::Vector3d::Zero();
    const bool have_final_position = readCurrentTouchPointPositionBase(final_position_base);
    Eigen::Vector3d final_delta_base = Eigen::Vector3d::Zero();
    const bool have_final_delta = retract_start_position_valid_ && have_final_position;
    if (have_final_delta) {
        final_delta_base = final_position_base - retract_start_position_base_;
    }
    if (have_final_delta) {
        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
                       << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                       << ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
                       << final_delta_base.y() << ", " << final_delta_base.z()
                       << "], final_tcp_dist=" << final_delta_base.norm();
    } else {
        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
                       << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                       << ", final_tcp_delta_base=unavailable";
    }

    if (!runArmStopIfCurrent([this] {
            return arm_ && arm_->stopL().ok();
        })) {
        enterFailed(Status::ROBOT_COMMAND_FAILED);
        return false;
    }
    holdCurrentControlledPosition();
    if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) &&
        !moveToInitPositionIfEnabled()) {
        finishActivityUnlocked(
            Phase::FAILED, Status::ROBOT_COMMAND_FAILED);
        return false;
    }
    const auto completed_phase = phase_after_retract_;
    const auto completed_status = final_status_after_retract_;
    finishActivityUnlocked(completed_phase, completed_status);
    return phase_ != Phase::FAILED;
}

bool TouchScreenTask::readControlledJointPositions(std::vector<double>& q_out) const {
    if (!arm_) {
        return false;
    }

    const auto state = arm_->getJointState();
    const auto model = arm_->getRobotModel();
    std::unordered_map<std::string, double> q_map;
    q_map.reserve(model.joint_names.size());
    for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) {
        q_map[model.joint_names[i]] = state.position[i];
    }

    const auto& control_joint_names = config_.alignment().ibvs().control_joint_names();
    q_out.resize(static_cast<size_t>(control_joint_names.size()));
    for (int i = 0; i < control_joint_names.size(); ++i) {
        const auto it = q_map.find(control_joint_names[i]);
        if (it == q_map.end()) {
            return false;
        }
        q_out[static_cast<size_t>(i)] = it->second;
    }
    return true;
}

bool TouchScreenTask::sendJointVelocity(const std::vector<double>& qdot) const {
    if (!arm_ || qdot.size() != static_cast<size_t>(
                         config_.alignment().ibvs().control_joint_names_size())) {
        return false;
    }

    device::JointVelocityCommand cmd;
    cmd.velocity = qdot;
    return runArmActuationIfCurrent([this, &cmd] {
        return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok();
    });
}

bool TouchScreenTask::sendZeroJointVelocity() const {
    std::vector<double> zero(
        static_cast<size_t>(config_.alignment().ibvs().control_joint_names_size()), 0.0);
    return sendJointVelocity(zero);
}

void TouchScreenTask::hardStopIbvsMotion() {
    sendZeroJointVelocity();
    ibvs_.resetTwistCommandState();
}

bool TouchScreenTask::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const {
    if (!arm_) {
        return false;
    }

    try {
        const auto pose = arm_->fk(true);
        p_out << pose.x, pose.y, pose.z;
        return p_out.allFinite();
    } catch (...) {
        return false;
    }
}

void TouchScreenTask::logTouchingSpeedLState() const {
    if (!arm_ || config_.touch().motion_case() != cmvr::config::TouchScreenTaskTouchConfig::kSpeedL ||
        phase_ != Phase::TOUCHING) {
        return;
    }

    const Eigen::Matrix<double, 6, 1> cmd_twist_base = toEigen6(arm_->getSpeedLCommandTwistBase());
    Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
    const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base);
    const bool have_delta = touch_start_position_valid_ && have_current_position;
    Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero();
    if (have_delta) {
        cumulative_delta_base = current_position_base - touch_start_position_base_;
    }

    std::ostringstream state_log;
    state_log << "[TouchScreenTask][TOUCHING] speedl_cmd_base=["
              << cmd_twist_base[0] << ", "
              << cmd_twist_base[1] << ", "
              << cmd_twist_base[2] << ", "
              << cmd_twist_base[3] << ", "
              << cmd_twist_base[4] << ", "
              << cmd_twist_base[5] << "]";
    if (have_delta) {
        state_log << ", cum_tcp_delta_base=["
                  << cumulative_delta_base.x() << ", "
                  << cumulative_delta_base.y() << ", "
                  << cumulative_delta_base.z() << "]"
                  << ", cum_tcp_dist=" << cumulative_delta_base.norm();
    } else {
        state_log << ", cum_tcp_delta_base=[unavailable]";
    }
    CMVR_LOG(DEBUG) << state_log.str();
}

bool TouchScreenTask::holdCurrentControlledPosition() const {
    if (!arm_) {
        return false;
    }

    const auto state = arm_->getJointState();
    const auto model = arm_->getRobotModel();
    std::unordered_map<std::string, double> q_map;
    q_map.reserve(model.joint_names.size());
    for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) {
        q_map[model.joint_names[i]] = state.position[i];
    }

    device::JointPositionCommand joints;
    joints.position.reserve(static_cast<size_t>(
        config_.alignment().ibvs().control_joint_names_size()));
    for (const auto& name : config_.alignment().ibvs().control_joint_names()) {
        const auto it = q_map.find(name);
        if (it == q_map.end()) {
            return false;
        }
        joints.position.push_back(it->second);
    }

    return runArmActuationIfCurrent([this, &joints] {
        return arm_ && arm_->servoJ(joints).ok();
    });
}

bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out) const {
    return buildInitJointPositionsFromConfig(config_, positions_out);
}

bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
    if (!config_.initialization().before_start()) {
        return true;
    }
    if (!arm_) {
        last_status_ = Status::ROBOT_COMMAND_FAILED;
        return false;
    }

    std::vector<double> init_positions;
    if (!buildInitJointPositions(init_positions)) {
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    device::JointPositionCommand init_cmd{init_positions};
    device::MotionOptions options;
    options.velocity = config_.initialization().velocity();
    options.acceleration = config_.initialization().acceleration();
    options.cancellation_requested = activityCancellationRequested();
    if (!runArmActuationIfCurrent([this, &init_cmd, &options] {
            return arm_ && arm_->moveJ(init_cmd, options).ok();
        })) {
        last_status_ = Status::ROBOT_COMMAND_FAILED;
        return false;
    }
    return true;
}

bool TouchScreenTask::moveToInitPositionIfEnabled() const {
    if (!config_.initialization().after_finish()) {
        return true;
    }
    std::vector<double> init_positions;
    if (!arm_ || !buildInitJointPositions(init_positions)) {
        return false;
    }

    device::JointPositionCommand init_cmd{init_positions};
    device::MotionOptions options;
    options.velocity = config_.initialization().velocity();
    options.acceleration = config_.initialization().acceleration();
    options.cancellation_requested = activityCancellationRequested();
    return runArmActuationIfCurrent([this, &init_cmd, &options] {
        return arm_ && arm_->moveJ(init_cmd, options).ok();
    });
}

bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
    if (stop_forward_motion) {
        if (!runArmStopIfCurrent([this] {
                return arm_ && arm_->stopL().ok();
            })) {
            return false;
        }
    }

    phase_ = Phase::DWELLING;
    phase_start_time_ = Clock::now();
    last_status_ = Status::TOUCH_TRIGGERED;
    return true;
}

bool TouchScreenTask::startTouchPhase() {
    if (!arm_) {
        last_status_ = Status::ROBOT_COMMAND_FAILED;
        return false;
    }

    phase_ = Phase::TOUCHING;
    phase_start_time_ = Clock::now();
    touch_command_started_ = true;
    retract_command_started_ = false;
    retract_start_position_valid_ = false;
    retract_start_position_base_.setZero();
    last_status_ = Status::TOUCHING;

    if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) {
        touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_);
        const auto& speed_l = config_.touch().speed_l();
        if (speed_l.max_distance_m() > 0.0 && !touch_start_position_valid_) {
            CMVR_LOG(ERROR) << "[TouchScreenTask] startTouchPhase failed: cannot read touch start pose "
                            << "for speedL distance-based touching";
            last_status_ = Status::ROBOT_STATE_FAILED;
            return false;
        }
        const auto velocity = toCartesianVelocity(
            cmvr::common::math::toEigenVec6(speed_l.twist_tool()));
        if (!runArmActuationIfCurrent([this, velocity, &speed_l] {
                return arm_ && arm_->speedL(
                    velocity,
                    speed_l.acceleration(),
                    0.0,
                    device::FrameType::Tool).ok();
            })) {
            last_status_ = Status::ROBOT_COMMAND_FAILED;
            return false;
        }
        return true;
    }

    Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
    const auto& move_l = config_.touch().move_l();
    if (!computeLinearMoveDeltaTool(cmvr::common::math::toEigenVec6(move_l.direction_tool()),
                                    move_l.distance_m(),
                                    touch_forward_delta)) {
        last_status_ = Status::INVALID_CONFIG;
        return false;
    }

    device::CartesianPose pose_cmd;
    pose_cmd.x = touch_forward_delta.x();
    pose_cmd.y = touch_forward_delta.y();
    pose_cmd.z = touch_forward_delta.z();
    device::MotionOptions options;
    options.velocity = move_l.velocity();
    options.acceleration = move_l.acceleration();
    options.jerk = move_l.jerk();
    options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(),
                                         move_l.joint_velocity_limits().end());
    options.cancellation_requested = activityCancellationRequested();
    if (!runArmActuationIfCurrent([this, &pose_cmd, &options] {
            return arm_ && arm_->moveL(
                pose_cmd, options, device::FrameType::Tool).ok();
        })) {
        last_status_ = Status::ROBOT_COMMAND_FAILED;
        return false;
    }

    if (!moveToInitPositionIfEnabled()) {
        last_status_ = Status::ROBOT_COMMAND_FAILED;
        return false;
    }

    finishActivityUnlocked(Phase::DONE, Status::DONE);
    retract_start_position_valid_ = false;
    retract_start_position_base_.setZero();
    return true;
}

bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
                                       const Status final_status_after_retract) {
    if (!arm_) {
        return false;
    }

    const auto& retract = config_.retract();
    retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
    const auto retract_cmd = toCartesianVelocity(
        cmvr::common::math::toEigenVec6(retract.twist_tool()));
    if (retract_start_position_valid_) {
        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
                       << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
                       << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
                       << "], acceleration=" << retract.acceleration()
                       << ", duration_s=" << retract.duration_s()
                       << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
                       << retract_start_position_base_.y() << ", "
                       << retract_start_position_base_.z() << "]";
    } else {
        CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
                       << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
                       << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
                       << "], acceleration=" << retract.acceleration()
                       << ", duration_s=" << retract.duration_s()
                       << ", start_tcp_base=unavailable";
    }

    if (!runArmActuationIfCurrent([this, &retract_cmd, &retract] {
            return arm_ && arm_->speedL(
                retract_cmd,
                retract.acceleration(),
                0.0,
                device::FrameType::Tool).ok();
        })) {
        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed";
        return false;
    }

    phase_ = Phase::RETRACTING;
    phase_after_retract_ = next_phase_after_retract;
    final_status_after_retract_ = final_status_after_retract;
    phase_start_time_ = Clock::now();
    last_retract_log_time_ = phase_start_time_;
    retract_command_started_ = true;
    last_status_ = Status::RETRACTING;
    return true;
}

void TouchScreenTask::enterFailed(const Status status) {
    (void)runArmStopIfCurrent([this] {
        return arm_ && arm_->stopL().ok();
    });
    hardStopIbvsMotion();
    holdCurrentControlledPosition();
    const auto final_status = moveToInitPositionIfEnabled()
        ? status
        : Status::ROBOT_COMMAND_FAILED;
    finishActivityUnlocked(Phase::FAILED, final_status);
}

bool TouchScreenTask::updateTouchPressure() {
    last_touch_pressure_sum_ = 0.0;
    last_touch_nonzero_count_ = 0;
    if (!dexhand_) {
        return false;
    }

    const auto& tactile = config_.touch().tactile();
    std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
    if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
                                       toTactileRegion(tactile.region()),
                                       tactile_regions)) {
        return false;
    }

    double resultant_value = 0.0;
    double resultant_fz = 0.0;
    try {
        for (const auto& tactile_region : tactile_regions) {
            const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second);
            resultant_value += tactileForceValue(resultant_force, tactile.criterion());
            resultant_fz += static_cast<double>(resultant_force.fz);
        }
    } catch (...) {
        return false;
    }

    last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0;
    last_touch_pressure_sum_ = resultant_value;
    if (phase_ == Phase::TOUCHING) {
        CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << resultant_fz
                  << ", criterion_value=" << resultant_value
                  << ", threshold=" << tactile.force_threshold()
                  ;
    }
    return true;
}

} // namespace cmvr::task