From 086aada29525258f7faadebc8818a5db5d6e5a60 Mon Sep 17 00:00:00 2001
From: Tobias Eidelpes <tobias@eidelpes.info>
Date: Sun, 13 Jun 2021 23:00:58 +0200
Subject: [PATCH] Set parameters required by assignment

---
 components/i_feed/devices/vehicle.py     |  2 +-
 components/orchestration/orchestrator.py | 56 +++++++++++++++---------
 2 files changed, 37 insertions(+), 21 deletions(-)

diff --git a/components/i_feed/devices/vehicle.py b/components/i_feed/devices/vehicle.py
index a0c5bd5..65f0e85 100644
--- a/components/i_feed/devices/vehicle.py
+++ b/components/i_feed/devices/vehicle.py
@@ -27,7 +27,7 @@ UPDATE_INTERVAL = 1
 # At x km the NCE shall happen
 NCE_KM = 2.4
 # Time in seconds to recover from NCE (will be scaled)
-TIME_TO_RECOVER = 5
+TIME_TO_RECOVER = 15
 # Resets vehicle at km x
 RESET_KM = 4
 
diff --git a/components/orchestration/orchestrator.py b/components/orchestration/orchestrator.py
index 1937438..5277a01 100644
--- a/components/orchestration/orchestrator.py
+++ b/components/orchestration/orchestrator.py
@@ -3,6 +3,7 @@ import pickle
 import sys
 from typing import List, Dict
 from datetime import datetime, timedelta
+from math import floor, ceil
 
 import requests
 from dse_shared_libs import daf, traffic_light_state, traffic_light_color, target_velocity
@@ -99,34 +100,49 @@ class Orchestrator:
                                      params={'lat': loc.latitude, 'lon': loc.longitude})
         traffic_lights_geo = response.json()
         current_vel = received_daf_object.velocity
-        target_vel = 130
+        target_vel = 130 * SCALING
         print('Nearest traffic lights: {}'.format(traffic_lights_geo['cursor']))
         for traffic_light in traffic_lights_geo['cursor']:
-            # Should only ever contain one traffic light
+            # Should only ever contain one traffic light (the next in line)
             tl_id = traffic_light['id']
             distance = traffic_light['calculatedRange']
-            next_switch_time = self.tls[tl_id]['last_switch'] + timedelta(seconds=self.tls[tl_id]['switching_time'])
-            time_until_switch = next_switch_time - datetime.now()
+            switching_time = self.tls[tl_id]['switching_time'] / float(SCALING)
+            # Time until next switch must be scaled accordingly
+            next_switch_time = self.tls[tl_id]['last_switch'] + timedelta(seconds=switching_time)
+            time_until_switch = (next_switch_time - datetime.now()).total_seconds()
             print('Distance to TL: {}'.format(distance))
-            print('Time until switch in seconds: {}'.format(time_until_switch.total_seconds()))
+            print('Time until switch in seconds: {}'.format(time_until_switch))
             if self.tls[tl_id]['color'] is TrafficLightColor.RED:
-                # Ceil because we want to get there AFTER the light changed
-                speed_needed = (distance / float(time_until_switch.total_seconds())) * 3.6
-                if current_vel > speed_needed:
-                    print('Current velocity too high ({}), adjusting to {}'.format(current_vel, speed_needed))
-                    target_vel = speed_needed
+                speed_needed_max = (distance / float(time_until_switch)) * 3.6
+                if speed_needed_max < 130 * SCALING:
+                    if current_vel < speed_needed_max:
+                        target_vel = current_vel
+                    else:
+                        target_vel = floor(speed_needed_max)
                 else:
-                    print('Current velocity lower ({})'.format(current_vel))
-                    target_vel = current_vel
+                    # Cannot make it on next green
+                    i = 2
+                    while speed_needed_max > 130 * SCALING:
+                        next_green_phase_start = time_until_switch + switching_time * i
+                        speed_needed_max = (distance / float(next_green_phase_start)) * 3.6
+                        i = i + 2
+                    target_vel = floor(speed_needed_max)
             else:
-                # Check if we can cross in time with max speed (130)
-                speed_needed = (distance / float(time_until_switch.total_seconds())) * 3.6
-                if speed_needed >= 130:
-                    # Cannot make it in time, wait for next green
-                    print('Wait on green -> red -> green switch')
-                    time_until_green = time_until_switch + timedelta(seconds=self.tls[tl_id]['switching_time'])
-                    target_vel = (distance / float(time_until_green.total_seconds())) * 3.6
-                    print('Speed lower than 130: {}'.format(target_vel))
+                # Check if we can reach TL in time
+                speed_needed_min = (distance / float(time_until_switch)) * 3.6
+                if speed_needed_min < 130 * SCALING:
+                    if current_vel < speed_needed_min:
+                        target_vel = 130 * SCALING
+                    else:
+                        target_vel = current_vel
+                else:
+                    i = 1
+                    speed_needed_max = 132 * SCALING
+                    while speed_needed_max > 130 * SCALING:
+                        next_green_phase_start = time_until_switch + switching_time * i
+                        speed_needed_max = (distance / float(next_green_phase_start)) * 3.6
+                        i = i + 2
+                    target_vel = floor(speed_needed_max)
 
         response_channel = self._velocity_mbs[received_daf_object.vehicle_identification_number]
         print('Target velocity: {}'.format(target_vel))