mecha10-core 0.8.1

use crate::{Mecha10Error, Result};
use anyhow::Context as _;
use serde::de::DeserializeOwned;
use std::env;
use std::fs;
use std::path::Path;

pub mod defaults {
    /// Default odometry update rate in Hz.
    pub fn default_odom_rate_hz() -> f32 {
        50.0
    }

    /// Default maximum linear velocity in m/s.
    pub fn default_max_linear_vel() -> f32 {
        10.0
    }

    /// Default maximum angular velocity in rad/s.
    pub fn default_max_angular_vel() -> f32 {
        5.0
    }

    /// Default physics acceleration ceiling in m/s^2. Matches the `mecha10 init`-scaffolded
    /// `mecha10.json` template's own value (`config/mecha10.json.template`) - a project that
    /// deletes its `actuator.locomotion` block entirely should still get reasonably fast
    /// movement, not fall back to the old, overly conservative `2.0` that pre-dates
    /// `mecha10.json` being the sole source for this value (acceleration used to also come from
    /// the robot model's own `model.json`, which bundled `5.0` for the stock rover).
    pub fn default_max_acceleration() -> f32 {
        8.0
    }
}

pub const LOCOMOTION_MAX_LINEAR_VEL_ENV_VAR: &str = "MECHA10_LOCOMOTION_MAX_LINEAR_SPEED";
pub const LOCOMOTION_MAX_ANGULAR_VEL_ENV_VAR: &str = "MECHA10_LOCOMOTION_MAX_ANGULAR_SPEED";
pub const LOCOMOTION_MAX_ACCELERATION_ENV_VAR: &str = "MECHA10_LOCOMOTION_ACCELERATION";

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LocomotionLimits {
    pub max_linear_vel: f32,
    pub max_angular_vel: f32,
    pub max_acceleration: f32,
}

/// Resolve this process's locomotion limits from the env vars `mecha10 dev`/`mecha10 run`
/// inject from the project's `mecha10.json` `actuator.locomotion`. Every node that
/// enforces or reports linear/angular velocity (motor, teleop, llm-command, rl, navigation
/// behaviors) calls this instead of carrying its own config field or compiled default -
/// there is exactly one place in a project to set these values.
pub fn locomotion_limits() -> LocomotionLimits {
    LocomotionLimits {
        max_linear_vel: env_f32_or(LOCOMOTION_MAX_LINEAR_VEL_ENV_VAR, defaults::default_max_linear_vel()),
        max_angular_vel: env_f32_or(LOCOMOTION_MAX_ANGULAR_VEL_ENV_VAR, defaults::default_max_angular_vel()),
        max_acceleration: env_f32_or(
            LOCOMOTION_MAX_ACCELERATION_ENV_VAR,
            defaults::default_max_acceleration(),
        ),
    }
}

fn env_f32_or(key: &str, default: f32) -> f32 {
    env::var(key).ok().and_then(|v| v.parse().ok()).unwrap_or(default)
}

pub const ACTUATOR_JOINT_LIMITS_ENV_VAR: &str = "MECHA10_ACTUATOR_JOINT_LIMITS";

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct JointLimits {
    pub min_angle_rad: f32,
    pub max_angle_rad: f32,
}

#[derive(serde::Deserialize)]
struct JointLimitEntry {
    joint_id: String,
    min_angle_rad: f32,
    max_angle_rad: f32,
}

