From 43d84dc29eed42f3eae9016aaa586c1cca503005 Mon Sep 17 00:00:00 2001
From: Mikolaj Wielgus <wielgusmikolaj@gmail.com>
Date: Wed, 26 Jun 2024 00:47:16 +0200
Subject: [PATCH] router: restore calculation of probe length (was temp.
 hardcoded to 0.0)

---
 src/drawing/guide.rs | 18 +++++++++---------
 src/router/router.rs | 42 +++++++++++++++++++++++++++++++++++++-----
 2 files changed, 46 insertions(+), 14 deletions(-)

diff --git a/src/drawing/guide.rs b/src/drawing/guide.rs
index 9780260..80c1776 100644
--- a/src/drawing/guide.rs
+++ b/src/drawing/guide.rs
@@ -220,21 +220,21 @@ impl<'a, CW: Copy, R: RulesTrait> Guide<'a, CW, R> {
         }
     }
 
-    pub fn cane_head(&self, dot: LooseDotIndex) -> CaneHead {
+    pub fn cane_head(&self, face: LooseDotIndex) -> CaneHead {
         CaneHead {
-            face: dot,
-            cane: self.drawing.cane(dot),
+            face,
+            cane: self.drawing.cane(face),
         }
     }
 
-    pub fn rear_head(&self, dot: LooseDotIndex) -> Head {
-        self.head(self.rear(self.cane_head(dot)))
+    pub fn rear_head(&self, face: LooseDotIndex) -> Head {
+        self.head(self.rear(self.cane_head(face)))
     }
 
-    pub fn head(&self, dot: DotIndex) -> Head {
-        match dot {
-            DotIndex::Fixed(fixed) => BareHead { dot: fixed }.into(),
-            DotIndex::Loose(loose) => self.cane_head(loose).into(),
+    pub fn head(&self, face: DotIndex) -> Head {
+        match face {
+            DotIndex::Fixed(dot) => BareHead { dot }.into(),
+            DotIndex::Loose(dot) => self.cane_head(dot).into(),
         }
     }
 
diff --git a/src/router/router.rs b/src/router/router.rs
index 0cf3fd7..7912281 100644
--- a/src/router/router.rs
+++ b/src/router/router.rs
@@ -8,12 +8,13 @@ use thiserror::Error;
 use crate::{
     drawing::{
         band::BandFirstSegIndex,
-        dot::FixedDotIndex,
+        dot::{DotIndex, FixedDotIndex},
         graph::{MakePrimitive, PrimitiveIndex},
+        guide::{CaneHead, Head, HeadTrait},
         primitive::MakePrimitiveShape,
         rules::RulesTrait,
     },
-    geometry::shape::ShapeTrait,
+    geometry::{primitive::PrimitiveShapeTrait, shape::ShapeTrait},
     graph::GetPetgraphIndex,
     layout::Layout,
     router::{
@@ -50,6 +51,37 @@ impl<'a, R: RulesTrait> RouterAstarStrategy<'a, R> {
             target,
         }
     }
+
+    fn bihead_length(&self) -> f64 {
+        self.head_length(&self.trace.head)
+            + match self.trace.head.face() {
+                DotIndex::Fixed(..) => 0.0,
+                DotIndex::Loose(face) => {
+                    self.head_length(&self.tracer.layout.drawing().guide().rear_head(face))
+                }
+            }
+    }
+
+    fn head_length(&self, head: &Head) -> f64 {
+        match head {
+            Head::Bare(..) => 0.0,
+            Head::Cane(cane_head) => {
+                self.tracer
+                    .layout
+                    .drawing()
+                    .primitive(cane_head.cane.seg)
+                    .shape()
+                    .length()
+                    + self
+                        .tracer
+                        .layout
+                        .drawing()
+                        .primitive(cane_head.cane.bend)
+                        .shape()
+                        .length()
+            }
+        }
+    }
 }
 
 impl<'a, R: RulesTrait> AstarStrategy<&UnGraph<NavvertexWeight, (), usize>, f64, BandFirstSegIndex>
@@ -85,18 +117,18 @@ impl<'a, R: RulesTrait> AstarStrategy<&UnGraph<NavvertexWeight, (), usize>, f64,
             return None;
         }
 
-        let before_probe_length = 0.0; //self.tracer.layout.band_length(self.trace.head.face());
+        let prev_bihead_length = self.bihead_length();
 
         let width = self.trace.width;
         let result = self
             .tracer
             .step(*graph, &mut self.trace, edge.target(), width);
 
-        let probe_length = 0.0; //self.tracer.layout.band_length(self.trace.head.face());
+        let probe_length = self.bihead_length() - prev_bihead_length;
 
         if result.is_ok() {
             self.tracer.undo_step(*graph, &mut self.trace);
-            Some(probe_length - before_probe_length)
+            Some(probe_length)
         } else {
             None
         }