use serde::{Deserialize, Serialize};

#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
pub struct Vec3 {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

impl Vec3 {
    pub fn new(x: f64, y: f64, z: f64) -> Self {
        Self { x, y, z }
    }
    pub fn lerp(&self, other: &Self, t: f64) -> Self {
        Self {
            x: self.x + t * (other.x - self.x),
            y: self.y + t * (other.y - self.y),
            z: self.z + t * (other.z - self.z),
        }
    }
    pub fn add(&self, other: &Self) -> Self {
        Self::new(self.x + other.x, self.y + other.y, self.z + other.z)
    }
    pub fn sub(&self, other: &Self) -> Self {
        Self::new(self.x - other.x, self.y - other.y, self.z - other.z)
    }
    pub fn scale(&self, s: f64) -> Self {
        Self::new(self.x * s, self.y * s, self.z * s)
    }
    pub fn neg(&self) -> Self {
        self.scale(-1.0)
    }
    pub fn dot(&self, other: &Self) -> f64 {
        self.x * other.x + self.y * other.y + self.z * other.z
    }
    pub fn length_sq(&self) -> f64 {
        self.dot(self)
    }
    pub fn length(&self) -> f64 {
        self.length_sq().sqrt()
    }
}

/// Unit quaternion `(x, y, z, w)` where `w` is the scalar part.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct Quat {
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub w: f64,
}

impl Quat {
    pub fn identity() -> Self {
        Self {
            x: 0.0,
            y: 0.0,
            z: 0.0,
            w: 1.0,
        }
    }
    pub fn new(x: f64, y: f64, z: f64, w: f64) -> Self {
        Self { x, y, z, w }
    }

    pub fn dot(&self, other: &Self) -> f64 {
        self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w
    }

    pub fn normalize(&self) -> Self {
        let len = (self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w).sqrt();
        if len < f64::EPSILON {
            return Self::identity();
        }
        Self {
            x: self.x / len,
            y: self.y / len,
            z: self.z / len,
            w: self.w / len,
        }
    }

    pub fn conjugate(&self) -> Self {
        Self {
            x: -self.x,
            y: -self.y,
            z: -self.z,
            w: self.w,
        }
    }

    pub fn mul(&self, other: &Self) -> Self {
        Self {
            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
        }
    }

    pub fn rotate_vec(&self, v: &Vec3) -> Vec3 {
        let (qx, qy, qz, w) = (self.x, self.y, self.z, self.w);
        let (vx, vy, vz) = (v.x, v.y, v.z);
        let tx = 2.0 * (qy * vz - qz * vy);
        let ty = 2.0 * (qz * vx - qx * vz);
        let tz = 2.0 * (qx * vy - qy * vx);
        Vec3 {
            x: vx + w * tx + qy * tz - qz * ty,
            y: vy + w * ty + qz * tx - qx * tz,
            z: vz + w * tz + qx * ty - qy * tx,
        }
    }

    pub fn slerp(&self, other: &Self, t: f64) -> Self {
        let mut dot = self.dot(other);
        let other_adj = if dot < 0.0 {
            dot = -dot;
            Quat {
                x: -other.x,
                y: -other.y,
                z: -other.z,
                w: -other.w,
            }
        } else {
            other.clone()
        };
        let dot = dot.min(1.0_f64);
        if dot > 0.9995 {
            return Quat {
                x: self.x + t * (other_adj.x - self.x),
                y: self.y + t * (other_adj.y - self.y),
                z: self.z + t * (other_adj.z - self.z),
                w: self.w + t * (other_adj.w - self.w),
            }
            .normalize();
        }
        let theta_0 = dot.acos();
        let sin_theta = theta_0.sin();
        let theta = theta_0 * t;
        let s0 = (theta_0 - theta).sin() / sin_theta;
        let s1 = theta.sin() / sin_theta;
        Quat {
            x: s0 * self.x + s1 * other_adj.x,
            y: s0 * self.y + s1 * other_adj.y,
            z: s0 * self.z + s1 * other_adj.z,
            w: s0 * self.w + s1 * other_adj.w,
        }
    }
}

impl Default for Quat {
    fn default() -> Self {
        Self::identity()
    }
}

impl PartialEq for Quat {
    fn eq(&self, other: &Self) -> bool {
        self.x == other.x && self.y == other.y && self.z == other.z && self.w == other.w
    }
}

/// Rigid-body spatial transform: translation + rotation.
#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
pub struct Transform {
    pub position: Vec3,
    pub rotation: Quat,
}

impl Transform {
    pub fn identity() -> Self {
        Self {
            position: Vec3::default(),
            rotation: Quat::identity(),
        }
    }
    pub fn translation(x: f64, y: f64, z: f64) -> Self {
        Self {
            position: Vec3::new(x, y, z),
            rotation: Quat::identity(),
        }
    }

    /// `T^{-1}`: inverse rigid-body transform such that `T.compose(&T.inverse()) ≅ identity`.
    pub fn inverse(&self) -> Self {
        let inv_rot = self.rotation.conjugate();
        let inv_pos = inv_rot.rotate_vec(&self.position).neg();
        Self {
            position: inv_pos,
            rotation: inv_rot,
        }
    }

    /// Compose `self` with `rhs`: equivalent to matrix product `self * rhs`.
    /// If `self = T_{a→b}` and `rhs = T_{b→c}`, result is `T_{a→c}`.
    pub fn compose(&self, rhs: &Self) -> Self {
        let rotated = self.rotation.rotate_vec(&rhs.position);
        Self {
            position: self.position.add(&rotated),
            rotation: self.rotation.mul(&rhs.rotation),
        }
    }

    pub fn interpolate(&self, other: &Self, t: f64) -> Self {
        Self {
            position: self.position.lerp(&other.position, t),
            rotation: self.rotation.slerp(&other.rotation, t),
        }
    }
}

/// A transform stamped with its source/target frame IDs and a timestamp.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct StampedTransform {
    pub frame_from: String,
    pub frame_to: String,
    /// Nanoseconds since Unix epoch.
    pub timestamp_ns: u64,
    pub transform: Transform,
}

impl StampedTransform {
    pub fn new(
        frame_from: impl Into<String>,
        frame_to: impl Into<String>,
        timestamp_ns: u64,
        transform: Transform,
    ) -> Self {
        Self {
            frame_from: frame_from.into(),
            frame_to: frame_to.into(),
            timestamp_ns,
            transform,
        }
    }
}