/// Resolve `joint_id`'s joint angle limits from the project's `mecha10.json` `actuator.joints`
/// array, injected by `mecha10 dev`/`mecha10 run` as one JSON-encoded env var
/// (`MECHA10_ACTUATOR_JOINT_LIMITS`, a `[{"joint_id": ..., "min_angle_rad": ..., "max_angle_rad":
/// ...}, ...]` array) - mirrors `locomotion_limits()`'s role for drivetrain nodes, but for an
/// open-ended set of named actuators rather than 3 fixed scalars, so it can't be individual env
/// vars. Returns `None` when the env var is unset or `joint_id` isn't in it - callers (e.g. the
/// `servo`/`pan-tilt` nodes in `mecha10-nodes`, out of scope here) fall back to their own
/// per-instance config default in that case, the same "project config wins, node default is the
/// fallback" precedent `NodeConfigBase` already establishes.
pub fn joint_limits(joint_id: &str) -> Option<JointLimits> {
    let raw = env::var(ACTUATOR_JOINT_LIMITS_ENV_VAR).ok()?;
    let entries: Vec<JointLimitEntry> = serde_json::from_str(&raw).ok()?;
    entries
        .into_iter()
        .find(|entry| entry.joint_id == joint_id)
        .map(|entry| JointLimits {
            min_angle_rad: entry.min_angle_rad,
            max_angle_rad: entry.max_angle_rad,
        })
}

/// Read an environment variable, falling back to `default` if it's unset (or
/// not valid unicode).
///
/// # Example
///
/// ```
/// use mecha10_core::config::env_or_default;
///
/// std::env::remove_var("MECHA10_EXAMPLE_ENV_VAR");
/// assert_eq!(env_or_default("MECHA10_EXAMPLE_ENV_VAR", "fallback"), "fallback");
/// ```
pub fn env_or_default(key: &str, default: &str) -> String {
    env::var(key).unwrap_or_else(|_| default.to_string())
}

/// Convert an `http(s)://` URL into its `ws(s)://` equivalent.
///
/// - `https://` is replaced with `wss://`
/// - `http://` is replaced with `ws://`
/// - if the input has no recognized scheme prefix at all, `wss://` is
///   prepended rather than leaving the URL scheme-less. A schemeless URL
///   (e.g. a `control_plane` value copied from config without a protocol)
///   would otherwise silently produce a malformed relay URL that fails to
///   connect.
///
/// # Example
///
/// ```
/// use mecha10_core::config::to_websocket_url;
///
/// assert_eq!(to_websocket_url("https://example.com"), "wss://example.com");
/// assert_eq!(to_websocket_url("http://example.com"), "ws://example.com");
/// assert_eq!(to_websocket_url("example.com"), "wss://example.com");
/// ```
pub fn to_websocket_url(url: &str) -> String {
    if let Some(rest) = url.strip_prefix("https://") {
        format!("wss://{}", rest)
    } else if let Some(rest) = url.strip_prefix("http://") {
        format!("ws://{}", rest)
    } else {
        format!("wss://{}", url)
    }
}

/// Build the WebRTC relay URL for camera signaling from a resolved control plane base URL:
/// an explicit `WEBRTC_RELAY_URL` env var override if set, else the control plane URL converted
/// to a websocket scheme (see [`to_websocket_url`]) with `/webrtc-relay` appended.
///
/// Shared by `mecha10-cli-core`'s `EnvironmentsConfig::relay_url` and `launcher-service`'s
/// `ProjectConfig::relay_url` (LAB-2892, cargo-dupes group E89), which were byte-for-byte
/// identical modulo the `self`/method they're defined on.
///
/// # Example
///
/// ```
/// use mecha10_core::config::relay_url;
///
/// std::env::remove_var("WEBRTC_RELAY_URL");
/// assert_eq!(relay_url("https://mecha.industries"), "wss://mecha.industries/webrtc-relay");
/// ```
pub fn relay_url(control_plane_url: &str) -> String {
    if let Ok(url) = env::var("WEBRTC_RELAY_URL") {
        return url;
    }

    format!("{}/webrtc-relay", to_websocket_url(control_plane_url))
}

/// Resolve the environment config matching a symbolic environment name ("dev"/"development",
/// "staging", "prod"/"production", anything else -> `None`).
///
/// Generic over the per-environment config type `E` since `mecha10-cli-core`'s and
/// `launcher-service`'s `EnvironmentConfig` types carry different fields; `prod` is `Option<&E>`
/// so callers whose `prod` slot is unconditionally populated (not itself optional) can just pass
/// `Some(&self.prod)`.
///
/// Shared by both crates' `EnvironmentsConfig::get` (LAB-2892, cargo-dupes group N53).
pub fn lookup_environment<'a, E>(env: &str, dev: &'a E, staging: Option<&'a E>, prod: Option<&'a E>) -> Option<&'a E> {
    match env {
        "dev" | "development" => Some(dev),
        "staging" => staging,
        "prod" | "production" => prod,
        _ => None,
    }
}

