fleshing out tractorbeam

7 files changed, 293 insertions, 52 deletions

diff --git a/README.md b/README.md
index a78b7f7..be6675f 100644
--- a/README.md
+++ b/README.md
@@ -10,6 +10,7 @@ modules
 - bookmarking objects (e.g. stations, estimates position & velocity outside navigation)
 - hacking modules to disable other modules
 - some sort of armor / shield module
+- tractorbeam has range of effectiveness
 
 world
 - tiers of minerals, modules
diff --git a/src/client/tractorbeam.rs b/src/client/tractorbeam.rs
index 74d912f..c8a6d92 100644
--- a/src/client/tractorbeam.rs
+++ b/src/client/tractorbeam.rs
@@ -27,25 +27,25 @@ pub fn client_tractorbeam(mut stream: TcpStream, mut buff_r: BufReader<TcpStream
             match data.status {
                 tractorbeam::Status::None => write!(
                     stdout,
-                    "{}Press o to pull, p to push, t to bring to 5m.",
+                    "{}Press o to pull, p to push, b to bring to 5m.",
                     clear
                 )
                 .unwrap(),
                 tractorbeam::Status::Push => write!(
                     stdout,
-                    "{}Press o to pull, p to stop pushing, t to bring to 5m.",
+                    "{}Press o to pull, p to stop pushing, b to bring to 5m.",
                     clear
                 )
                 .unwrap(),
                 tractorbeam::Status::Pull => write!(
                     stdout,
-                    "{}Press o to stop pulling, p to push, t to bring to 5m.",
+                    "{}Press o to stop pulling, p to push, b to bring to 5m.",
                     clear
                 )
                 .unwrap(),
                 tractorbeam::Status::Bring => write!(
                     stdout,
-                    "{}Press o to pulling, p to push, t to stop bringing to 5m.",
+                    "{}Press o to pulling, p to push, b to stop bringing to 5m.",
                     clear
                 )
                 .unwrap(),
diff --git a/src/constants.rs b/src/constants.rs
index a2373ff..4c48cac 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -14,6 +14,8 @@ pub const SHIP_NAVIGATION_TIME: u64 = 3;
 pub const SHIP_NAVIGATION_RANGE: f64 = 100.0;
 pub const SHIP_REFINERY_TIME: u64 = 5;
 pub const SHIP_TRACTORBEAM_STRENGTH: f64 = 0.1;
+pub const SHIP_TRACTORBEAM_RANGE: f64 = 50.0;
+pub const SHIP_TRACTORBEAM_BRING_TO_DISTANCE: f64 = 5.0;
 pub const SHIP_ENGINES_FUEL_START: f64 = 100.0;
 pub const SHIP_ENGINES_ACCELERATION: f64 = 0.1;
 
@@ -22,3 +24,4 @@ pub const HYDROGEN_SIZE: usize = 1;
 pub const CRUDE_MINERALS_SIZE: usize = 10;
 
 pub const SLEEP_DURATION: u64 = 100;
+pub const FLOAT_PRECISION: f64 = 0.001;
diff --git a/src/mass.rs b/src/mass.rs
index 3d90573..a307295 100644
--- a/src/mass.rs
+++ b/src/mass.rs
@@ -98,16 +98,16 @@ impl Mass {
 
         Mass {
             mass_type: astroid,
-            position: Vector::new((
+            position: Vector::new(
                 rng.sample(p_range),
                 rng.sample(p_range),
                 rng.sample(p_range),
-            )),
-            velocity: Vector::new((
+            ),
+            velocity: Vector::new(
                 rng.sample(v_range),
                 rng.sample(v_range),
                 rng.sample(v_range),
-            )),
+            ),
             effects: Effects::new(),
         }
     }
@@ -179,10 +179,7 @@ impl Mass {
             if let Some(target_name) = &navigation.target_name {
                 let mut target = masses.remove(target_name).unwrap();
                 mining.process(self.position.clone(), masses, &mut target, storage);
-                tractorbeam.process();
-                let acceleration =
-                    tractorbeam.get_acceleration(self.position.clone(), target.position.clone());
-                target.effects.give_acceleration(acceleration);
+                tractorbeam.process(self.position.clone(), &mut target);
                 masses.insert(target_name.to_string(), target);
             }
 
@@ -193,13 +190,13 @@ impl Mass {
 
             engines.process(self.position.clone(), self.velocity.clone(), target);
             refinery.process(storage);
-            navigation.process(self.position.clone(), masses);
             construction.process(
                 self.velocity.clone(),
                 self.position.clone(),
                 masses,
                 storage,
             );
+            navigation.process(self.position.clone(), masses);
             self.effects.give_acceleration(engines.take_acceleration());
         }
 
diff --git a/src/math.rs b/src/math.rs
index 9a9f7b8..e266a88 100644
--- a/src/math.rs
+++ b/src/math.rs
@@ -19,12 +19,8 @@ pub struct Vector {
 }
 
 impl Vector {
-    pub fn new(v: (f64, f64, f64)) -> Vector {
-        Vector {
-            x: v.0,
-            y: v.1,
-            z: v.2,
-        }
+    pub fn new(x: f64, y: f64, z: f64) -> Vector {
+        Vector { x, y, z }
     }
 
     pub fn distance_from(&self, other: Vector) -> f64 {
diff --git a/src/modules/tractorbeam.rs b/src/modules/tractorbeam.rs
index 4f91292..28aa181 100644
--- a/src/modules/tractorbeam.rs
+++ b/src/modules/tractorbeam.rs
@@ -4,21 +4,89 @@ use crate::math::Vector;
 
 #[derive(Serialize, Deserialize, Debug, Clone, Default)]
 pub struct Tractorbeam {
+    pub range: f64,
     pub status: Status,
     strength: f64,
     desired_distance: Option<f64>,
+    control_system: ControlSystem,
+}
+
+#[derive(Serialize, Deserialize, Debug, Clone, Default)]
+struct ControlSystem {
+    previous_error: f64,
+    integral: f64,
+    kp: f64,
+    ki: f64,
+    kd: f64,
+    dt: f64,
+}
+
+impl ControlSystem {
+    pub fn new() -> ControlSystem {
+        ControlSystem {
+            previous_error: 0.0,
+            integral: 0.0,
+            kp: 1.0,
+            ki: 0.01,
+            kd: 0.001,
+            dt: 0.0001,
+        }
+    }
+
+    pub fn compute(&mut self, strength: f64, distance: f64, desired_distance: f64) -> f64 {
+        let error = desired_distance - distance;
+        self.integral += error * self.dt;
+        let derivative = (error - self.previous_error) / self.dt;
+        let output = self.kp * error + self.ki * self.integral + self.kd * derivative;
+        self.previous_error = error;
+
+        if output.abs() > strength {
+            if output.is_sign_positive() {
+                strength
+            } else {
+                strength * -1.0
+            }
+        } else {
+            output
+        }
+    }
 }
 
 impl Tractorbeam {
     pub fn new() -> Tractorbeam {
         Tractorbeam {
+            range: constants::SHIP_TRACTORBEAM_RANGE,
             status: Status::None,
             strength: constants::SHIP_TRACTORBEAM_STRENGTH,
             desired_distance: None,
+            control_system: ControlSystem::new(),
         }
     }
 
-    pub fn process(&mut self) {}
+    pub fn process(&mut self, ship_position: Vector, target: &mut Mass) {
+        let distance = ship_position.distance_from(target.position.clone());
+        if self.range < distance {
+            self.off()
+        } else {
+            let direction = target.position.clone() - ship_position.clone();
+            let acceleration = match self.status {
+                Status::Push => direction.unitize() * self.strength,
+                Status::Pull => direction.unitize() * -1.0 * self.strength,
+                Status::Bring => match self.desired_distance {
+                    Some(desired_distance) => {
+                        direction.unitize()
+                            * self
+                                .control_system
+                                .compute(self.strength, distance, desired_distance)
+                    }
+                    None => Vector::default(),
+                },
+                Status::None => Vector::default(),
+            };
+
+            target.effects.give_acceleration(acceleration);
+        }
+    }
 
     pub fn get_client_data(&self, target: Option<&Mass>) -> String {
         let client_data = ClientData {
@@ -33,36 +101,11 @@ impl Tractorbeam {
         match recv.as_str() {
             "o" => self.toggle_pull(),
             "p" => self.toggle_push(),
-            "b" => self.toggle_bring(5.0),
+            "b" => self.toggle_bring(constants::SHIP_TRACTORBEAM_BRING_TO_DISTANCE),
             _ => (),
         }
     }
 
-    pub fn get_acceleration(&self, ship_position: Vector, target_position: Vector) -> Vector {
-        let acceleration = ship_position.clone() - target_position.clone();
-        match self.status {
-            Status::Push => acceleration.unitize() * -0.05,
-            Status::Pull => acceleration.unitize() * 0.05,
-            Status::Bring => match self.desired_distance {
-                Some(desired_distance) => {
-                    if desired_distance > ship_position.distance_from(target_position) {
-                        acceleration.unitize() * -0.05
-                    //some sort of velocity limiter
-                    //if target.speed_torwards(ship) < 10.0 {
-                    //    acceleration.unitize() * -0.05
-                    //} else {
-                    //    Vector::default()
-                    //}
-                    } else {
-                        acceleration.unitize() * 0.05
-                    }
-                }
-                None => Vector::default(),
-            },
-            Status::None => Vector::default(),
-        }
-    }
-
     fn toggle_pull(&mut self) {
         self.status = match self.status {
             Status::None => Status::Pull,
@@ -83,6 +126,7 @@ impl Tractorbeam {
 
     fn toggle_bring(&mut self, desired_distance: f64) {
         self.desired_distance = Some(desired_distance);
+        self.control_system = ControlSystem::new();
         self.status = match self.status {
             Status::None => Status::Bring,
             Status::Pull => Status::Bring,
@@ -90,6 +134,10 @@ impl Tractorbeam {
             Status::Bring => Status::None,
         }
     }
+
+    fn off(&mut self) {
+        self.status = Status::None
+    }
 }
 
 #[derive(Serialize, Deserialize, Debug, Clone)]
@@ -98,7 +146,7 @@ pub struct ClientData {
     pub status: Status,
 }
 
-#[derive(Serialize, Deserialize, Debug, Clone)]
+#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)]
 pub enum Status {
     None,
     Push,
diff --git a/tests/tests.rs b/tests/tests.rs
index ddbbaf7..574e9f6 100644
--- a/tests/tests.rs
+++ b/tests/tests.rs
@@ -1,7 +1,7 @@
 extern crate space;
 
 #[cfg(test)]
-mod test {
+mod tests {
     use std::collections::HashMap;
     use std::thread::sleep;
     use std::time::Duration;
@@ -14,6 +14,7 @@ mod test {
     use space::modules::mining;
     use space::modules::navigation;
     use space::modules::refinery;
+    use space::modules::tractorbeam;
     use space::modules::types::ModuleType;
 
     fn setup() -> (Mass, HashMap<String, Mass>) {
@@ -45,7 +46,7 @@ mod test {
         assert!(navigation_data.status == navigation::Status::Targeting);
 
         let astroid = masses.get_mut("astroid").unwrap();
-        astroid.position = Vector::new((constants::SHIP_NAVIGATION_RANGE + 1.0, 0.0, 0.0));
+        astroid.position = Vector::new(constants::SHIP_NAVIGATION_RANGE + 1.0, 0.0, 0.0);
         ship.process(&mut masses);
         let data = ship.get_client_data(ModuleType::Navigation, &masses);
         let navigation_data: navigation::ClientData = serde_json::from_str(&data).unwrap();
@@ -62,7 +63,7 @@ mod test {
         assert!(navigation_data.status == navigation::Status::Targeted);
 
         let astroid = masses.get_mut("astroid").unwrap();
-        astroid.position = Vector::new((constants::SHIP_NAVIGATION_RANGE + 1.0, 0.0, 0.0));
+        astroid.position = Vector::new(constants::SHIP_NAVIGATION_RANGE + 1.0, 0.0, 0.0);
         ship.process(&mut masses);
         let data = ship.get_client_data(ModuleType::Navigation, &masses);
         let navigation_data: navigation::ClientData = serde_json::from_str(&data).unwrap();
@@ -81,7 +82,26 @@ mod test {
         assert!(mining_data.status == mining::Status::Mining);
 
         let mut astroid = masses.get_mut("astroid").unwrap();
-        astroid.position = Vector::new((constants::SHIP_MINING_RANGE + 1.0, 0.0, 0.0));
+        astroid.position = Vector::new(constants::SHIP_MINING_RANGE + 1.0, 0.0, 0.0);
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Mining, &masses);
+        let mining_data: mining::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(mining_data.status == mining::Status::None);
+    }
+
+    #[test]
+    fn test_mining_navigation_range() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        ship.give_received_data(ModuleType::Mining, String::from("F"));
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Mining, &masses);
+        let mining_data: mining::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(mining_data.status == mining::Status::Mining);
+
+        let mut astroid = masses.get_mut("astroid").unwrap();
+        astroid.position = Vector::new(constants::SHIP_NAVIGATION_RANGE + 1.0, 0.0, 0.0);
         ship.process(&mut masses);
         let data = ship.get_client_data(ModuleType::Mining, &masses);
         let mining_data: mining::ClientData = serde_json::from_str(&data).unwrap();
@@ -180,7 +200,7 @@ mod test {
         setup_ship_target(&mut ship, &mut masses);
 
         let mut astroid = masses.remove("astroid").unwrap();
-        astroid.velocity = Vector::new((constants::SHIP_ENGINES_ACCELERATION * 2.0, 0.0, 0.0));
+        astroid.velocity = Vector::new(constants::SHIP_ENGINES_ACCELERATION * 2.0, 0.0, 0.0);
         astroid.process(&mut masses);
         masses.insert(String::from("astroid"), astroid);
 
@@ -215,4 +235,180 @@ mod test {
         ship.process(&mut masses);
         assert!(ship.velocity.x == constants::SHIP_ENGINES_ACCELERATION * -2.0);
     }
+
+    #[test]
+    fn test_tractorbeam_push_range() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(1.0, 0.0, 0.0);
+        masses.insert(String::from("astroid"), astroid);
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("p"));
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::Push);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(constants::SHIP_TRACTORBEAM_RANGE + 1.0, 0.0, 0.0);
+        astroid.process(&mut masses);
+        masses.insert(String::from("astroid"), astroid);
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::None);
+    }
+
+    #[test]
+    fn test_tractorbeam_pull_range() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(1.0, 0.0, 0.0);
+        masses.insert(String::from("astroid"), astroid);
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("o"));
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::Pull);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(constants::SHIP_TRACTORBEAM_RANGE + 1.0, 0.0, 0.0);
+        astroid.process(&mut masses);
+        masses.insert(String::from("astroid"), astroid);
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::None);
+    }
+
+    #[test]
+    fn test_tractorbeam_bring_range() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(1.0, 0.0, 0.0);
+        masses.insert(String::from("astroid"), astroid);
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("b"));
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::Bring);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        astroid.position = Vector::new(constants::SHIP_TRACTORBEAM_RANGE + 1.0, 0.0, 0.0);
+        astroid.process(&mut masses);
+        masses.insert(String::from("astroid"), astroid);
+        ship.process(&mut masses);
+        let data = ship.get_client_data(ModuleType::Tractorbeam, &masses);
+        let tractorbeam_data: tractorbeam::ClientData = serde_json::from_str(&data).unwrap();
+        assert!(tractorbeam_data.status == tractorbeam::Status::None);
+    }
+
+    #[test]
+    fn test_tractorbeam() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        let start = 2.0;
+        astroid.velocity = Vector::new(start, 0.0, 0.0);
+        astroid.process(&mut masses);
+        assert!(astroid.position == Vector::new(start, 0.0, 0.0));
+        masses.insert(String::from("astroid"), astroid);
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("o"));
+        let mut iterated = 1.0;
+        loop {
+            ship.process(&mut masses);
+
+            let mut astroid = masses.remove("astroid").unwrap();
+            astroid.process(&mut masses);
+
+            let estimated_velocity = start - (constants::SHIP_TRACTORBEAM_STRENGTH * iterated);
+            assert!(
+                astroid.velocity.x.abs() - estimated_velocity.abs() < constants::FLOAT_PRECISION
+            );
+            masses.insert(String::from("astroid"), astroid);
+
+            iterated += 1.0;
+            if iterated > 10.0 {
+                break;
+            }
+        }
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("p"));
+        let mut iterated = 1.0;
+        loop {
+            ship.process(&mut masses);
+
+            let mut astroid = masses.remove("astroid").unwrap();
+            astroid.process(&mut masses);
+
+            let estimated_velocity = start + (constants::SHIP_TRACTORBEAM_STRENGTH * iterated);
+            assert!(
+                astroid.velocity.x.abs() - estimated_velocity.abs() < constants::FLOAT_PRECISION
+            );
+            masses.insert(String::from("astroid"), astroid);
+
+            iterated += 1.0;
+            if iterated > 10.0 {
+                break;
+            }
+        }
+    }
+
+    #[test]
+    fn test_tractorbeam_bring() {
+        let (mut ship, mut masses) = setup();
+        setup_ship_target(&mut ship, &mut masses);
+
+        let mut astroid = masses.remove("astroid").unwrap();
+        let start = 25.0;
+        astroid.position = Vector::new(start, 0.0, 0.0);
+        astroid.process(&mut masses);
+        masses.insert(String::from("astroid"), astroid);
+
+        ship.give_received_data(ModuleType::Tractorbeam, String::from("b"));
+        let mut iterated = 1.0;
+        loop {
+            ship.process(&mut masses);
+
+            let mut astroid = masses.remove("astroid").unwrap();
+            astroid.process(&mut masses);
+
+            if ship.position.distance_from(astroid.position.clone())
+                < constants::SHIP_TRACTORBEAM_BRING_TO_DISTANCE
+                && astroid.velocity.magnitude() < 1.0
+            {
+                break;
+            }
+
+            masses.insert(String::from("astroid"), astroid);
+
+            iterated += 1.0;
+            if iterated > 100.0 {
+                assert!(false);
+                break;
+            }
+        }
+
+        /* for plotting
+        //let mut xy = Vec::new();
+        //xy.push((iterated, astroid.position.x));
+        let l =
+            plotlib::line::Line::new(&xy[..]).style(plotlib::style::LineStyle::new().colour("red"));
+        let v = plotlib::view::ContinuousView::new().add(&l);
+        plotlib::page::Page::single(&v)
+            .save("line.svg")
+            .expect("error");
+        std::process::Command::new("feh").arg("--conversion-timeout").arg("1").arg("line.svg").output().expect("problem");
+        */
+    }
 }