#![forbid(unsafe_code)]

use std::env;
use std::process;

use kcan::protocol::{direct_command_frame, mit_command_frame, mit_helper_frame};
use kcan::protocols::robstride::{robstride_command_frame, robstride_status_query_frame};
use kcan::{
    CanFrame, DirectCommand, MitCommand, MitHelperCommand, MotorModel, OriginMode,
    RobStrideCommand, RobStrideFrameKind, RobStrideMitCommand, RobStrideModel,
    RobStridePositionCommand, RobStrideRawCommand, RobStrideSpeedCommand,
};

const HELP: &str = "kcan - Knott Dynamics CAN actuator command

Usage:
  kcan help
  kcan models [cubemars|robstride]
  kcan cubemars mit <model> <id> [position_rad velocity_rad_s kp kd torque_nm]
  kcan cubemars helper <model> <id> <enable|disable|zero|enable-legacy|disable-legacy>
  kcan cubemars direct <id> <duty|current|brake-current|velocity|position|origin|position-velocity> <values...>
  kcan robstride status <model> <id>
  kcan robstride mit <model> <id> [position_rad velocity_rad_s kp kd torque_nm]
  kcan robstride position <model> <id> <position_rad> <velocity_rad_s> <max_torque_nm>
  kcan robstride speed <model> <id> <velocity_rad_s> <max_torque_nm>
  kcan robstride raw <model> <id> <mit|position|speed|status|config> <8 hex bytes>

Scrin and Aisling dashboards stay available through feature-gated examples.
";

fn main() {
    if let Err(message) = run(env::args().skip(1).collect()) {
        eprintln!("error: {message}");
        eprintln!("run `kcan help` for usage");
        process::exit(2);
    }
}

fn run(args: Vec<String>) -> Result<(), String> {
    let output = dispatch(&args)?;
    if !output.is_empty() {
        println!("{output}");
    }
    Ok(())
}

fn dispatch(args: &[String]) -> Result<String, String> {
    match args {
        [] => Ok(HELP.trim_end().to_owned()),
        [command] if command == "help" || command == "--help" || command == "-h" => {
            Ok(HELP.trim_end().to_owned())
        }
        [command, rest @ ..] if command == "models" => models_command(rest),
        [family, rest @ ..] if family == "cubemars" => cubemars_command(rest),
        [family, rest @ ..] if family == "robstride" || family == "rs" => robstride_command(rest),
        [other, ..] => Err(format!("unknown command {other:?}")),
    }
}

fn models_command(args: &[String]) -> Result<String, String> {
    match args {
        [] => Ok(format!(
            "CubeMars:\n{}\n\nRobStride:\n{}",
            cubemars_models(),
            robstride_models()
        )),
        [family] if family == "cubemars" || family == "cm" => Ok(cubemars_models()),
        [family] if family == "robstride" || family == "rs" => Ok(robstride_models()),
        [other, ..] => Err(format!("unknown model family {other:?}")),
    }
}

fn cubemars_models() -> String {
    MotorModel::all()
        .iter()
        .map(|model| {
            let spec = model.spec();
            format!(
                "{:<9} layout={:<8} gear={} max_erpm={}",
                model.name(),
                if model.uses_extended_mit_layout() {
                    "extended"
                } else {
                    "standard"
                },
                spec.gear_ratio,
                spec.max_velocity_erpm
            )
        })
        .collect::<Vec<_>>()
        .join("\n")
}

fn robstride_models() -> String {
    RobStrideModel::all()
        .iter()
        .map(|model| {
            let spec = model.spec();
            format!(
                "{:<5} torque={:.1}Nm speed={:.0}rpm kp={:.0}..{:.0} kd={:.0}..{:.0}",
                model.name(),
                spec.max_torque_nm,
                spec.max_speed_rpm,
                spec.kp_min,
                spec.kp_max,
                spec.kd_min,
                spec.kd_max
            )
        })
        .collect::<Vec<_>>()
        .join("\n")
}