/// Resolve the config for the currently active environment, falling back to `dev` when
/// [`lookup_environment`] finds no match for `env_name` (e.g. `staging`/`prod` selected but not
/// configured in this project).
///
/// Shared by both crates' `EnvironmentsConfig::current` (LAB-2892, cargo-dupes group E88).
pub fn current_environment<'a, E>(env_name: &str, dev: &'a E, staging: Option<&'a E>, prod: Option<&'a E>) -> &'a E {
    lookup_environment(env_name, dev, staging, prod).unwrap_or(dev)
}

/// Build the base set of environment variables that a spawned robot/node process needs to
/// reach the control plane, WebRTC relay, and Redis, and to identify itself.
///
/// This is the subset that's genuinely identical between `mecha10 dev`'s node spawning
/// (`packages/cli/src/handlers/dev/ops/spawn.rs`'s `project_env` construction) and the
/// launcher service's `start_robot_binary`
/// (`packages/launcher-service/src/service.rs`) - both derive `CONTROL_PLANE_URL`,
/// `WEBRTC_RELAY_URL`, `REDIS_URL`, and `ROBOT_ID`/`ROBOT_API_KEY` from the same inputs, so
/// this gives them one place to do it instead of two hand-rolled copies. Callers layer their
/// own extras on top (e.g. the launcher additionally sets `MECHA10_ENV`,
/// `MECHA10_PLATFORM_URL`, `MECHA10_DEVICE_ID`, and `MECHA10_`-prefixed duplicates of these
/// same values; `mecha10 dev` additionally injects TURN credentials and simulator overrides).
///
/// `api_key` is `None` when there are no credentials to inject (e.g. not logged in) - in that
/// case `ROBOT_API_KEY` is simply omitted from the result rather than set to an empty string,
/// matching `mecha10 dev`'s existing behavior.
///
/// # Example
///
/// ```
/// use mecha10_core::config::robot_process_env;
///
/// let env = robot_process_env(
///     "https://api.mecha.industries",
///     "wss://api.mecha.industries/webrtc-relay",
///     "redis://localhost:6379",
///     "robot-001",
///     Some("secret-key"),
/// );
/// assert!(env.contains(&("CONTROL_PLANE_URL", "https://api.mecha.industries".to_string())));
/// assert!(env.contains(&("ROBOT_API_KEY", "secret-key".to_string())));
/// ```
pub fn robot_process_env(
    control_plane_url: &str,
    relay_url: &str,
    redis_url: &str,
    robot_id: &str,
    api_key: Option<&str>,
) -> Vec<(&'static str, String)> {
    let mut env = vec![
        ("CONTROL_PLANE_URL", control_plane_url.to_string()),
        ("WEBRTC_RELAY_URL", relay_url.to_string()),
        ("REDIS_URL", redis_url.to_string()),
        ("ROBOT_ID", robot_id.to_string()),
    ];
    if let Some(key) = api_key {
        env.push(("ROBOT_API_KEY", key.to_string()));
    }
    env
}

