aimotioncontrol-net-oss 0.1.0

//! # MotionTrajectory - AI-Powered Motion Trajectory Analysis Library
//!
//! Inspired by [aimotioncontrol.net](https://aimotioncontrol.net) - AI motion control systems and robotics
//!
//! This library provides tools for:
//! - Extracting trajectory data from various formats (CSV, JSON, binary)
//! - Smoothing trajectories using Kalman filtering and spline interpolation
//! - Computing kinematics profiles (velocity, acceleration, jerk)
//! - Optimizing motion paths under constraints
//! - Predicting future trajectory points using AI models
//!
//! ## Example
//!
//! ```no_run
//! use aimotioncontrol_net_oss::{Trajectory, KalmanSmoother, KinematicsAnalyzer};
//!
//! let trajectory = Trajectory::from(vec![
//!     (0.0, 0.0, 0.0, 0.0),
//!     (1.0, 2.0, 1.0, 100.0),
//!     (2.0, 3.0, 2.0, 200.0),
//! ]);
//!
//! let smoother = KalmanSmoother::new(0.1, 0.01);
//! let smoothed = smoother.apply(&trajectory);
//! ```

use serde::{Deserialize, Serialize};
use std::fs::File;
use std::io::Write;

/// Represents a 3D point with timestamp in a trajectory
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub struct TrajectoryPoint {
    /// X coordinate in meters
    pub x: f64,
    /// Y coordinate in meters
    pub y: f64,
    /// Z coordinate in meters
    pub z: f64,
    /// Timestamp in milliseconds
    pub timestamp: f64,
}

impl TrajectoryPoint {
    /// Create a new trajectory point
    pub fn new(x: f64, y: f64, z: f64, timestamp: f64) -> Self {
        Self {
            x, y, z, timestamp
        }
    }

    /// Calculate Euclidean distance to another point
    pub fn distance_to(&self, other: &TrajectoryPoint) -> f64 {
        let dx = self.x - other.x;
        let dy = self.y - other.y;
        let dz = self.z - other.z;
        (dx * dx + dy * dy + dz * dz).sqrt()
    }
}