fn cubemars_command(args: &[String]) -> Result<String, String> {
    match args {
        [command, rest @ ..] if command == "mit" => {
            let frame = cubemars_mit(rest)?;
            Ok(format_frame(&frame))
        }
        [command, rest @ ..] if command == "helper" => {
            let frame = cubemars_helper(rest)?;
            Ok(format_frame(&frame))
        }
        [command, rest @ ..] if command == "direct" => {
            let frame = cubemars_direct(rest)?;
            Ok(format_frame(&frame))
        }
        [] => Err("missing CubeMars command".to_owned()),
        [other, ..] => Err(format!("unknown CubeMars command {other:?}")),
    }
}

fn cubemars_mit(args: &[String]) -> Result<CanFrame, String> {
    let (model, motor_id, values) = parse_model_and_id(args)?;
    let command = match values {
        [] => MitCommand::neutral(),
        [position, velocity, kp, kd, torque] => MitCommand {
            position_rad: parse_f64(position, "position_rad")?,
            velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
            kp: parse_f64(kp, "kp")?,
            kd: parse_f64(kd, "kd")?,
            torque_nm: parse_f64(torque, "torque_nm")?,
        },
        _ => return Err("mit expects either no values or five numeric values".to_owned()),
    };

    mit_command_frame(model, motor_id, command).map_err(|err| err.to_string())
}

fn cubemars_helper(args: &[String]) -> Result<CanFrame, String> {
    let (model, motor_id, values) = parse_model_and_id(args)?;
    let [helper] = values else {
        return Err("helper expects one helper name".to_owned());
    };

    let helper = helper
        .parse::<MitHelperCommand>()
        .map_err(|err| err.replace("MIT helper command", "helper"))?;

    mit_helper_frame(model, motor_id, helper).map_err(|err| err.to_string())
}

fn cubemars_direct(args: &[String]) -> Result<CanFrame, String> {
    let [motor_id, command, values @ ..] = args else {
        return Err("direct expects an id, command, and values".to_owned());
    };
    let motor_id = parse_u8(motor_id, "id")?;
    let command = match (command.as_str(), values) {
        ("duty", [value]) => DirectCommand::DutyCycle(parse_f64(value, "duty")?),
        ("current", [value]) => DirectCommand::Current(parse_f64(value, "current")?),
        ("brake-current", [value]) => {
            DirectCommand::BrakeCurrent(parse_f64(value, "brake-current")?)
        }
        ("velocity", [value]) => DirectCommand::Velocity(parse_i32(value, "velocity")?),
        ("position", [value]) => DirectCommand::Position(parse_f64(value, "position")?),
        ("origin", [mode]) => DirectCommand::SetOrigin(parse_origin_mode(mode)?),
        ("position-velocity", [position, velocity, acceleration]) => {
            DirectCommand::PositionVelocity {
                position_degrees: parse_f64(position, "position")?,
                velocity_erpm: parse_i32(velocity, "velocity")?,
                acceleration_erpm_per_sec: parse_i32(acceleration, "acceleration")?,
            }
        }
        (other, _) => return Err(format!("unknown or incomplete direct command {other:?}")),
    };

    direct_command_frame(motor_id, command).map_err(|err| err.to_string())
}

