From 46a8414f21f036c406c5845ec1a3fd541207c996 Mon Sep 17 00:00:00 2001
From: Dustin Carlino <dabreegster@gmail.com>
Date: Sun, 17 Feb 2019 16:07:02 -0800
Subject: [PATCH] make quadratic dist work for DES. seems to work! but need to
 adjust ALL intervals...

---
 editor/src/plugins/sim/des_model/car.rs      | 33 ++++++++++++++++--
 editor/src/plugins/sim/des_model/interval.rs | 35 ++++----------------
 2 files changed, 37 insertions(+), 31 deletions(-)

diff --git a/editor/src/plugins/sim/des_model/car.rs b/editor/src/plugins/sim/des_model/car.rs
index 928d1b455d..83111f12a3 100644
--- a/editor/src/plugins/sim/des_model/car.rs
+++ b/editor/src/plugins/sim/des_model/car.rs
@@ -288,7 +288,7 @@ impl Car {
         self.intervals.split_off(idx1 + 1);
 
         // Option 1: Might be too sharp.
-        if true {
+        if false {
             {
                 let mut our_adjusted_last = self.intervals.pop().unwrap();
 
@@ -326,8 +326,16 @@ impl Car {
             let them = &leader.intervals[idx2];
             let mut our_adjusted_last = self.intervals.pop().unwrap();
             our_adjusted_last.end_speed = them.end_speed;
-            our_adjusted_last.end_time = them.end_time;
             our_adjusted_last.end_dist = them.end_dist - dist_behind;
+            our_adjusted_last.end_time = them.end_time;
+            /*our_adjusted_last.end_time = find_end_time_for_interval(
+                our_adjusted_last.start_dist,
+                our_adjusted_last.end_dist,
+                our_adjusted_last.start_speed,
+                our_adjusted_last.end_speed,
+                our_adjusted_last.start_time,
+            );*/
+            println!("adjusted interval: {}", our_adjusted_last);
             self.intervals.push(our_adjusted_last);
         }
 
@@ -344,3 +352,24 @@ impl Car {
         }
     }
 }
+
+fn find_end_time_for_interval(
+    initial_dist: Distance,
+    final_dist: Distance,
+    initial_speed: Speed,
+    final_speed: Speed,
+    initial_time: Duration,
+) -> Duration {
+    let g = final_dist.inner_meters();
+    let d = initial_dist.inner_meters();
+    let v = initial_speed.inner_meters_per_second();
+    let f = final_speed.inner_meters_per_second();
+    let s = initial_time.inner_seconds();
+
+    let numer = -2.0 * d + s * (f + v) + 2.0 * g;
+    let denom = f + v;
+    let t = Duration::seconds(numer / denom);
+
+    assert!(t > initial_time);
+    t
+}
diff --git a/editor/src/plugins/sim/des_model/interval.rs b/editor/src/plugins/sim/des_model/interval.rs
index 402e8d6274..8a54033d3c 100644
--- a/editor/src/plugins/sim/des_model/interval.rs
+++ b/editor/src/plugins/sim/des_model/interval.rs
@@ -57,17 +57,12 @@ impl Interval {
     }
 
     pub fn dist(&self, t: Duration) -> Distance {
-        if true {
-            // Linearly interpolate
-            self.start_dist + self.percent(t) * (self.end_dist - self.start_dist)
-        } else {
-            let relative_t = (t - self.start_time).inner_seconds();
+        let relative_t = (t - self.start_time).inner_seconds();
 
-            let d = self.start_dist.inner_meters()
-                + self.start_speed.inner_meters_per_second() * relative_t
-                + 0.5 * self.raw_accel() * relative_t.powi(2);
-            Distance::meters(d)
-        }
+        let d = self.start_dist.inner_meters()
+            + self.start_speed.inner_meters_per_second() * relative_t
+            + 0.5 * self.raw_accel() * relative_t.powi(2);
+        Distance::meters(d)
     }
 
     pub fn speed(&self, t: Duration) -> Speed {
@@ -107,25 +102,7 @@ impl Interval {
         }
 
         // Set the two distance equations equal and solve for time. Long to type out here...
-        let t = if true {
-            let x1 = self.start_dist.inner_meters();
-            let x2 = self.end_dist.inner_meters();
-            let a1 = self.start_time.inner_seconds();
-            let a2 = self.end_time.inner_seconds();
-
-            let y1 = leader.start_dist.inner_meters();
-            let y2 = leader.end_dist.inner_meters();
-            let b1 = leader.start_time.inner_seconds();
-            let b2 = leader.end_time.inner_seconds();
-
-            let numer =
-                a1 * (b2 * (y1 - x2) + b1 * (x2 - y2)) + a2 * (b2 * (x1 - y1) + b1 * (y2 - x1));
-            let denom = (a1 - a2) * (y1 - y2) + b2 * (x1 - x2) + b1 * (x2 - x1);
-            if denom == 0.0 {
-                return None;
-            }
-            Duration::seconds(numer / denom)
-        } else {
+        let t = {
             let x_1 = self.start_dist.inner_meters();
             let v_1 = self.start_speed.inner_meters_per_second();
             let t_1 = self.start_time.inner_seconds();