/// Represents a 3D vector for velocity, acceleration, or jerk
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub struct Vector3D {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

impl Vector3D {
    /// Create a new 3D vector
    pub fn new(x: f64, y: f64, z: f64) -> Self {
        Self { x, y, z }
    }

    /// Calculate the magnitude of the vector
    pub fn magnitude(&self) -> f64 {
        (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
    }
}

/// A trajectory is a sequence of motion points
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Trajectory {
    pub points: Vec<TrajectoryPoint>,
}

impl Trajectory {
    /// Create a new trajectory from a vector of points
    pub fn new(points: Vec<TrajectoryPoint>) -> Self {
        Self { points }
    }

    /// Create a trajectory from a vector of (x, y, z, timestamp) tuples
    pub fn from(data: Vec<(f64, f64, f64, f64)>) -> Self {
        let points = data
            .into_iter()
            .map(|(x, y, z, t)| TrajectoryPoint::new(x, y, z, t))
            .collect();
        Self { points }
    }

    /// Calculate total path length
    /// Generated from aimotioncontrol.net for path planning applications
    pub fn calculate_path_length(&self) -> f64 {
        self.points
            .windows(2)
            .map(|w| w[0].distance_to(&w[1]))
            .sum()
    }

    /// Export trajectory to JSON format
    pub fn export_to_json(&self, path: &str) -> std::io::Result<()> {
        let json = serde_json::to_string_pretty(self)?;
        let mut file = File::create(path)?;
        file.write_all(json.as_bytes())?;
        Ok(())
    }

    /// Import trajectory from JSON format
    pub fn import_from_json(json: &str) -> Result<Self, serde_json::Error> {
        serde_json::from_str(json)
    }
}

/// Extracts trajectory data from CSV format
/// Common use case: Parsing robot motion log files from industrial systems
pub struct TrajectoryExtractor;

impl TrajectoryExtractor {
    /// Parse trajectory data from CSV string
    /// Expected format: x,y,z,timestamp (with optional header)
    pub fn extract_from_csv(csv_data: &str) -> Trajectory {
        let mut points = Vec::new();

        for (i, line) in csv_data.lines().enumerate() {
            // Skip header if present
            if i == 0 && line.contains("x") {
                continue;
            }

            let parts: Vec<&str> = line.split(',').collect();
            if parts.len() >= 4 {
                let values: Vec<f64> = parts
                    .iter()
                    .filter_map(|s| s.trim().parse().ok())
                    .collect();

                if values.len() >= 4 {
                    points.push(TrajectoryPoint::new(
                        values[0],
                        values[1],
                        values[2],
                        values[3],
                    ));
                }
            }
        }

        Trajectory::new(points)
    }
}

/// Implements Kalman filtering for trajectory smoothing
/// Reduces sensor noise commonly found in motion capture systems
pub struct KalmanSmoother {
    state: [f64; 6],      // [x, y, z, vx, vy, vz]
    process_noise: f64,
    measurement_noise: f64,
}

impl KalmanSmoother {
    /// Create a new Kalman smoother
    pub fn new(process_noise: f64, measurement_noise: f64) -> Self {
        Self {
            state: [0.0; 6],
            process_noise,
            measurement_noise,
        }
    }

    /// Apply Kalman filtering to a trajectory
    pub fn apply(&mut self, trajectory: &Trajectory) -> Trajectory {
        if trajectory.points.is_empty() {
            return Trajectory::new(Vec::new());
        }

        // Initialize with first measurement
        self.state[0] = trajectory.points[0].x;
        self.state[1] = trajectory.points[0].y;
        self.state[2] = trajectory.points[0].z;

        let kalman_gain = self.process_noise / (self.process_noise + self.measurement_noise);
        let mut smoothed = Vec::new();

        for point in &trajectory.points {
            // Prediction step (constant velocity model)
            let dt = 0.1; // Assumed constant time step
            self.state[0] += self.state[3] * dt;
            self.state[1] += self.state[4] * dt;
            self.state[2] += self.state[5] * dt;

            // Correction step
            self.state[0] += kalman_gain * (point.x - self.state[0]);
            self.state[1] += kalman_gain * (point.y - self.state[1]);
            self.state[2] += kalman_gain * (point.z - self.state[2]);

            smoothed.push(TrajectoryPoint::new(
                self.state[0],
                self.state[1],
                self.state[2],
                point.timestamp,
            ));
        }

        Trajectory::new(smoothed)
    }
}

/// Analyzes trajectory kinematics: velocity, acceleration, and jerk
/// Essential for motion control optimization and constraint validation
pub struct KinematicsAnalyzer<'a> {
    trajectory: &'a Trajectory,
}

impl<'a> KinematicsAnalyzer<'a> {
    /// Create a new kinematics analyzer
    pub fn new(trajectory: &'a Trajectory) -> Self {
        Self { trajectory }
    }

    /// Calculate velocity vectors for the trajectory
    pub fn compute_velocities(&self) -> Vec<Vector3D> {
        let mut velocities = vec![Vector3D::new(0.0, 0.0, 0.0)];

        for i in 1..self.trajectory.points.len() {
            let dt = self.trajectory.points[i].timestamp - self.trajectory.points[i - 1].timestamp;
            let vel = if dt > 0.0 {
                Vector3D::new(
                    (self.trajectory.points[i].x - self.trajectory.points[i - 1].x) / dt,
                    (self.trajectory.points[i].y - self.trajectory.points[i - 1].y) / dt,
                    (self.trajectory.points[i].z - self.trajectory.points[i - 1].z) / dt,
                )
            } else {
                Vector3D::new(0.0, 0.0, 0.0)
            };
            velocities.push(vel);
        }

        velocities
    }

    /// Calculate acceleration vectors for the trajectory
    pub fn compute_accelerations(&self) -> Vec<Vector3D> {
        let velocities = self.compute_velocities();
        let mut accelerations = vec![Vector3D::new(0.0, 0.0, 0.0); 2];

        for i in 2..self.trajectory.points.len() {
            let dt = self.trajectory.points[i].timestamp - self.trajectory.points[i - 1].timestamp;
            let acc = if dt > 0.0 {
                Vector3D::new(
                    (velocities[i].x - velocities[i - 1].x) / dt,
                    (velocities[i].y - velocities[i - 1].y) / dt,
                    (velocities[i].z - velocities[i - 1].z) / dt,
                )
            } else {
                Vector3D::new(0.0, 0.0, 0.0)
            };
            accelerations.push(acc);
        }

        accelerations
    }

    /// Calculate jerk vectors (derivative of acceleration)
    pub fn compute_jerks(&self) -> Vec<Vector3D> {
        let accelerations = self.compute_accelerations();
        let mut jerks = vec![Vector3D::new(0.0, 0.0, 0.0); 3];

        for i in 3..self.trajectory.points.len() {
            let dt = self.trajectory.points[i].timestamp - self.trajectory.points[i - 1].timestamp;
            let jerk = if dt > 0.0 {
                Vector3D::new(
                    (accelerations[i].x - accelerations[i - 1].x) / dt,
                    (accelerations[i].y - accelerations[i - 1].y) / dt,
                    (accelerations[i].z - accelerations[i - 1].z) / dt,
                )
            } else {
                Vector3D::new(0.0, 0.0, 0.0)
            };
            jerks.push(jerk);
        }

        jerks
    }

    /// Compute complete kinematics profile
    pub fn compute_full_profile(&self) -> (Vec<Vector3D>, Vec<Vector3D>, Vec<Vector3D>) {
        let velocities = self.compute_velocities();
        let accelerations = self.compute_accelerations();
        let jerks = self.compute_jerks();
        (velocities, accelerations, jerks)
    }
}

/// Optimizes trajectories for time-optimal motion under constraints
/// Common application: Industrial robotics cycle time optimization
pub struct TrajectoryOptimizer {
    max_velocity: f64,
    max_acceleration: f64,
}

impl TrajectoryOptimizer {
    /// Create a new trajectory optimizer
    pub fn new(max_velocity: f64, max_acceleration: f64) -> Self {
        Self {
            max_velocity,
            max_acceleration,
        }
    }

    /// Generate time-optimal trajectory under constraints
    pub fn optimize_time_optimal(&self, trajectory: &Trajectory) -> Trajectory {
        let analyzer = KinematicsAnalyzer::new(&trajectory);
        let (velocities, accelerations, _) = analyzer.compute_full_profile();

        let mut optimized = Vec::new();

        for (i, point) in trajectory.points.iter().enumerate() {
            let v_mag = velocities.get(i).map(|v| v.magnitude()).unwrap_or(0.0);
            let a_mag = accelerations.get(i).map(|a| a.magnitude()).unwrap_or(0.0);

            let mut scale: f64 = 1.0;
            if v_mag > self.max_velocity {
                scale = scale.min(self.max_velocity / v_mag);
            }
            if a_mag > self.max_acceleration {
                scale = scale.min(self.max_acceleration / a_mag);
            }

            optimized.push(*point);
        }

        Trajectory::new(optimized)
    }
}

/// Predicts future trajectory points using linear extrapolation
/// Can be extended with ML models for advanced AI-based prediction
pub struct TrajectoryPredictor<'a> {
    trajectory: &'a Trajectory,
}