fn robstride_command(args: &[String]) -> Result<String, String> {
    match args {
        [command, rest @ ..] if command == "status" => {
            let (_model, motor_id, values) = parse_robstride_model_and_id(rest)?;
            if !values.is_empty() {
                return Err("status does not take payload values".to_owned());
            }
            Ok(format_frame(
                &robstride_status_query_frame(motor_id).map_err(|err| err.to_string())?,
            ))
        }
        [command, rest @ ..] if command == "mit" => {
            let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
            let command = match values {
                [] => RobStrideMitCommand::neutral(),
                [position, velocity, kp, kd, torque] => RobStrideMitCommand {
                    position_rad: parse_f64(position, "position_rad")?,
                    velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
                    kp: parse_f64(kp, "kp")?,
                    kd: parse_f64(kd, "kd")?,
                    torque_nm: parse_f64(torque, "torque_nm")?,
                },
                _ => return Err("mit expects either no values or five numeric values".to_owned()),
            };
            Ok(format_frame(
                &robstride_command_frame(model, motor_id, RobStrideCommand::Mit(command))
                    .map_err(|err| err.to_string())?,
            ))
        }
        [command, rest @ ..] if command == "position" => {
            let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
            let [position, velocity, torque] = values else {
                return Err("position expects position, velocity, and max torque".to_owned());
            };
            Ok(format_frame(
                &robstride_command_frame(
                    model,
                    motor_id,
                    RobStrideCommand::Position(RobStridePositionCommand {
                        position_rad: parse_f64(position, "position_rad")?,
                        velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
                        max_torque_nm: parse_f64(torque, "max_torque_nm")?,
                    }),
                )
                .map_err(|err| err.to_string())?,
            ))
        }
        [command, rest @ ..] if command == "speed" => {
            let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
            let [velocity, torque] = values else {
                return Err("speed expects velocity and max torque".to_owned());
            };
            Ok(format_frame(
                &robstride_command_frame(
                    model,
                    motor_id,
                    RobStrideCommand::Speed(RobStrideSpeedCommand {
                        velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
                        max_torque_nm: parse_f64(torque, "max_torque_nm")?,
                    }),
                )
                .map_err(|err| err.to_string())?,
            ))
        }
        [command, rest @ ..] if command == "raw" => {
            let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
            let [kind, bytes @ ..] = values else {
                return Err("raw expects kind and 8 hex bytes".to_owned());
            };
            if bytes.len() != 8 {
                return Err("raw expects exactly 8 hex bytes".to_owned());
            }
            let mut data = [0_u8; 8];
            for (index, value) in bytes.iter().enumerate() {
                data[index] = parse_hex_byte(value)?;
            }
            let kind = parse_robstride_kind(kind)?;
            Ok(format_frame(
                &robstride_command_frame(
                    model,
                    motor_id,
                    RobStrideCommand::Raw(RobStrideRawCommand { kind, data }),
                )
                .map_err(|err| err.to_string())?,
            ))
        }
        [] => Err("missing RobStride command".to_owned()),
        [other, ..] => Err(format!("unknown RobStride command {other:?}")),
    }
}

fn parse_model_and_id(args: &[String]) -> Result<(MotorModel, u8, &[String]), String> {
    let [model, motor_id, rest @ ..] = args else {
        return Err("expected model and id".to_owned());
    };
    let model = MotorModel::from_name(model)
        .ok_or_else(|| format!("unsupported CubeMars model {model:?}"))?;
    let motor_id = parse_u8(motor_id, "id")?;
    Ok((model, motor_id, rest))
}

fn parse_robstride_model_and_id(
    args: &[String],
) -> Result<(RobStrideModel, u8, &[String]), String> {
    let [model, motor_id, rest @ ..] = args else {
        return Err("expected model and id".to_owned());
    };
    let model = RobStrideModel::from_name(model)
        .ok_or_else(|| format!("unsupported RobStride model {model:?}"))?;
    let motor_id = parse_u8(motor_id, "id")?;
    Ok((model, motor_id, rest))
}

fn parse_origin_mode(value: &str) -> Result<OriginMode, String> {
    value.parse()
}

fn parse_robstride_kind(value: &str) -> Result<RobStrideFrameKind, String> {
    value.parse()
}

fn parse_u8(value: &str, name: &str) -> Result<u8, String> {
    let parsed = if let Some(hex) = value
        .strip_prefix("0x")
        .or_else(|| value.strip_prefix("0X"))
    {
        u8::from_str_radix(hex, 16)
    } else {
        value.parse::<u8>()
    };

    parsed.map_err(|err| format!("invalid {name} {value:?}: {err}"))
}

fn parse_i32(value: &str, name: &str) -> Result<i32, String> {
    value
        .parse::<i32>()
        .map_err(|err| format!("invalid {name} {value:?}: {err}"))
}

fn parse_f64(value: &str, name: &str) -> Result<f64, String> {
    value
        .parse::<f64>()
        .map_err(|err| format!("invalid {name} {value:?}: {err}"))
}

fn parse_hex_byte(value: &str) -> Result<u8, String> {
    let value = value
        .strip_prefix("0x")
        .or_else(|| value.strip_prefix("0X"))
        .unwrap_or(value);
    u8::from_str_radix(value, 16).map_err(|err| format!("invalid hex byte {value:?}: {err}"))
}

fn format_frame(frame: &CanFrame) -> String {
    frame.to_string()
}
