1 files changed, 76 insertions, 28 deletions

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,