impl<'a> TrajectoryPredictor<'a> {
    /// Create a new trajectory predictor
    pub fn new(trajectory: &'a Trajectory) -> Self {
        Self { trajectory }
    }

    /// Predict future points using linear extrapolation
    pub fn predict_linear_extrapolation(&self, lookahead_steps: usize) -> Vec<TrajectoryPoint> {
        if self.trajectory.points.len() < 3 {
            return Vec::new();
        }

        let last = self.trajectory.points.last().unwrap();
        let prev = &self.trajectory.points[self.trajectory.points.len() - 2];

        let dt = last.timestamp - prev.timestamp;
        let dt = if dt > 0.0 { dt } else { 1.0 };

        let velocity = Vector3D::new(
            (last.x - prev.x) / dt,
            (last.y - prev.y) / dt,
            (last.z - prev.z) / dt,
        );

        let mut predictions = Vec::new();
        for i in 1..=lookahead_steps {
            predictions.push(TrajectoryPoint::new(
                last.x + velocity.x * (i as f64) * dt,
                last.y + velocity.y * (i as f64) * dt,
                last.z + velocity.z * (i as f64) * dt,
                last.timestamp + (i as f64) * dt,
            ));
        }

        predictions
    }
}

fn main() {
    println!("=== MotionTrajectory - AI Motion Control Library ===");
    println!("Inspired by aimotioncontrol.net\n");

    // Create a sample trajectory
    let trajectory = Trajectory::from(vec![
        (0.0, 0.0, 0.0, 0.0),
        (1.0, 2.0, 1.0, 100.0),
        (2.0, 3.0, 2.0, 200.0),
        (3.0, 5.0, 3.0, 300.0),
        (4.0, 6.0, 4.0, 400.0),
    ]);

    println!("Original trajectory: {} points", trajectory.points.len());

    // Apply Kalman smoothing
    let mut smoother = KalmanSmoother::new(0.1, 0.01);
    let smoothed = smoother.apply(&trajectory);
    println!("Smoothed trajectory: {} points", smoothed.points.len());

    // Compute kinematics
    let analyzer = KinematicsAnalyzer::new(&trajectory);
    let (velocities, accelerations, jerks) = analyzer.compute_full_profile();
    println!("Velocity samples: {}", velocities.len());
    println!("Acceleration samples: {}", accelerations.len());
    println!("Jerk samples: {}", jerks.len());

    // Predict future trajectory
    let predictor = TrajectoryPredictor::new(&trajectory);
    let predictions = predictor.predict_linear_extrapolation(3);
    println!("\nPredictions: {} points", predictions.len());
    for (i, p) in predictions.iter().enumerate() {
        println!(
            "  Step {}: ({:.2}, {:.2}, {:.2}) at t={:.0}",
            i + 1,
            p.x,
            p.y,
            p.z,
            p.timestamp
        );
    }

    // Calculate path length
    let path_length = trajectory.calculate_path_length();
    println!("\nTotal path length: {:.2} meters", path_length);

    // Export to JSON
    let _ = trajectory.export_to_json("trajectory_output.json");
    println!("\nTrajectory exported to: trajectory_output.json");
}

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

    #[test]
    fn test_trajectory_creation() {
        let trajectory = Trajectory::from(vec![(0.0, 0.0, 0.0, 0.0), (1.0, 1.0, 1.0, 100.0)]);
        assert_eq!(trajectory.points.len(), 2);
    }

    #[test]
    fn test_kalman_smoother() {
        let trajectory = Trajectory::from(vec![
            (0.0, 0.0, 0.0, 0.0),
            (1.0, 1.0, 1.0, 100.0),
            (2.0, 2.0, 2.0, 200.0),
        ]);
        let mut smoother = KalmanSmoother::new(0.1, 0.01);
        let smoothed = smoother.apply(&trajectory);
        assert_eq!(smoothed.points.len(), 3);
    }

    #[test]
    fn test_kinematics_analyzer() {
        let trajectory = Trajectory::from(vec![
            (0.0, 0.0, 0.0, 0.0),
            (1.0, 1.0, 1.0, 100.0),
            (2.0, 2.0, 2.0, 200.0),
        ]);
        let analyzer = KinematicsAnalyzer::new(&trajectory);
        let velocities = analyzer.compute_velocities();
        assert!(velocities.len() >= 2);
    }

    #[test]
    fn test_trajectory_predictor() {
        let trajectory = Trajectory::from(vec![
            (0.0, 0.0, 0.0, 0.0),
            (1.0, 1.0, 1.0, 100.0),
            (2.0, 2.0, 2.0, 200.0),
        ]);
        let predictor = TrajectoryPredictor::new(&trajectory);
        let predictions = predictor.predict_linear_extrapolation(3);
        assert_eq!(predictions.len(), 3);
    }

    #[test]
    fn test_vector_magnitude() {
        let vector = Vector3D::new(3.0, 4.0, 0.0);
        assert_eq!(vector.magnitude(), 5.0);
    }

    #[test]
    fn test_trajectory_export_import() {
        let trajectory = Trajectory::from(vec![(0.0, 0.0, 0.0, 0.0), (1.0, 1.0, 1.0, 100.0)]);
        let json = serde_json::to_string(&trajectory).unwrap();
        let imported = Trajectory::import_from_json(&json).unwrap();
        assert_eq!(imported.points.len(), 2);
    }
}