/// Load configuration from a file with environment variable substitution.
///
/// Supports:
/// - JSON, YAML, TOML files (based on extension)
/// - Environment variable substitution with `${VAR_NAME}` syntax
/// - Environment-specific config sections (dev, production, staging)
/// - Validation via serde
///
/// This does *not* run [`Validate::validate`] -- stable Rust has no
/// specialization, so a generic function over `T` can't conditionally call a
/// trait method only when `T` happens to implement it. Use
/// [`load_and_validate`] instead for config types that implement
/// [`Validate`]; its `T: Validate` bound is real, so it can (and does) call
/// `validate()` unconditionally.
///
/// # Environment Selection
///
/// If the config file has top-level keys matching environment names (dev, development,
/// prod, production, staging), the function will select the appropriate section based
/// on the `MECHA10_ENV` environment variable (defaults to "development").
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_core::prelude::*;
///
/// #[derive(Debug, Deserialize)]
/// struct MyConfig {
///     rate: f32,
///     endpoint: String,
/// }
///
/// # fn main() -> Result<()> {
/// // Load from file
/// let config: MyConfig = load_config("config.yaml")?;
///
/// // If config.json contains:
/// // {
/// //   "dev": { "rate": 10.0, "endpoint": "http://localhost" },
/// //   "production": { "rate": 1.0, "endpoint": "https://api.example.com" }
/// // }
/// // And MECHA10_ENV=development, then config will use the "dev" section
/// # Ok(())
/// # }
/// ```
pub fn load_config<T, P>(path: P) -> Result<T>
where
    T: DeserializeOwned,
    P: AsRef<Path>,
{
    let path = path.as_ref();

    let content = fs::read_to_string(path)
        .with_context(|| format!("Failed to read config file at '{}'", path.display()))
        .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?;

    let content = substitute_env_vars(&content)
        .with_context(|| format!("Failed to substitute environment variables in '{}'", path.display()))
        .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?;

    let value: serde_json::Value = match path.extension().and_then(|s| s.to_str()) {
        Some("json") => serde_json::from_str(&content)
            .with_context(|| format!("Failed to parse JSON config from '{}'", path.display()))
            .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?,
        Some("yaml") | Some("yml") => serde_yaml::from_str(&content)
            .with_context(|| format!("Failed to parse YAML config from '{}'", path.display()))
            .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?,
        Some("toml") => {
            let toml_value: toml::Value = toml::from_str(&content)
                .with_context(|| format!("Failed to parse TOML config from '{}'", path.display()))
                .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?;
            serde_json::to_value(toml_value)
                .map_err(|e| Mecha10Error::Configuration(format!("Failed to convert TOML: {}", e)))?
        }
        Some(ext) => {
            return Err(Mecha10Error::Configuration(format!(
                "Unsupported config file extension: {}",
                ext
            )));
        }
        None => {
            return Err(Mecha10Error::Configuration("Config file has no extension".to_string()));
        }
    };

    let final_value = extract_env_config(value);

    let config: T = serde_json::from_value(final_value)
        .with_context(|| format!("Failed to parse config from '{}'", path.display()))
        .map_err(|e| Mecha10Error::Configuration(format!("{:#}", e)))?;

    Ok(config)
}

/// Extract environment-specific config section.
///
/// Node configs MUST have environment keys (dev, development, prod, production, staging, test).
/// Returns the appropriate section based on MECHA10_ENV (defaults to "development").
///
/// Flat configs are not supported - all node configs must be nested by environment.
fn extract_env_config(value: serde_json::Value) -> serde_json::Value {
    let env_keys = ["dev", "development", "prod", "production", "staging", "test"];

    if let serde_json::Value::Object(ref map) = value {
        let current_env = env::var("MECHA10_ENV").unwrap_or_else(|_| "development".to_string());

        let env_key = match current_env.as_str() {
            "development" | "dev" => {
                if map.contains_key("dev") {
                    "dev"
                } else {
                    "development"
                }
            }
            "production" | "prod" => {
                if map.contains_key("production") {
                    "production"
                } else {
                    "prod"
                }
            }
            other => other,
        };

        if let Some(env_config) = map.get(env_key) {
            return env_config.clone();
        }

        let has_any_env_keys = map.keys().any(|k| env_keys.contains(&k.as_str()));
        if has_any_env_keys {
            return serde_json::Value::Object(serde_json::Map::new());
        }

        return serde_json::Value::Object(serde_json::Map::new());
    }

    serde_json::Value::Object(serde_json::Map::new())
}

