use std::sync::Arc;
use std::time::Duration;

use rmcp::handler::server::router::tool::ToolRouter;
use rmcp::handler::server::wrapper::Parameters;
use rmcp::model::{ServerCapabilities, ServerInfo};
use rmcp::{ServerHandler, schemars, tool, tool_handler, tool_router};

use crate::grbl;
use crate::serial::SerialConnection;

#[derive(Debug, serde::Deserialize, schemars::JsonSchema)]
pub struct MoveParams {
    #[schemars(description = "X axis value in mm")]
    pub x: Option<f64>,
    #[schemars(description = "Y axis value in mm")]
    pub y: Option<f64>,
    #[schemars(description = "Z axis value in mm")]
    pub z: Option<f64>,
    #[schemars(description = "Feed rate in mm/min. If omitted, rapid move (G00) is used")]
    pub feed_rate: Option<f64>,
}

#[derive(Clone)]
pub struct GrblServer {
    serial: Arc<SerialConnection>,
    tool_router: ToolRouter<Self>,
}

impl GrblServer {
    pub fn new(serial: Arc<SerialConnection>) -> Self {
        Self {
            serial,
            tool_router: Self::tool_router(),
        }
    }
}

#[tool_router]
impl GrblServer {
    #[tool(
        description = "Identify the CNC controller by reading its startup banner and build info. Use this first to confirm the device is GRBL-compatible."
    )]
    async fn probe_device(&self) -> String {
        match self.serial.probe().await {
            Ok(output) => output,
            Err(e) => format!("Failed to probe device: {e}"),
        }
    }

    #[tool(
        description = "Query the CNC controller's current status including machine state and XYZ position."
    )]
    async fn status(&self) -> String {
        match self.serial.send_realtime(b'?').await {
            Ok(raw) => {
                for line in raw.lines() {
                    let line = line.trim();
                    if line.starts_with('<')
                        && let Some(status) = grbl::parse_status(line)
                    {
                        let (x, y, z) = status.machine_pos;
                        let mut text = format!(
                            "State: {}\nMachine Position: X={:.3} Y={:.3} Z={:.3}",
                            status.state, x, y, z
                        );
                        if let Some((wx, wy, wz)) = status.work_pos {
                            text.push_str(&format!(
                                "\nWork Position: X={wx:.3} Y={wy:.3} Z={wz:.3}"
                            ));
                        }
                        return text;
                    }
                }
                raw
            }
            Err(e) => format!("Failed to query status: {e}"),
        }
    }

    #[tool(
        description = "Execute the homing cycle ($H). The machine moves to its limit switches to establish a known position. This may take up to 60 seconds."
    )]
    async fn home(&self) -> String {
        match self
            .serial
            .send_command("$H", Duration::from_secs(60))
            .await
        {
            Ok(resp) => {
                let parsed = grbl::parse_response(resp.lines().last().unwrap_or(""));
                match parsed {
                    grbl::GrblResponse::Ok => "Homing cycle completed successfully.".to_string(),
                    grbl::GrblResponse::Error(code) => {
                        format!("Homing failed with GRBL error code {code}")
                    }
                    _ => resp,
                }
            }
            Err(e) => format!("Homing failed: {e}"),
        }
    }

    #[tool(
        description = "Move the CNC head by a relative offset in X, Y, and/or Z (incremental mode G91). At least one axis must be specified. Units are millimeters. If feed_rate is provided, uses controlled move (G01); otherwise uses rapid move (G00)."
    )]
    async fn move_relative(&self, Parameters(params): Parameters<MoveParams>) -> String {
        if params.x.is_none() && params.y.is_none() && params.z.is_none() {
            return "Error: at least one axis (x, y, or z) must be specified.".to_string();
        }

        let cmd = grbl::build_move_command(params.x, params.y, params.z, params.feed_rate);

        if let Err(e) = self
            .serial
            .send_command("G91", Duration::from_secs(5))
            .await
        {
            return format!("Failed to set relative mode: {e}");
        }

        match self
            .serial
            .send_command(&cmd, Duration::from_secs(30))
            .await
        {
            Ok(resp) => {
                let last_line = resp.lines().last().unwrap_or("");
                match grbl::parse_response(last_line) {
                    grbl::GrblResponse::Ok => format!("Relative move sent: {cmd}"),
                    grbl::GrblResponse::Error(code) => {
                        format!("Move failed with GRBL error code {code}: {resp}")
                    }
                    _ => resp,
                }
            }
            Err(e) => format!("Move failed: {e}"),
        }
    }

    #[tool(
        description = "Move the CNC head to an absolute position in X, Y, and/or Z (absolute mode G90). At least one axis must be specified. Units are millimeters. If feed_rate is provided, uses controlled move (G01); otherwise uses rapid move (G00)."
    )]
    async fn move_absolute(&self, Parameters(params): Parameters<MoveParams>) -> String {
        if params.x.is_none() && params.y.is_none() && params.z.is_none() {
            return "Error: at least one axis (x, y, or z) must be specified.".to_string();
        }

        let cmd = grbl::build_move_command(params.x, params.y, params.z, params.feed_rate);

        if let Err(e) = self
            .serial
            .send_command("G90", Duration::from_secs(5))
            .await
        {
            return format!("Failed to set absolute mode: {e}");
        }

        match self
            .serial
            .send_command(&cmd, Duration::from_secs(30))
            .await
        {
            Ok(resp) => {
                let last_line = resp.lines().last().unwrap_or("");
                match grbl::parse_response(last_line) {
                    grbl::GrblResponse::Ok => format!("Absolute move sent: {cmd}"),
                    grbl::GrblResponse::Error(code) => {
                        format!("Move failed with GRBL error code {code}: {resp}")
                    }
                    _ => resp,
                }
            }
            Err(e) => format!("Move failed: {e}"),
        }
    }
}

#[tool_handler(router = self.tool_router)]
impl ServerHandler for GrblServer {
    fn get_info(&self) -> ServerInfo {
        ServerInfo::new(ServerCapabilities::builder().enable_tools().build())
            .with_instructions("GRBL CNC controller. Tools: probe_device, status, home, move_relative, move_absolute. All positions in millimeters.")
    }
}