/// Load configuration from environment variables with a prefix.
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_core::prelude::*;
///
/// #[derive(Debug, Deserialize)]
/// struct MyConfig {
///     rate: f32,
///     endpoint: String,
/// }
///
/// # fn main() -> Result<()> {
/// // Load from environment variables:
/// // MYAPP_RATE=10.5
/// // MYAPP_ENDPOINT=http://localhost:8080
/// let config: MyConfig = load_config_from_env("MYAPP")?;
/// # Ok(())
/// # }
/// ```
pub fn load_config_from_env<T>(prefix: &str) -> Result<T>
where
    T: DeserializeOwned,
{
    let mut config_map = serde_json::Map::new();

    for (key, value) in env::vars() {
        if let Some(suffix) = key.strip_prefix(&format!("{}_", prefix)) {
            let json_key = suffix.to_lowercase();

            let json_value = serde_json::from_str(&value).unwrap_or(serde_json::Value::String(value));

            config_map.insert(json_key, json_value);
        }
    }

    let config: T = serde_json::from_value(serde_json::Value::Object(config_map))
        .map_err(|e| Mecha10Error::Configuration(format!("Failed to parse config from environment: {}", e)))?;

    Ok(config)
}

/// Substitute environment variables in a string.
///
/// Syntax: `${VAR_NAME}` or `$VAR_NAME`
///
/// # Example
///
/// ```rust
/// use mecha10_core::config::substitute_env_vars;
///
/// std::env::set_var("MY_VAR", "hello");
/// let result = substitute_env_vars("Value is: ${MY_VAR}").unwrap();
/// assert_eq!(result, "Value is: hello");
/// ```
pub fn substitute_env_vars(input: &str) -> Result<String> {
    let mut result = input.to_string();

    let re = regex::Regex::new(r"\$\{([A-Z_][A-Z0-9_]*)\}").unwrap();

    for cap in re.captures_iter(input) {
        let var_name = &cap[1];
        let var_value = env::var(var_name).map_err(|_| {
            Mecha10Error::Configuration(format!(
                "Environment variable {} not found (required by config)",
                var_name
            ))
        })?;

        result = result.replace(&format!("${{{}}}", var_name), &var_value);
    }

    let re = regex::Regex::new(r"\$([A-Z_][A-Z0-9_]*)").unwrap();

    for cap in re.captures_iter(input) {
        let var_name = &cap[1];
        if let Ok(var_value) = env::var(var_name) {
            result = result.replace(&format!("${}", var_name), &var_value);
        }
    }

    Ok(result)
}

pub trait Validate {
    /// Validate the configuration.
    ///
    /// Returns an error if validation fails.
    fn validate(&self) -> Result<()>;
}

/// Load and validate configuration.
///
/// This is a convenience function that combines [`load_config`] with
/// [`Validate::validate`]: it deserializes `T` from `path`, then calls
/// `validate()` on the result and propagates any error it returns.
pub fn load_and_validate<T, P>(path: P) -> Result<T>
where
    T: DeserializeOwned + Validate,
    P: AsRef<Path>,
{
    let config: T = load_config(path)?;
    config.validate()?;
    Ok(config)
}

/// Load project configuration with JSON Schema validation
///
/// This function loads a mecha10.json file and validates it against the built-in schema.
/// Use this for loading project-level configuration files.
///
/// Requires the `schema-validation` feature.
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_core::config::load_project_config;
///
/// fn main() -> Result<(), Box<dyn std::error::Error>> {
///     let config = load_project_config("mecha10.json")?;
///     println!("Project: {}", config["name"]);
///     Ok(())
/// }
/// ```
#[cfg(feature = "schema-validation")]
pub fn load_project_config<P: AsRef<Path>>(path: P) -> Result<serde_json::Value> {
    use crate::schema_validation::load_and_validate_project;
    load_and_validate_project(path)
}

/// Convert a config key to its on-disk display path.
///
/// Keys are category-prefixed (`nodes/...`, `behaviors/...`, `rl/...` - see
/// `mecha10_robot_config::sync::derive_config_key`/`collect_rl_configs`), so the prefix already
/// tells us which directory the config lives under; this just appends `config.json` (except for
/// `rl/...`, whose keys are already full paths - see the `rl/jobs/train.yml` example below).
///
/// Examples:
/// - `mecha10.json` -> `mecha10.json`
/// - `nodes/@mecha10/motor` -> `nodes/@mecha10/motor/config.json`
/// - `behaviors/@mecha10/idle-wander` -> `behaviors/@mecha10/idle-wander/config.json`
/// - `simulation` -> `simulation/config.json`
/// - `rl/jobs/train.yml` -> `rl/jobs/train.yml` (already a full path - see
///   `mecha10_robot_config::sync::collect_rl_configs`, no fixed `config.json` filename)
pub fn config_key_to_path(key: &str) -> String {
    if key == "mecha10.json" || key.starts_with("rl/") {
        key.to_string()
    } else {
        format!("{}/config.json", key)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_config_key_to_path_project_config() {
        assert_eq!(config_key_to_path("mecha10.json"), "mecha10.json");
    }

    #[test]
    fn test_config_key_to_path_scoped_node() {
        assert_eq!(
            config_key_to_path("nodes/@mecha10/motor"),
            "nodes/@mecha10/motor/config.json"
        );
    }

    #[test]
    fn test_config_key_to_path_scoped_behavior() {
        assert_eq!(
            config_key_to_path("behaviors/@mecha10/idle-wander"),
            "behaviors/@mecha10/idle-wander/config.json"
        );
    }

    #[test]
    fn test_config_key_to_path_simple_key() {
        assert_eq!(config_key_to_path("simulation"), "simulation/config.json");
    }

    #[test]
    fn test_config_key_to_path_rl_config() {
        assert_eq!(config_key_to_path("rl/jobs/train.yml"), "rl/jobs/train.yml");
        assert_eq!(config_key_to_path("rl/jobs/train-cat.yml"), "rl/jobs/train-cat.yml");
    }

    #[test]
    fn test_to_websocket_url_https() {
        assert_eq!(to_websocket_url("https://mecha.industries"), "wss://mecha.industries");
    }

    #[test]
    fn test_to_websocket_url_http() {
        assert_eq!(to_websocket_url("http://localhost:8000"), "ws://localhost:8000");
    }

    #[test]
    fn test_to_websocket_url_no_scheme() {
        assert_eq!(to_websocket_url("mecha.industries"), "wss://mecha.industries");
    }

    #[test]
    fn test_robot_process_env_with_api_key() {
        let env = robot_process_env(
            "https://api.mecha.industries",
            "wss://api.mecha.industries/webrtc-relay",
            "redis://localhost:6379",
            "robot-001",
            Some("secret-key"),
        );
        assert_eq!(
            env,
            vec![
                ("CONTROL_PLANE_URL", "https://api.mecha.industries".to_string()),
                (
                    "WEBRTC_RELAY_URL",
                    "wss://api.mecha.industries/webrtc-relay".to_string()
                ),
                ("REDIS_URL", "redis://localhost:6379".to_string()),
                ("ROBOT_ID", "robot-001".to_string()),
                ("ROBOT_API_KEY", "secret-key".to_string()),
            ]
        );
    }

    #[test]
    fn test_robot_process_env_without_api_key_omits_it() {
        let env = robot_process_env(
            "https://api.mecha.industries",
            "wss://api.mecha.industries/webrtc-relay",
            "redis://localhost:6379",
            "robot-001",
            None,
        );
        assert!(!env.iter().any(|(k, _)| *k == "ROBOT_API_KEY"));
        assert_eq!(env.len(), 4);
    }

    struct AlwaysValid;

    impl Validate for AlwaysValid {
        fn validate(&self) -> Result<()> {
            Ok(())
        }
    }

    struct AlwaysInvalid;

    impl Validate for AlwaysInvalid {
        fn validate(&self) -> Result<()> {
            Err(Mecha10Error::Configuration("always invalid".to_string()))
        }
    }

    #[test]
    fn test_validate_ok_for_a_valid_impl() {
        assert!(AlwaysValid.validate().is_ok());
    }

    #[test]
    fn test_validate_propagates_errors_for_an_invalid_impl() {
        let err = AlwaysInvalid.validate().unwrap_err();
        assert!(matches!(err, Mecha10Error::Configuration(_)));
    }

    static LOCOMOTION_ENV_LOCK: std::sync::Mutex<()> = std::sync::Mutex::new(());

    #[test]
    fn test_locomotion_limits_falls_back_to_compiled_defaults_when_env_unset() {
        let _guard = LOCOMOTION_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::remove_var(LOCOMOTION_MAX_LINEAR_VEL_ENV_VAR);
        env::remove_var(LOCOMOTION_MAX_ANGULAR_VEL_ENV_VAR);
        env::remove_var(LOCOMOTION_MAX_ACCELERATION_ENV_VAR);

        let limits = locomotion_limits();

        assert_eq!(limits.max_linear_vel, defaults::default_max_linear_vel());
        assert_eq!(limits.max_angular_vel, defaults::default_max_angular_vel());
        assert_eq!(limits.max_acceleration, defaults::default_max_acceleration());
    }

    #[test]
    fn test_locomotion_limits_reads_env_vars_when_set() {
        let _guard = LOCOMOTION_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::set_var(LOCOMOTION_MAX_LINEAR_VEL_ENV_VAR, "2.5");
        env::set_var(LOCOMOTION_MAX_ANGULAR_VEL_ENV_VAR, "3.5");
        env::set_var(LOCOMOTION_MAX_ACCELERATION_ENV_VAR, "4.5");

        let limits = locomotion_limits();

        assert_eq!(limits.max_linear_vel, 2.5);
        assert_eq!(limits.max_angular_vel, 3.5);
        assert_eq!(limits.max_acceleration, 4.5);

        env::remove_var(LOCOMOTION_MAX_LINEAR_VEL_ENV_VAR);
        env::remove_var(LOCOMOTION_MAX_ANGULAR_VEL_ENV_VAR);
        env::remove_var(LOCOMOTION_MAX_ACCELERATION_ENV_VAR);
    }

    static ACTUATOR_JOINT_LIMITS_ENV_LOCK: std::sync::Mutex<()> = std::sync::Mutex::new(());

    #[test]
    fn test_joint_limits_returns_none_when_env_unset() {
        let _guard = ACTUATOR_JOINT_LIMITS_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::remove_var(ACTUATOR_JOINT_LIMITS_ENV_VAR);

        assert_eq!(joint_limits("shoulder"), None);
    }

    #[test]
    fn test_joint_limits_returns_matching_joint_id() {
        let _guard = ACTUATOR_JOINT_LIMITS_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::set_var(
            ACTUATOR_JOINT_LIMITS_ENV_VAR,
            r#"[{"joint_id":"shoulder","min_angle_rad":-1.5,"max_angle_rad":1.5},{"joint_id":"elbow","min_angle_rad":0.0,"max_angle_rad":2.0}]"#,
        );

        assert_eq!(
            joint_limits("shoulder"),
            Some(JointLimits {
                min_angle_rad: -1.5,
                max_angle_rad: 1.5,
            })
        );
        assert_eq!(
            joint_limits("elbow"),
            Some(JointLimits {
                min_angle_rad: 0.0,
                max_angle_rad: 2.0,
            })
        );

        env::remove_var(ACTUATOR_JOINT_LIMITS_ENV_VAR);
    }

    #[test]
    fn test_joint_limits_returns_none_when_joint_id_absent() {
        let _guard = ACTUATOR_JOINT_LIMITS_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::set_var(
            ACTUATOR_JOINT_LIMITS_ENV_VAR,
            r#"[{"joint_id":"elbow","min_angle_rad":0.0,"max_angle_rad":2.0}]"#,
        );

        assert_eq!(joint_limits("shoulder"), None);

        env::remove_var(ACTUATOR_JOINT_LIMITS_ENV_VAR);
    }

    #[test]
    fn test_joint_limits_returns_none_on_malformed_json() {
        let _guard = ACTUATOR_JOINT_LIMITS_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        env::set_var(ACTUATOR_JOINT_LIMITS_ENV_VAR, "not valid json");

        assert_eq!(joint_limits("shoulder"), None);

        env::remove_var(ACTUATOR_JOINT_LIMITS_ENV_VAR);
    }
}