From af62ef96e78b3b87842982a7c54b977d13aeffa2 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Mon, 16 Oct 2023 15:20:59 -0400
Subject: [PATCH 01/21] ACU: sun avoidance

---
 socs/agents/acu/agent.py     | 139 +++++++++-
 socs/agents/acu/avoidance.py | 500 +++++++++++++++++++++++++++++++++++
 2 files changed, 634 insertions(+), 5 deletions(-)
 create mode 100644 socs/agents/acu/avoidance.py

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index e3fab147c..b794cf992 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -13,9 +13,10 @@
 from ocs import ocs_agent, site_config
 from ocs.ocs_twisted import TimeoutLock
 from soaculib.twisted_backend import TwistedHttpBackend
-from twisted.internet import protocol, reactor
+from twisted.internet import protocol, reactor, threads
 from twisted.internet.defer import DeferredList, inlineCallbacks
 
+from socs.agents.acu import avoidance
 from socs.agents.acu import drivers as sh
 from socs.agents.acu import exercisor
 
@@ -62,11 +63,13 @@ class ACUAgent:
             list should be drawn from "az", "el", and "third".
         disable_idle_reset (bool):
             If True, don't auto-start idle_reset process for LAT.
+        solar_avoidance (float):
 
     """
 
     def __init__(self, agent, acu_config='guess', exercise_plan=None,
-                 startup=False, ignore_axes=None, disable_idle_reset=False):
+                 startup=False, ignore_axes=None, disable_idle_reset=False,
+                 solar_avoidance=None):
         # Separate locks for exclusive access to az/el, and boresight motions.
         self.azel_lock = TimeoutLock()
         self.boresight_lock = TimeoutLock()
@@ -102,6 +105,16 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         if len(self.ignore_axes):
             agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes)
 
+        self.solar_params = {
+            'active_avoidance': False,
+            'radius': 0,
+        }
+        if solar_avoidance is not None and solar_avoidance > 0:
+            self.solar_params.update({
+                'active_avoidance': True,
+                'radius': solar_avoidance,
+            })
+
         self.exercise_plan = exercise_plan
 
         self.log = agent.log
@@ -148,6 +161,11 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
                                self._simple_process_stop,
                                blocking=False,
                                startup=startup)
+        agent.register_process('solar_avoidance',
+                               self.solar_avoidance,
+                               self._simple_process_stop,
+                               blocking=False,
+                               startup=startup)
         agent.register_process('generate_scan',
                                self.generate_scan,
                                self._simple_process_stop,
@@ -216,6 +234,9 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         agent.register_task('clear_faults',
                             self.clear_faults,
                             blocking=False)
+        agent.register_task('update_solar',
+                            self.update_solar,
+                            blocking=False)
 
         # Automatic exercise program...
         if exercise_plan:
@@ -1169,10 +1190,22 @@ def go_to(self, session, params):
                         f'{axis}={target} not in accepted range, '
                         f'[{limits[0]}, {limits[1]}].')
 
-            self.log.info(f'Commanded position: az={target_az}, el={target_el}')
+            self.log.info(f'Requested position: az={target_az}, el={target_el}')
             session.set_status('running')
 
-            all_ok, msg = yield self._go_to_axes(session, az=target_az, el=target_el)
+            legs, msg = yield self._get_sunsafe_moves(target_az, target_el)
+            if msg is not None:
+                self.log.error(msg)
+                return False, msg
+
+            if len(legs) > 1:
+                self.log.info(f'Executing move via {len(legs)} separate legs (sun optimized)')
+
+            for leg_az, leg_el in legs:
+                all_ok, msg = yield self._go_to_axes(session, az=leg_az, el=leg_el)
+                if not all_ok:
+                    break
+
             if all_ok and params['end_stop']:
                 yield self._set_modes(az='Stop', el='Stop')
 
@@ -1717,6 +1750,98 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
             return False, 'Problems during scan'
         return True, 'Scan ended cleanly'
 
+    #
+    # Sun Safety and Solar Avoidance
+    #
+
+    @inlineCallbacks
+    def solar_avoidance(self, session, params):
+        """solar_avoidance()
+
+        **Process** - Avoid the Sun.
+
+        """
+        # The main jobs of this process are:
+        # - track Sun and report its position
+        # - maintain a Sun Safety map for other ops to query.
+        # - guide the platform to Sun Safe position, if needed.
+
+        recomp = False
+
+        def _notify_recomputed(was_recomp, start_time):
+            nonlocal recomp
+            recomp = False
+            if was_recomp:
+                self.log.info('Recomputed Sun Safety Map (took %.1fs)' %
+                              (time.time() - start_time))
+
+        self.sun = avoidance.SunTracker()
+        session.data = {}
+        session.set_status('running')
+
+        while session.status in ['starting', 'running']:
+            try:
+                az, el = [self.data['status']['summary'][f'{ax}_current_position']
+                          for ax in ['Azimuth', 'Elevation']]
+                if az is None or el is None:
+                    raise KeyError
+            except KeyError:
+                session.data = {}
+                yield dsleep(1)
+                continue
+
+            info = self.sun.get_sun_pos(az, el)
+            session.data.update(info)
+
+            if not recomp:
+                recomp = True
+                threads.deferToThread(self.sun.reset, staleness=12 * 3600).addCallback(
+                    _notify_recomputed, time.time())
+
+            if self.sun.base_time is not None:
+                t = self.sun.check_trajectory([az], [el])['sun_time']
+                session.data['sun_safe_time'] = t if t > 0 else 0
+            yield dsleep(10)
+
+    @inlineCallbacks
+    def update_solar(self, session, params):
+        """update_solar()
+
+        **Task** - Update solar avoidance parameters.
+        """
+        pass
+
+    def _get_sun_policy(self, key):
+        return True
+
+    def _get_sunsafe_moves(self, target_az, target_el):
+        if not self._get_sun_policy('sunsafe_moves'):
+            return [(target_az, target_el)], None
+
+        if self.sun is None or self.sun.base_time is None:
+            return None, 'Sun Safety Map not computed; run the solar_avoidance process.'
+        # check for staleness!
+
+        # Check the target position and block it outright.
+        if self.sun.check_trajectory([target_az], [target_el])['sun_time'] <= 0:
+            return None, 'Requested target position is not Sun-Safe.'
+
+        # Ok, so where are we now ...
+        try:
+            az, el = [self.data['status']['summary'][f'{ax}_current_position']
+                      for ax in ['Azimuth', 'Elevation']]
+            if az is None or el is None:
+                raise KeyError
+        except KeyError:
+            return None, 'Current position could not be determined.'
+
+        moves = self.sun.analyze_paths(az, el, target_az, target_el)
+        move, decisions = avoidance.select_move(moves, {})
+        if move is None:
+            return None, 'No Sun-Safe moves could be identified!'
+
+        return list(move['moves'].nodes[1:]), None
+
     @ocs_agent.param('starting_index', type=int, default=0)
     def exercise(self, session, params):
         """exercise(starting_index=0)
@@ -1852,6 +1977,9 @@ def add_agent_args(parser_in=None):
                         nargs='+', help="One or more axes to ignore.")
     pgroup.add_argument("--disable-idle-reset", action='store_true',
                         help="Disable idle_reset, even for LAT.")
+    pgroup.add_argument("--solar-avoidance", type=float,
+                        help="If set (and > 0), enable active solar avoidance "
+                        "using this radius (in deg) for the exclusion zone.")
     return parser_in
 
 
@@ -1864,7 +1992,8 @@ def main(args=None):
     _ = ACUAgent(agent, args.acu_config, args.exercise_plan,
                  startup=not args.no_processes,
                  ignore_axes=args.ignore_axes,
-                 disable_idle_reset=args.disable_idle_reset)
+                 disable_idle_reset=args.disable_idle_reset,
+                 solar_avoidance=args.solar_avoidance)
 
     runner.run(agent, auto_reconnect=True)
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
new file mode 100644
index 000000000..3e3fea9c4
--- /dev/null
+++ b/socs/agents/acu/avoidance.py
@@ -0,0 +1,500 @@
+"""Sun Avoidance
+
+This module provides code to support Sun Avoidance in the ACU Agent.
+The basic idea is to create a map in equatorial coordinates where the
+value of the map indicates when that region of the sky will next be
+within the Sun Exclusion Zone (likely defined as some radius around
+the Sun).
+
+Using that pre-computed map, any az-el pointings can be checked for
+Sun safety (i.e. whether they are safe positoins), at least for the
+following 24 hours.  The map can be used to identify safest routes
+between two az-el pointings.
+
+"""
+import datetime
+import math
+import time
+
+import ephem
+import numpy as np
+from pixell import enmap
+from so3g.proj import coords, quat
+
+try:
+    import pylab as pl
+except ModuleNotFoundError:
+    pass
+
+DEG = np.pi / 180
+
+HOUR = 3600
+DAY = 86400
+NO_TIME = DAY * 2
+
+
+class SunTracker:
+    """Provide guidance on what horizion coordinate positions are
+    sun-safe.
+
+    Key concepts:
+    - Sun Safety Map
+    - Az-el trajectory
+
+    Args:
+      exclusion_radius (float, deg): radius of circle around the Sun
+        to consider as "unsafe".
+      map_res (float, deg): resolution to use for the Sun Safety Map.
+      time_res (float, s): Time resolution at which to evaluate Sun
+        trajectory.
+      site (str or None): Site to use (so3g site, defaults to so_lat).
+
+    """
+
+    def __init__(self, exclusion_radius=20., map_res=0.5,
+                 time_res=300., site=None, horizon=0.):
+        # Store in radians.
+        self.exclusion_radius = exclusion_radius * DEG
+        self.res = map_res * DEG
+        self.time_res = time_res
+        self.horizon = horizon
+
+        if site is None:
+            # This is close enough.
+            site = coords.SITES['so_lat']
+        site_eph = ephem.Observer()
+        site_eph.lon = site.lon * DEG
+        site_eph.lat = site.lat * DEG
+        site_eph.elevation = site.elev
+        self._site = site_eph
+        self.base_time = None
+        self.fake_now = None
+
+    def _now(self):
+        if self.fake_now:
+            return self.fake_now
+        return time.time()
+
+    def reset(self, base_time=None, staleness=None):
+        """Compute and store the Sun Safety Map for a specific
+        timestamp.
+
+        This basic computation is required prior to calling other
+        functions that use the Sun Safety Map.
+
+        If staleness is provided, then the map is only updated if it
+        has not yet been computed, or if the requested base_time is
+        earlier than the base_time of the currently stored map, or if
+        the requested base_time is more than staleness seconds in the
+        future from the currently store map.
+
+        """
+        # Set a reference time -- the map of sun times is usable from
+        # this reference time to at least 12 hours in the future.
+        if base_time is None:
+            base_time = self._now()
+
+        if self.base_time is not None and staleness is not None:
+            if ((base_time - self.base_time) > 0
+                    and (base_time - self.base_time) < staleness):
+                return False
+
+        # Identify zenith (ra, dec) at base_time.
+        Qz = coords.CelestialSightLine.naive_az_el(
+            base_time, 180. * DEG, 90. * DEG).Q
+        ra_z, dec_z, _ = quat.decompose_lonlat(Qz)
+
+        # Map extends from dec -80 to +80.
+        shape, wcs = enmap.band_geometry(
+            dec_cut=80 * DEG, res=self.res, proj='car')
+
+        # The map of sun time deltas
+        sun_times = enmap.zeros(shape, wcs=wcs) - 1
+        sun_dist = enmap.zeros(shape, wcs=wcs) - 1
+
+        # Quaternion rotation for each point in the map.
+        dec, ra = sun_times.posmap()
+        map_q = quat.rotation_lonlat(ra.ravel(), dec.ravel())
+
+        self._site.date = \
+            datetime.datetime.utcfromtimestamp(base_time + 0)
+        v = ephem.Sun(self._site)
+
+        # Get the map of angular distance to the Sun.
+        qsun = quat.rotation_lonlat(v.ra, v.dec)
+        sun_dist[:] = (quat.decompose_iso(~qsun * map_q)[0]
+                       .reshape(sun_dist.shape) / coords.DEG)
+
+        # Get the map where each pixel says the time delay between
+        # base_time and when the time when the sky coordinate will be
+        # in the Sun mask.  This is not terribly fast.  The Sun moves
+        # slowly enough that one could do a decent job of filling in
+        # the rest of the map based on the t=0 footprint.  Fix me.
+        for dt in np.arange(0, 24 * HOUR, self.time_res):
+            qsun = quat.rotation_lonlat(v.ra - dt / HOUR * 15. * DEG, v.dec)
+            qoff = ~qsun * map_q
+            r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape)
+            mask = (sun_times < 0) * (r < self.exclusion_radius)
+            sun_times[mask] = dt
+
+        # Fill in remaining -1 with NO_TIME.
+        sun_times[sun_times < 0] = NO_TIME
+
+        # Store the sun_times map and stuff.
+        self.base_time = base_time
+        self.sun_times = sun_times
+        self.sun_dist = sun_dist
+        self.map_q = map_q
+        return True
+
+    def _save(self, filename):
+        import pickle
+
+        # Pickle results of "reset"
+        pickle.dump((self.base_time, self.map_q, self.sun_dist.wcs,
+                     self.sun_dist, self.sun_times),
+                    open(filename, 'wb'))
+
+    def _load(self, filename):
+        import pickle
+        X = pickle.load(open(filename, 'rb'))
+        self.base_time = X[0]
+        self.sun_times = enmap.ndmap(X[4], wcs=X[2])
+        self.sun_dist = enmap.ndmap(X[3], wcs=X[2])
+        self.map_q = X[1]
+
+    def _azel_pix(self, az, el, dt=0, round=True, segments=False):
+        """Return the pixel indices of the Sun Safety Map that are
+        hit by the trajectory (az, el) at time dt.
+
+        Args:
+          az (array of float, deg): Azimuth.
+          el (array of float, deg): Elevation.
+          dt (array of float, s): Time offset relative to the base
+            time, at which to evaluate the trajectory.
+          round (bool): If True, round results to integer (for easy
+            look-up in the map).
+          segments (bool): If True, split up the trajectory into
+            segments (a list of pix_ji sections) such that they don't
+            cross the map boundaries at any point.
+
+        """
+        az = np.asarray(az)
+        el = np.asarray(el)
+        qt = coords.CelestialSightLine.naive_az_el(
+            self.base_time + dt, az * DEG, el * DEG).Q
+        ra, dec, _ = quat.decompose_lonlat(qt)
+        pix_ji = self.sun_times.sky2pix((dec, ra))
+        if round:
+            pix_ji = pix_ji.round().astype(int)
+            # Handle out of bounds as follows:
+            # - RA indices are mod-ed into range.
+            # - dec indices are clamped to the map edge.
+            j, i = pix_ji
+            j[j < 0] = 0
+            j[j >= self.sun_times.shape[-2]] = self.sun_times.shape[-2] - 1
+            i[:] = i % self.sun_times.shape[-1]
+
+        if segments:
+            jumps = ((abs(np.diff(pix_ji[0])) > self.sun_times.shape[-2] / 2)
+                     + (abs(np.diff(pix_ji[1])) > self.sun_times.shape[-1] / 2))
+            jump = jumps.nonzero()[0]
+            starts = np.hstack((0, jump + 1))
+            stops = np.hstack((jump + 1, len(pix_ji[0])))
+            return [pix_ji[:, a:b] for a, b in zip(starts, stops)]
+
+        return pix_ji
+
+    def check_trajectory(self, az, el, t=None, raw=False):
+        """For a telescope trajectory (vectors az, el, in deg), assumed to
+        occur at time t, get the minimum value of the Sun Safety Map
+        traversed by that trajectory.  Also get the minimum value of
+        the Sun Distance map.
+
+        This requires the Sun Safety Map to have been computed with a
+        base_time of t - 24 hours or later.
+
+        Returns the Sun Safety time for the trajectory, in seconds,
+        and nearest Sun approach, in degrees.
+
+        """
+        if t is None:
+            t = self.base_time
+        j, i = self._azel_pix(az, el, dt=t - self.base_time)
+        sun_delta = self.sun_times[j, i]
+        sun_dists = self.sun_dist[j, i]
+
+        # If sun is below horizon, rail sun_dist to 180 deg.
+        if self.get_sun_pos(t=t)['sun_azel'][1] < self.horizon:
+            sun_dists[:] = 180.
+
+        if raw:
+            return sun_delta, sun_dists
+        return {
+            'sun_time': sun_delta.min(),
+            'sun_dist_min': sun_dists.min(),
+            'sun_dist_mean': sun_dists.mean(),
+        }
+
+    def get_sun_pos(self, az=None, el=None, t=None):
+        """Get info on the Sun's location at time t.  If (az, el) are also
+        specified, returns the angular separation between that
+        pointing and Sun's center.
+
+        """
+        if t is None:
+            t = self._now()
+        self._site.date = \
+            datetime.datetime.utcfromtimestamp(t)
+        v = ephem.Sun(self._site)
+        qsun = quat.rotation_lonlat(v.ra, v.dec)
+
+        qzen = coords.CelestialSightLine.naive_az_el(t, 0, np.pi / 2).Q
+        neg_zen_az, zen_el, _ = quat.decompose_lonlat(~qzen * qsun)
+
+        results = {
+            'sun_radec': (v.ra / DEG, v.dec / DEG),
+            'sun_azel': (-neg_zen_az / DEG, zen_el / DEG),
+        }
+        if az is not None:
+            qtel = coords.CelestialSightLine.naive_az_el(
+                t, az * DEG, el * DEG).Q
+            r = quat.decompose_iso(~qtel * qsun)[0]
+            results['sun_dist'] = r / DEG
+        return results
+
+    def show_map(self, axes=None, show=True):
+        """Plot the Sun Safety Map and Sun Distance Map on the provided axes
+        (a list)."""
+        if axes is None:
+            fig, axes = pl.subplots(2, 1)
+            fig.tight_layout()
+        else:
+            fig = None
+
+        imgs = []
+        for axi, ax in enumerate(axes):
+            if axi == 0:
+                # Sun safe time
+                x = self.sun_times / HOUR
+                x[x == NO_TIME] = np.nan
+                title = 'Sun safe time (hours)'
+            elif axi == 1:
+                # Sun distance
+                x = self.sun_dist
+                title = 'Sun distance (degrees)'
+            im = ax.imshow(x, origin='lower', cmap='Oranges')
+            ji = self._azel_pix(0, np.array([90.]))
+            ax.scatter(ji[1], ji[0], marker='x', color='white')
+            ax.set_title(title)
+            pl.colorbar(im, ax=ax)
+            imgs.append(im)
+
+        if show:
+            pl.show()
+
+        return fig, axes, imgs
+
+    def analyze_paths(self, az0, el0, az1, el1, t=None,
+                      plot_file=None, policy=None):
+        if t is None:
+            t = self._now()
+
+        if plot_file:
+            assert (t == self.base_time)  # Can only plot "now" results.
+            fig, axes, imgs = self.show_map(show=False)
+            last_el = None
+
+        # Test all trajectories with intermediate el.
+        all_moves = []
+
+        base = {
+            'req_start': (az0, el0),
+            'req_stop': (az1, el1),
+            'req_time': t,
+            'travel_el': (el0 + el1) / 2,
+            'travel_el_confined': True,
+            'direct': True,
+        }
+
+        # Suitable list of test els.
+        if el0 == el1:
+            el_nodes = [el0]
+        else:
+            el_nodes = sorted([el0, el1])
+        if 10. < el_nodes[0]:
+            el_nodes.insert(0, 10.)
+        if 90. > el_nodes[-1]:
+            el_nodes.append(90.)
+
+        el_sep = 1.
+        el_cands = []
+        for i in range(len(el_nodes) - 1):
+            n = math.ceil((el_nodes[i + 1] - el_nodes[i]) / el_sep)
+            assert (n >= 1)
+            el_cands.extend(list(
+                np.linspace(el_nodes[i], el_nodes[i + 1], n + 1)[:-1]))
+        el_cands.append(el_nodes[-1])
+
+        for iel in el_cands:
+            detail = dict(base)
+            detail.update({
+                'direct': False,
+                'travel_el': iel,
+                'travel_el_confined': (iel >= min(el0, el1)) and (iel <= max(el0, el1)),
+            })
+            moves = MoveSequence(az0, el0, az0, iel, az1, iel, az1, el1, simplify=True)
+
+            detail['moves'] = moves
+            traj_info = self.check_trajectory(*moves.get_traj(), t=t)
+            detail.update(traj_info)
+            all_moves.append(detail)
+            if plot_file and (last_el is None or abs(last_el - iel) > 5):
+                c = 'black'
+                for j, i in self._azel_pix(*moves.get_traj(), round=True, segments=True):
+                    for ax in axes:
+                        a, = ax.plot(i, j, color=c, lw=1)
+                last_el = iel
+
+        # Include the "direct" path.
+        direct = dict(base)
+        direct['moves'] = MoveSequence(az0, el0, az1, el1)
+        traj_info = self.check_trajectory(*direct['moves'].get_traj(), t=t)
+        direct.update(traj_info)
+        all_moves.append(direct)
+
+        if plot_file:
+            # Add the direct traj, in blue.
+            segments = self._azel_pix(*direct['moves'].get_traj(), round=True, segments=True)
+            for ax in axes:
+                for j, i in segments:
+                    ax.plot(i, j, color='blue')
+                for seg, rng, mrk in [(segments[0], slice(0, 1), 'o'),
+                                      (segments[-1], slice(-1, None), 'x')]:
+                    ax.scatter(seg[1][rng], seg[0][rng], marker=mrk, color='blue')
+            # Add the selected trajectory in green.
+            selected = None
+            if policy is not None:
+                selected = select_move(all_moves, policy)[0]
+            if selected is not None:
+                traj = selected['moves'].get_traj()
+                segments = self._azel_pix(*traj, round=True, segments=True)
+                for ax in axes:
+                    for j, i in segments:
+                        ax.plot(i, j, color='green')
+
+            pl.savefig(plot_file)
+        return all_moves
+
+
+class MoveSequence:
+    def __init__(self, *args, simplify=False):
+        self.nodes = []
+        if len(args) == 0:
+            return
+        is_tuples = [isinstance(a, tuple) for a in args]
+        if all(is_tuples):
+            pass
+        elif any(is_tuples):
+            raise ValueError('Constructor accepts tuples or az, el, az, el; not a mix.')
+        else:
+            assert (len(args) % 2 == 0)
+            args = [(args[i], args[i + 1]) for i in range(0, len(args), 2)]
+        for (az, el) in args:
+            self.nodes.append((az, el))
+        if simplify:
+            # Remove repeated nodes.
+            idx = 0
+            while idx < len(self.nodes) - 1:
+                if self.nodes[idx] == self.nodes[idx + 1]:
+                    self.nodes.pop(idx + 1)
+                else:
+                    idx += 1
+
+    def get_legs(self):
+        """Iterate over the legs of the MoveSequence; yields each ((az_start,
+        el_start), (az_end, az_end)).
+
+        """
+        for i in range(len(self.nodes) - 1):
+            yield self.nodes[i:i + 2]
+
+    def get_traj(self, res=0.5):
+        """Return (az, el) vectors with the full path for the MoveSequence.
+        No step in az or el will be greater than res.
+
+        """
+        xx, yy = [], []
+        for (x0, y0), (x1, y1) in self.get_legs():
+            n = max(2, math.ceil(abs(x1 - x0) / res), math.ceil(abs(y1 - y0) / res))
+            xx.append(np.linspace(x0, x1, n))
+            yy.append(np.linspace(y0, y1, n))
+        return np.hstack(tuple(xx)), np.hstack(tuple(yy))
+
+
+DEFAULT_POLICY = {
+    'min_el': 0,
+    'max_el': 90,
+    'el_dodging': True,
+    'min_sun_time': HOUR,
+    'response_time': HOUR * 4,
+}
+
+
+def select_move(moves, policy):
+    for k in policy.keys():
+        assert k in DEFAULT_POLICY
+    _p = dict(DEFAULT_POLICY)
+    _p.update(policy)
+
+    decisions = [{'rejected': False,
+                  'reason': None} for m in moves]
+
+    def reject(d, reason):
+        d['rejected'] = True
+        d['reason'] = reason
+
+    # According to policy, reject moves outright.
+    for m, d in zip(moves, decisions):
+        if d['rejected']:
+            continue
+
+        els = m['req_start'][1], m['req_stop'][1]
+
+        if m['sun_time'] < _p['min_sun_time']:
+            reject(d, 'Path too close to sun.')
+            continue
+
+        if m['travel_el'] < _p['min_el']:
+            reject(d, 'Path goes below minimum el.')
+            continue
+
+        if m['travel_el'] > _p['max_el']:
+            reject(d, 'Path goes above maximum el.')
+            continue
+
+        if not _p['el_dodging']:
+            if m['travel_el'] < min(*els):
+                reject(d, 'Path dodges (goes below necessary el range).')
+                continue
+            if m['travel_el'] > max(*els):
+                reject(d, 'Path dodges (goes above necessary el range).')
+
+    cands = [m for m, d in zip(moves, decisions)
+             if not d['rejected']]
+    if len(cands) == 0:
+        return None, decisions
+
+    def priority_func(m):
+        # Sorting key for move proposals.
+        els = m['req_start'][1], m['req_stop'][1]
+        return (
+            m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'],
+            m['direct'],
+            m['sun_dist_min'],
+            m['sun_dist_mean'],
+            -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])),
+            m['travel_el'],
+        )
+    cands.sort(key=priority_func)
+    return cands[-1], decisions

From 89322ba12188b5469d3b46f40089f0ea5ce7d542 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 25 Oct 2023 02:13:55 +0000
Subject: [PATCH 02/21] ACU sun: update .sun in the process

---
 socs/agents/acu/agent.py | 42 +++++++++++++++++++++++-----------------
 1 file changed, 24 insertions(+), 18 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index b794cf992..f67ee7bfa 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -1766,16 +1766,21 @@ def solar_avoidance(self, session, params):
         # - maintain a Sun Safety map for other ops to query.
         # - guide the platform to Sun Safe position, if needed.
 
-        recomp = False
-
-        def _notify_recomputed(was_recomp, start_time):
-            nonlocal recomp
-            recomp = False
-            if was_recomp:
-                self.log.info('Recomputed Sun Safety Map (took %.1fs)' %
-                              (time.time() - start_time))
-
-        self.sun = avoidance.SunTracker()
+        def _get_sun_map():
+            # To run in thread ...
+            start = time.time()
+            print(start)
+            new_sun = avoidance.SunTracker()
+            new_sun.reset()
+            return new_sun, time.time() - start
+
+        def _notify_recomputed(result):
+            new_sun, compute_time = result
+            self.log.info('(Re-)computed Sun Safety Map (took %.1fs)' %
+                          compute_time)
+            self.sun = new_sun
+
+        self.sun = None
         session.data = {}
         session.set_status('running')
 
@@ -1790,17 +1795,18 @@ def _notify_recomputed(was_recomp, start_time):
                 yield dsleep(1)
                 continue
 
-            info = self.sun.get_sun_pos(az, el)
-            session.data.update(info)
+            # if self.sun is None or (self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR):
+            if self.sun is None or (self.sun._now() - self.sun.base_time > 60):
+                threads.deferToThread(_get_sun_map).addCallback(
+                    _notify_recomputed)
 
-            if not recomp:
-                recomp = True
-                threads.deferToThread(self.sun.reset, staleness=12 * 3600).addCallback(
-                    _notify_recomputed, time.time())
-
-            if self.sun.base_time is not None:
+            if self.sun is not None:
+                print(self.sun.base_time)
+                info = self.sun.get_sun_pos(az, el)
+                session.data.update(info)
                 t = self.sun.check_trajectory([az], [el])['sun_time']
                 session.data['sun_safe_time'] = t if t > 0 else 0
+
             yield dsleep(10)
 
     @inlineCallbacks

From 55104b8c6d883a19acf827e9459dd15a78a3119c Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 25 Oct 2023 04:18:02 +0000
Subject: [PATCH 03/21] ACU sun: working non-blocking state machine to seek
 safety

Can be triggered for testing.  Needs logging and a bit more
safetyizing.
---
 socs/agents/acu/agent.py     | 87 +++++++++++++++++++++++++++++++++---
 socs/agents/acu/avoidance.py | 59 +++++++++++++++++++++---
 2 files changed, 133 insertions(+), 13 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index f67ee7bfa..a8a0df5af 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -108,12 +108,14 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         self.solar_params = {
             'active_avoidance': False,
             'radius': 0,
+            'next_drill': None,
         }
         if solar_avoidance is not None and solar_avoidance > 0:
             self.solar_params.update({
                 'active_avoidance': True,
                 'radius': solar_avoidance,
             })
+        self.avoidance_lockdown = False
 
         self.exercise_plan = exercise_plan
 
@@ -1172,6 +1174,9 @@ def go_to(self, session, params):
             if not acquired:
                 return False, f"Operation failed: {self.azel_lock.job} is running."
 
+            if self.avoidance_lockdown:
+                return False, "Motion blocked; avoidance in progress."
+
             self.log.info('Clearing faults to prepare for motion.')
             yield self.acu_control.clear_faults()
             yield dsleep(1)
@@ -1510,6 +1515,9 @@ def generate_scan(self, session, params):
           Process .stop method is called)..
 
         """
+        if self.avoidance_lockdown:
+            return False, "Motion blocked; avoidance in progress."
+
         self.log.info('User scan params: {params}', params=params)
 
         az_endpoint1 = params['az_endpoint1']
@@ -1769,18 +1777,23 @@ def solar_avoidance(self, session, params):
         def _get_sun_map():
             # To run in thread ...
             start = time.time()
-            print(start)
             new_sun = avoidance.SunTracker()
             new_sun.reset()
             return new_sun, time.time() - start
 
         def _notify_recomputed(result):
+            nonlocal req_out
             new_sun, compute_time = result
             self.log.info('(Re-)computed Sun Safety Map (took %.1fs)' %
                           compute_time)
             self.sun = new_sun
+            req_out = False
 
+        req_out = False
         self.sun = None
+        state = 'init'
+        last_state = state
+
         session.data = {}
         session.set_status('running')
 
@@ -1795,19 +1808,79 @@ def _notify_recomputed(result):
                 yield dsleep(1)
                 continue
 
-            # if self.sun is None or (self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR):
-            if self.sun is None or (self.sun._now() - self.sun.base_time > 60):
+            if not req_out and (self.sun is None
+                                or (self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR)):
+                req_out = True
                 threads.deferToThread(_get_sun_map).addCallback(
                     _notify_recomputed)
 
             if self.sun is not None:
-                print(self.sun.base_time)
                 info = self.sun.get_sun_pos(az, el)
                 session.data.update(info)
                 t = self.sun.check_trajectory([az], [el])['sun_time']
                 session.data['sun_safe_time'] = t if t > 0 else 0
 
-            yield dsleep(10)
+            if state == 'init':
+                if self.sun is not None:
+                    state = 'idle'
+            elif state == 'idle':
+                if t < 3600:
+                    state = 'shelter-abort'
+                tnd = self.solar_params['next_drill']
+                if tnd is not None and tnd <= time.time():
+                    state = 'shelter-abort'
+                    self.solar_params['next_drill'] = None
+            elif state == 'shelter-abort':
+                # raise stop flags and issue stop on motion ops
+                self.avoidance_lockdown = True
+                for op in ['generate_scan', 'go_to']:
+                    self.agent.stop(op)
+                    self.agent.abort(op)
+                state = 'shelter-wait-idle'
+                timeout = 30
+            elif state == 'shelter-wait-idle':
+                for op in ['generate_scan', 'go_to']:
+                    ok, msg, _session = self.agent.status(op)
+                    if _session.get('status', 'done') != 'done':
+                        break
+                else:
+                    state = 'shelter-move'
+                timeout -= 1
+                if timeout < 0:
+                    state = 'shelter-stop'
+            elif state == 'shelter-stop':
+                yield self._stop()
+                state = 'shelter-move'
+
+            elif state == 'shelter-move':
+                paths = self.sun.find_escape_paths(az, el)
+                legs = paths[0]['moves'].nodes[1:]
+                state = 'shelter-move-legs'
+                leg_d = None
+
+            elif state == 'shelter-move-legs':
+                def _leg_done(result):
+                    all_ok, msg = result
+                    print('leg done', all_ok, msg)
+                    nonlocal leg_d
+                    leg_d = None
+                if leg_d is None:
+                    if len(legs) == 0:
+                        state = 'safe-i-guess'
+                    else:
+                        leg_az, leg_el = legs.pop(0)
+                        leg_d = self._go_to_axes(session, az=leg_az, el=leg_el)
+                        leg_d.addCallback(_leg_done)
+
+            elif state == 'safe-i-guess':
+                self.avoidance_lockdown = False
+                state = 'idle'
+
+            session.data['state'] = state
+            if state != last_state:
+                self.log.info('solar_avoidance: state is now "{state}"', state=state)
+                last_state = state
+            yield dsleep(1)
 
     @inlineCallbacks
     def update_solar(self, session, params):
@@ -1815,7 +1888,9 @@ def update_solar(self, session, params):
 
         **Task** - Update solar avoidance parameters.
         """
-        pass
+        self.solar_params['next_drill'] = time.time() + 10
+        yield
+        return True, 'Did a thing.'
 
     def _get_sun_policy(self, key):
         return True
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 3e3fea9c4..33af3a6fb 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -232,6 +232,10 @@ def check_trajectory(self, az, el, t=None, raw=False):
             return sun_delta, sun_dists
         return {
             'sun_time': sun_delta.min(),
+            'sun_time_start': sun_delta[0],
+            'sun_time_stop': sun_delta[-1],
+            'sun_dist_start': sun_dists[0],
+            'sun_dist_stop': sun_dists[-1],
             'sun_dist_min': sun_dists.min(),
             'sun_dist_mean': sun_dists.mean(),
         }
@@ -296,7 +300,12 @@ def show_map(self, axes=None, show=True):
         return fig, axes, imgs
 
     def analyze_paths(self, az0, el0, az1, el1, t=None,
-                      plot_file=None, policy=None):
+                      plot_file=None, policy=None, dodging=True):
+        """Design and analyze a number of different paths between (az0, el0)
+        and (az1, el1).  Return the list, for further processing and
+        choice.
+
+        """
         if t is None:
             t = self._now()
 
@@ -322,9 +331,9 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
             el_nodes = [el0]
         else:
             el_nodes = sorted([el0, el1])
-        if 10. < el_nodes[0]:
+        if dodging and (10. < el_nodes[0]):
             el_nodes.insert(0, 10.)
-        if 90. > el_nodes[-1]:
+        if dodging and (90. > el_nodes[-1]):
             el_nodes.append(90.)
 
         el_sep = 1.
@@ -386,6 +395,34 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
             pl.savefig(plot_file)
         return all_moves
 
+    def find_escape_paths(self, az0, el0, t=None,
+                          plot_file=None, policy=None):
+        """Design and analyze a number of different paths that move from (az0,
+        el0) to a sun safe position.  Return the list, for further
+        processing and choice.
+
+        """
+        if t is None:
+            t = self._now()
+
+        # Preference is to not change altitude.  But we may need to
+        # lower it.
+        el1 = el0
+        paths = []
+        while len(paths) == 0 and el1 > 0:
+            paths1 = self.analyze_paths(az0, el0, 0., el1, t=t,
+                                        dodging=False)
+            paths2 = self.analyze_paths(az0, el0, 180., el1, t=t,
+                                        dodging=False)
+            best_path1, decisions = select_move(paths1, {},
+                                                escape=True)
+            best_path2, decisions = select_move(paths2, {},
+                                                escape=True)
+            paths = [bp for bp in [best_path1, best_path2] if bp is not None]
+            el1 -= 1.
+
+        return paths
+
 
 class MoveSequence:
     def __init__(self, *args, simplify=False):
@@ -441,7 +478,7 @@ def get_traj(self, res=0.5):
 }
 
 
-def select_move(moves, policy):
+def select_move(moves, policy, escape=False):
     for k in policy.keys():
         assert k in DEFAULT_POLICY
     _p = dict(DEFAULT_POLICY)
@@ -461,9 +498,17 @@ def reject(d, reason):
 
         els = m['req_start'][1], m['req_stop'][1]
 
-        if m['sun_time'] < _p['min_sun_time']:
-            reject(d, 'Path too close to sun.')
-            continue
+        if escape and (m['sun_time_start'] < _p['min_sun_time']):
+            if m['sun_dist_min'] < m['sun_dist_start']:
+                reject(d, 'Path moves even closer to sun.')
+                continue
+            if m['sun_time_stop'] < _p['min_sun_time']:
+                reject(d, 'Path does not end in sun-safe location.')
+                continue
+        else:
+            if m['sun_time'] < _p['min_sun_time']:
+                reject(d, 'Path too close to sun.')
+                continue
 
         if m['travel_el'] < _p['min_el']:
             reject(d, 'Path goes below minimum el.')

From d31f5a7aac7bdb7580ee0853408ca2b73c7c3cc1 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 25 Oct 2023 06:44:33 +0000
Subject: [PATCH 04/21] ACU sun: faster sun map computation

---
 socs/agents/acu/avoidance.py | 30 ++++++++++++++++--------------
 1 file changed, 16 insertions(+), 14 deletions(-)

diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 33af3a6fb..2343b740d 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -45,18 +45,15 @@ class SunTracker:
       exclusion_radius (float, deg): radius of circle around the Sun
         to consider as "unsafe".
       map_res (float, deg): resolution to use for the Sun Safety Map.
-      time_res (float, s): Time resolution at which to evaluate Sun
-        trajectory.
       site (str or None): Site to use (so3g site, defaults to so_lat).
 
     """
 
-    def __init__(self, exclusion_radius=20., map_res=0.5,
-                 time_res=300., site=None, horizon=0.):
+    def __init__(self, exclusion_radius=20., map_res=.5,
+                 site=None, horizon=0.):
         # Store in radians.
         self.exclusion_radius = exclusion_radius * DEG
         self.res = map_res * DEG
-        self.time_res = time_res
         self.horizon = horizon
 
         if site is None:
@@ -127,15 +124,20 @@ def reset(self, base_time=None, staleness=None):
 
         # Get the map where each pixel says the time delay between
         # base_time and when the time when the sky coordinate will be
-        # in the Sun mask.  This is not terribly fast.  The Sun moves
-        # slowly enough that one could do a decent job of filling in
-        # the rest of the map based on the t=0 footprint.  Fix me.
-        for dt in np.arange(0, 24 * HOUR, self.time_res):
-            qsun = quat.rotation_lonlat(v.ra - dt / HOUR * 15. * DEG, v.dec)
-            qoff = ~qsun * map_q
-            r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape)
-            mask = (sun_times < 0) * (r < self.exclusion_radius)
-            sun_times[mask] = dt
+        # in the Sun mask.
+        dt = -ra[0] * DAY / (2 * np.pi)
+        qsun = quat.rotation_lonlat(v.ra, v.dec)
+        qoff = ~qsun * map_q
+        r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape)
+        sun_times[r < self.exclusion_radius] = 0.
+        for g in sun_times:
+            if (g < 0).all():
+                continue
+            # Identify pixel on the right of the masked region.
+            flips = ((g == 0) * np.hstack((g[:-1] != g[1:], g[-1] != g[0]))).nonzero()[0]
+            dt0 = dt[flips[0]]
+            _dt = (dt - dt0) % DAY
+            g[g < 0] = _dt[g < 0]
 
         # Fill in remaining -1 with NO_TIME.
         sun_times[sun_times < 0] = NO_TIME

From 194504950970be16a76d412c83eab21e2e2adc70 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Mon, 30 Oct 2023 10:59:39 +0000
Subject: [PATCH 05/21] ACU sun: capacity to time-shift the Sun's position, for
 testing

Also ability to temporarily disable the feature.
---
 socs/agents/acu/agent.py     | 49 ++++++++++++++++++++++++++++++------
 socs/agents/acu/avoidance.py | 19 ++++++++------
 2 files changed, 53 insertions(+), 15 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index a8a0df5af..2b8184a79 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -109,6 +109,9 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
             'active_avoidance': False,
             'radius': 0,
             'next_drill': None,
+            'shift_sun_hours': 0,
+            'do_recompute': False,
+            'disable_until': 0,
         }
         if solar_avoidance is not None and solar_avoidance > 0:
             self.solar_params.update({
@@ -1777,7 +1780,7 @@ def solar_avoidance(self, session, params):
         def _get_sun_map():
             # To run in thread ...
             start = time.time()
-            new_sun = avoidance.SunTracker()
+            new_sun = avoidance.SunTracker(sun_time_shift=self.solar_params['shift_sun_hours'] * 3600.)
             new_sun.reset()
             return new_sun, time.time() - start
 
@@ -1808,9 +1811,17 @@ def _notify_recomputed(result):
                 yield dsleep(1)
                 continue
 
-            if not req_out and (self.sun is None
-                                or (self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR)):
+            no_map = self.sun is None
+            old_map = (not no_map
+                       and self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR)
+            do_recompute = (
+                not req_out
+                and (no_map or old_map or self.solar_params['recompute'])
+            )
+
+            if do_recompute:
                 req_out = True
+                self.solar_params['recompute'] = False
                 threads.deferToThread(_get_sun_map).addCallback(
                     _notify_recomputed)
 
@@ -1882,18 +1893,40 @@ def _leg_done(result):
                 last_state = state
             yield dsleep(1)
 
-    @inlineCallbacks
+    @ocs_agent.param('trigger_panic', type=bool, default=False)
+    @ocs_agent.param('shift_sun_hours', type=float, default=None)
+    @ocs_agent.param('temporary_disable', type=float, default=None)
     def update_solar(self, session, params):
         """update_solar()
 
         **Task** - Update solar avoidance parameters.
         """
-        self.solar_params['next_drill'] = time.time() + 10
-        yield
-        return True, 'Did a thing.'
+        do_recompute = False
+        now = time.time()
+
+        if params['trigger_panic']:
+            self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
+            self.solar_params['next_drill'] = now + 10
+        if params['shift_sun_hours'] is not None:
+            self.solar_params['shift_sun_hours'] = params['shift_sun_hours']
+            do_recompute = True
+        if params['temporary_disable'] is not None:
+            self.solar_params['disable_until'] = params['temporary_disable'] + now
+
+        if do_recompute:
+            self.solar_params['recompute'] = True
+
+        return True, 'Params updated.'
 
     def _get_sun_policy(self, key):
-        return True
+        now = time.time()
+        p = self.solar_params
+
+        if key == 'sunsafe_moves':
+            return p['active_avoidance'] and (now >= p['disable_until'])
+
+        else:
+            return p[key]
 
     def _get_sunsafe_moves(self, target_az, target_el):
         if not self._get_sun_policy('sunsafe_moves'):
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 2343b740d..6607ebc26 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -50,11 +50,12 @@ class SunTracker:
     """
 
     def __init__(self, exclusion_radius=20., map_res=.5,
-                 site=None, horizon=0.):
+                 sun_time_shift=0., site=None, horizon=0.):
         # Store in radians.
         self.exclusion_radius = exclusion_radius * DEG
         self.res = map_res * DEG
         self.horizon = horizon
+        self.sun_time_shift = sun_time_shift
 
         if site is None:
             # This is close enough.
@@ -72,6 +73,11 @@ def _now(self):
             return self.fake_now
         return time.time()
 
+    def _sun(self, t):
+        self._site.date = \
+            datetime.datetime.utcfromtimestamp(t + self.sun_time_shift)
+        return ephem.Sun(self._site)
+
     def reset(self, base_time=None, staleness=None):
         """Compute and store the Sun Safety Map for a specific
         timestamp.
@@ -113,9 +119,7 @@ def reset(self, base_time=None, staleness=None):
         dec, ra = sun_times.posmap()
         map_q = quat.rotation_lonlat(ra.ravel(), dec.ravel())
 
-        self._site.date = \
-            datetime.datetime.utcfromtimestamp(base_time + 0)
-        v = ephem.Sun(self._site)
+        v = self._sun(base_time)
 
         # Get the map of angular distance to the Sun.
         qsun = quat.rotation_lonlat(v.ra, v.dec)
@@ -250,9 +254,7 @@ def get_sun_pos(self, az=None, el=None, t=None):
         """
         if t is None:
             t = self._now()
-        self._site.date = \
-            datetime.datetime.utcfromtimestamp(t)
-        v = ephem.Sun(self._site)
+        v = self._sun(t)
         qsun = quat.rotation_lonlat(v.ra, v.dec)
 
         qzen = coords.CelestialSightLine.naive_az_el(t, 0, np.pi / 2).Q
@@ -262,6 +264,9 @@ def get_sun_pos(self, az=None, el=None, t=None):
             'sun_radec': (v.ra / DEG, v.dec / DEG),
             'sun_azel': (-neg_zen_az / DEG, zen_el / DEG),
         }
+        if self.sun_time_shift != 0:
+            results['WARNING'] = 'Fake Sun Position is in use!'
+
         if az is not None:
             qtel = coords.CelestialSightLine.naive_az_el(
                 t, az * DEG, el * DEG).Q

From aed7aaa072b0d226c5643219685e4c320fc33017 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Mon, 30 Oct 2023 17:04:27 +0000
Subject: [PATCH 06/21] ACU sun: generate_scan checks traj before starting

Also the safe position seek clears faults.
---
 socs/agents/acu/agent.py     | 61 +++++++++++++++++++++++++++++++-----
 socs/agents/acu/avoidance.py | 27 ++++++++++------
 2 files changed, 70 insertions(+), 18 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 2b8184a79..eee6944a5 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -1109,7 +1109,8 @@ def get_history(t):
         return success, msg
 
     @inlineCallbacks
-    def _go_to_axes(self, session, el=None, az=None, third=None):
+    def _go_to_axes(self, session, el=None, az=None, third=None,
+                    clear_faults=False):
         """Execute a movement along multiple axes, using "Preset"
         mode.  This just launches _go_to_axis on each required axis,
         and collects the results.
@@ -1119,6 +1120,7 @@ def _go_to_axes(self, session, el=None, az=None, third=None):
           az (float): target for Azimuth axis (ignored if None).
           el (float): target for Elevation axis (ignored if None).
           third (float): target for Boresight axis (ignored if None).
+          clear_faults (bool): whether to clear ACU faults first.
 
         Returns:
           ok (bool): True if all motions completed successfully and
@@ -1139,6 +1141,10 @@ def _go_to_axes(self, session, el=None, az=None, third=None):
         if len(move_defs) is None:
             return True, 'No motion requested.'
 
+        if clear_faults:
+            yield self.acu_control.clear_faults()
+            yield dsleep(1)
+
         moves = yield DeferredList([d for n, d in move_defs])
         all_ok, msgs = True, []
         for _ok, result in moves:
@@ -1583,6 +1589,15 @@ def generate_scan(self, session, params):
         self.log.info('The plan: {plan}', plan=plan)
         self.log.info('The scan_params: {scan_params}', scan_params=scan_params)
 
+        # Before any motion, check for sun safety.
+        ok, msg = self._check_scan_sunsafe(az_endpoint1, az_endpoint2, el_endpoint1,
+                                           az_speed, az_accel)
+        if ok:
+            self.log.info('Sun safety check passes: {msg}', msg=msg)
+        else:
+            self.log.error('Sun safety check fails: {msg}', msg=msg)
+            return False, 'Scan is not Sun Safe.'
+
         # Clear faults.
         self.log.info('Clearing faults to prepare for motion.')
         yield self.acu_control.clear_faults()
@@ -1835,7 +1850,7 @@ def _notify_recomputed(result):
                 if self.sun is not None:
                     state = 'idle'
             elif state == 'idle':
-                if t < 3600:
+                if t < 3600 and self._get_sun_policy('shelter_enabled'):
                     state = 'shelter-abort'
                 tnd = self.solar_params['next_drill']
                 if tnd is not None and tnd <= time.time():
@@ -1862,25 +1877,32 @@ def _notify_recomputed(result):
             elif state == 'shelter-stop':
                 yield self._stop()
                 state = 'shelter-move'
-
             elif state == 'shelter-move':
                 paths = self.sun.find_escape_paths(az, el)
+                if len(paths) == 0:
+                    print('failed to find escape paths @%.1f, az=%.3f el=%.3f' %
+                          (time.time(), az, el))
+
                 legs = paths[0]['moves'].nodes[1:]
                 state = 'shelter-move-legs'
                 leg_d = None
-
             elif state == 'shelter-move-legs':
                 def _leg_done(result):
+                    nonlocal state
                     all_ok, msg = result
-                    print('leg done', all_ok, msg)
-                    nonlocal leg_d
-                    leg_d = None
+                    if not all_ok:
+                        # Recompute the escape path.
+                        state = 'shelter-move-legs'
+                    else:
+                        nonlocal leg_d
+                        leg_d = None
                 if leg_d is None:
                     if len(legs) == 0:
                         state = 'safe-i-guess'
                     else:
                         leg_az, leg_el = legs.pop(0)
-                        leg_d = self._go_to_axes(session, az=leg_az, el=leg_el)
+                        leg_d = self._go_to_axes(session, az=leg_az, el=leg_el,
+                                                 clear_faults=True)
                         leg_d.addCallback(_leg_done)
 
             elif state == 'safe-i-guess':
@@ -1924,10 +1946,33 @@ def _get_sun_policy(self, key):
 
         if key == 'sunsafe_moves':
             return p['active_avoidance'] and (now >= p['disable_until'])
+        elif key == 'shelter_enabled':
+            return p['active_avoidance'] and (now >= p['disable_until'])
 
         else:
             return p[key]
 
+    def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az):
+        # Include a bit of buffer for turn-arounds.
+        az1, az2 = min(az1, az2), max(az1, az2)
+        turn = v_az**2 / a_az
+        az1 -= turn
+        az2 += turn
+        n = max(2, int(np.ceil((az2 - az1) / 1.)))
+        azs = np.linspace(az1, az2, n)
+
+        info = self.sun.check_trajectory(azs, azs * 0 + el)
+        safe = info['sun_time'] >= 3600
+        if safe:
+            msg = 'Scan is safe for %.1f hours' % (info['sun_time'] / 3600)
+        else:
+            msg = 'Scan will be unsafe in %.1f hours' % (info['sun_time'] / 3600)
+
+        if self._get_sun_policy('sunsafe_moves'):
+            return safe, msg
+        else:
+            return True, 'Sun-safety not active; %s' % msg
+
     def _get_sunsafe_moves(self, target_az, target_el):
         if not self._get_sun_policy('sunsafe_moves'):
             return [(target_az, target_el)], None
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 6607ebc26..4d235fda3 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -213,19 +213,26 @@ def _azel_pix(self, az, el, dt=0, round=True, segments=False):
 
     def check_trajectory(self, az, el, t=None, raw=False):
         """For a telescope trajectory (vectors az, el, in deg), assumed to
-        occur at time t, get the minimum value of the Sun Safety Map
-        traversed by that trajectory.  Also get the minimum value of
-        the Sun Distance map.
+        occur at time t (defaults to now), get the minimum value of
+        the Sun Safety Map traversed by that trajectory.  Also get the
+        minimum value of the Sun Distance map.
 
         This requires the Sun Safety Map to have been computed with a
         base_time of t - 24 hours or later.
 
-        Returns the Sun Safety time for the trajectory, in seconds,
-        and nearest Sun approach, in degrees.
+        Returns a dict with entries:
+
+        - ``'sun_time'``: Minimum Sun Safety Time on the traj.
+        - ``'sun_time_start'``: Sun Safety Time at first point.
+        - ``'sun_time_stop'``: Sun Safety Time at last point.
+        - ``'sun_dist_min'``: Minimum distance to Sun, in degrees.
+        - ``'sun_dist_mean'``: Mean distance to Sun.
+        - ``'sun_dist_start'``: Distance to Sun, at first point.
+        - ``'sun_dist_stop'``: Distance to Sun, at last point.
 
         """
         if t is None:
-            t = self.base_time
+            t = self._now()
         j, i = self._azel_pix(az, el, dt=t - self.base_time)
         sun_delta = self.sun_times[j, i]
         sun_dists = self.sun_dist[j, i]
@@ -421,10 +428,10 @@ def find_escape_paths(self, az0, el0, t=None,
                                         dodging=False)
             paths2 = self.analyze_paths(az0, el0, 180., el1, t=t,
                                         dodging=False)
-            best_path1, decisions = select_move(paths1, {},
-                                                escape=True)
-            best_path2, decisions = select_move(paths2, {},
-                                                escape=True)
+            best_path1, decisions1 = select_move(paths1, {},
+                                                 escape=True)
+            best_path2, decisions2 = select_move(paths2, {},
+                                                 escape=True)
             paths = [bp for bp in [best_path1, best_path2] if bp is not None]
             el1 -= 1.
 

From c339ea0c60a3aa6387310db9f3a0f5dc5195beba Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Mon, 30 Oct 2023 20:21:25 +0000
Subject: [PATCH 07/21] ACU sun: create Task to handle seek_to_sunsafe

---
 socs/agents/acu/agent.py | 141 +++++++++++++++++++++++++++------------
 1 file changed, 100 insertions(+), 41 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index eee6944a5..3cbe5ac23 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -242,6 +242,10 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         agent.register_task('update_solar',
                             self.update_solar,
                             blocking=False)
+        agent.register_task('seek_sun_safety',
+                            self.seek_sun_safety,
+                            blocking=False,
+                            aborter=self._simple_task_abort)
 
         # Automatic exercise program...
         if exercise_plan:
@@ -1809,8 +1813,7 @@ def _notify_recomputed(result):
 
         req_out = False
         self.sun = None
-        state = 'init'
-        last_state = state
+        last_panic = 0
 
         session.data = {}
         session.set_status('running')
@@ -1846,19 +1849,92 @@ def _notify_recomputed(result):
                 t = self.sun.check_trajectory([az], [el])['sun_time']
                 session.data['sun_safe_time'] = t if t > 0 else 0
 
+            # Are we currently in safe position?
+            safe_known = False
+            safe = False
+            if self.sun is not None:
+                safe_known = True
+                safe = t >= 3600
+
+            # Has a drill been requested?
+            drill_req = (self.solar_params['next_drill'] is not None
+                         and self.solar_params['next_drill'] <= time.time())
+
+            # Should we be doing a seek_sun_safety?
+            panic_for_real = safe_known and not safe and self._get_sun_policy('shelter_enabled')
+            panic_for_fun = drill_req
+
+            # Is seek_sun_safe running?
+            ok, msg, _session = self.agent.status('seek_sun_safety')
+            seek_running = (_session.get('status', 'done') != 'done')
+
+            # Block motion as long as we are not sun-safe.
+            self.avoidance_lockdown = panic_for_real or seek_running
+
+            session.data.update({
+                'danger_zone': panic_for_real,
+                'lockout': self.avoidance_lockdown,
+                'seek_is_running': seek_running,
+            })
+
+            if (panic_for_real or panic_for_fun) and (time.time() - last_panic > 60.):
+                self.log.warn('solar_avoidance is requesting seek_sun_safety.')
+                self.solar_params['next_drill'] = None
+                self.agent.start('seek_sun_safety')
+                last_panic = time.time()
+
+            yield dsleep(1)
+
+    @ocs_agent.param('trigger_panic', type=bool, default=False)
+    @ocs_agent.param('shift_sun_hours', type=float, default=None)
+    @ocs_agent.param('temporary_disable', type=float, default=None)
+    def update_solar(self, session, params):
+        """update_solar()
+
+        **Task** - Update solar avoidance parameters.
+        """
+        do_recompute = False
+        now = time.time()
+        self.log.info('update_solar params: {params}',
+                      params={k: v for k, v in params.items()
+                              if v is not None})
+
+        if params['trigger_panic']:
+            self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
+            self.solar_params['next_drill'] = now + 10
+        if params['shift_sun_hours'] is not None:
+            self.solar_params['shift_sun_hours'] = params['shift_sun_hours']
+            do_recompute = True
+        if params['temporary_disable'] is not None:
+            self.solar_params['disable_until'] = params['temporary_disable'] + now
+
+        if do_recompute:
+            self.solar_params['recompute'] = True
+
+        return True, 'Params updated.'
+
+    @inlineCallbacks
+    def seek_sun_safety(self, session, params):
+        """seek_sun_safety()
+
+        **Task** - Move the platform to a Sun-Safe position.
+
+        """
+        state = 'init'
+        last_state = state
+
+        session.data = {'state': state,
+                        'timestamp': time.time()}
+        session.set_status('running')
+
+        while session.status in ['starting', 'running'] and state not in ['safe-i-guess']:
+            az, el = [self.data['status']['summary'][f'{ax}_current_position']
+                      for ax in ['Azimuth', 'Elevation']]
+
             if state == 'init':
-                if self.sun is not None:
-                    state = 'idle'
-            elif state == 'idle':
-                if t < 3600 and self._get_sun_policy('shelter_enabled'):
-                    state = 'shelter-abort'
-                tnd = self.solar_params['next_drill']
-                if tnd is not None and tnd <= time.time():
-                    state = 'shelter-abort'
-                    self.solar_params['next_drill'] = None
+                state = 'shelter-abort'
             elif state == 'shelter-abort':
                 # raise stop flags and issue stop on motion ops
-                self.avoidance_lockdown = True
                 for op in ['generate_scan', 'go_to']:
                     self.agent.stop(op)
                     self.agent.abort(op)
@@ -1871,12 +1947,14 @@ def _notify_recomputed(result):
                         break
                 else:
                     state = 'shelter-move'
+                    last_move = time.time()
                 timeout -= 1
                 if timeout < 0:
                     state = 'shelter-stop'
             elif state == 'shelter-stop':
                 yield self._stop()
                 state = 'shelter-move'
+                last_move = time.time()
             elif state == 'shelter-move':
                 paths = self.sun.find_escape_paths(az, el)
                 if len(paths) == 0:
@@ -1888,14 +1966,19 @@ def _notify_recomputed(result):
                 leg_d = None
             elif state == 'shelter-move-legs':
                 def _leg_done(result):
-                    nonlocal state
+                    nonlocal state, last_move, leg_d
                     all_ok, msg = result
                     if not all_ok:
+                        print('leg failed:', leg_az, leg_el)
                         # Recompute the escape path.
-                        state = 'shelter-move-legs'
+                        if time.time() - last_move > 60:
+                            print('giving up for now')
+                            state = 'safe-i-guess'
+                        else:
+                            state = 'shelter-move'
                     else:
-                        nonlocal leg_d
                         leg_d = None
+                        last_move = time.time()
                 if leg_d is None:
                     if len(legs) == 0:
                         state = 'safe-i-guess'
@@ -1906,8 +1989,7 @@ def _leg_done(result):
                         leg_d.addCallback(_leg_done)
 
             elif state == 'safe-i-guess':
-                self.avoidance_lockdown = False
-                state = 'idle'
+                pass
 
             session.data['state'] = state
             if state != last_state:
@@ -1915,30 +1997,7 @@ def _leg_done(result):
                 last_state = state
             yield dsleep(1)
 
-    @ocs_agent.param('trigger_panic', type=bool, default=False)
-    @ocs_agent.param('shift_sun_hours', type=float, default=None)
-    @ocs_agent.param('temporary_disable', type=float, default=None)
-    def update_solar(self, session, params):
-        """update_solar()
-
-        **Task** - Update solar avoidance parameters.
-        """
-        do_recompute = False
-        now = time.time()
-
-        if params['trigger_panic']:
-            self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
-            self.solar_params['next_drill'] = now + 10
-        if params['shift_sun_hours'] is not None:
-            self.solar_params['shift_sun_hours'] = params['shift_sun_hours']
-            do_recompute = True
-        if params['temporary_disable'] is not None:
-            self.solar_params['disable_until'] = params['temporary_disable'] + now
-
-        if do_recompute:
-            self.solar_params['recompute'] = True
-
-        return True, 'Params updated.'
+        return True, "Exited."
 
     def _get_sun_policy(self, key):
         now = time.time()

From 3d3f52bf3f589ef86261459938b3070f37086c74 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Tue, 31 Oct 2023 09:41:10 +0000
Subject: [PATCH 08/21] ACU sun: consolidate SunTracker code and organize
 policy

---
 socs/agents/acu/avoidance.py | 251 ++++++++++++++++-------------------
 1 file changed, 117 insertions(+), 134 deletions(-)

diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 4d235fda3..e422560c7 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -33,6 +33,19 @@
 NO_TIME = DAY * 2
 
 
+DEFAULT_POLICY = {
+    'exclusion_radius': 20,
+    'min_el': 0,
+    'max_el': 90,
+    'min_az': -45,
+    'max_az': 405,
+    'el_horizon': 0,
+    'el_dodging': True,
+    'min_sun_time': HOUR,
+    'response_time': HOUR * 4,
+}
+
+
 class SunTracker:
     """Provide guidance on what horizion coordinate positions are
     sun-safe.
@@ -49,13 +62,23 @@ class SunTracker:
 
     """
 
-    def __init__(self, exclusion_radius=20., map_res=.5,
-                 sun_time_shift=0., site=None, horizon=0.):
-        # Store in radians.
-        self.exclusion_radius = exclusion_radius * DEG
+    def __init__(self, policy=None, site=None,
+                 map_res=.5, sun_time_shift=0., fake_now=None,
+                 compute=True, base_time=None):
+        # Note res is stored in radians.
         self.res = map_res * DEG
-        self.horizon = horizon
         self.sun_time_shift = sun_time_shift
+        self.fake_now = fake_now
+        self.base_time = base_time
+
+        # Process and store the instrument config and safety policy.
+        if policy is None:
+            policy = {}
+        for k in policy.keys():
+            assert k in DEFAULT_POLICY
+        _p = dict(DEFAULT_POLICY)
+        _p.update(policy)
+        self.policy = _p
 
         if site is None:
             # This is close enough.
@@ -65,8 +88,9 @@ def __init__(self, exclusion_radius=20., map_res=.5,
         site_eph.lat = site.lat * DEG
         site_eph.elevation = site.elev
         self._site = site_eph
-        self.base_time = None
-        self.fake_now = None
+
+        if compute:
+            self.reset(base_time)
 
     def _now(self):
         if self.fake_now:
@@ -78,30 +102,19 @@ def _sun(self, t):
             datetime.datetime.utcfromtimestamp(t + self.sun_time_shift)
         return ephem.Sun(self._site)
 
-    def reset(self, base_time=None, staleness=None):
+    def reset(self, base_time=None):
         """Compute and store the Sun Safety Map for a specific
         timestamp.
 
         This basic computation is required prior to calling other
         functions that use the Sun Safety Map.
 
-        If staleness is provided, then the map is only updated if it
-        has not yet been computed, or if the requested base_time is
-        earlier than the base_time of the currently stored map, or if
-        the requested base_time is more than staleness seconds in the
-        future from the currently store map.
-
         """
         # Set a reference time -- the map of sun times is usable from
         # this reference time to at least 12 hours in the future.
         if base_time is None:
             base_time = self._now()
 
-        if self.base_time is not None and staleness is not None:
-            if ((base_time - self.base_time) > 0
-                    and (base_time - self.base_time) < staleness):
-                return False
-
         # Identify zenith (ra, dec) at base_time.
         Qz = coords.CelestialSightLine.naive_az_el(
             base_time, 180. * DEG, 90. * DEG).Q
@@ -132,8 +145,8 @@ def reset(self, base_time=None, staleness=None):
         dt = -ra[0] * DAY / (2 * np.pi)
         qsun = quat.rotation_lonlat(v.ra, v.dec)
         qoff = ~qsun * map_q
-        r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape)
-        sun_times[r < self.exclusion_radius] = 0.
+        r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape) / DEG
+        sun_times[r <= self.policy['exclusion_radius']] = 0.
         for g in sun_times:
             if (g < 0).all():
                 continue
@@ -151,23 +164,6 @@ def reset(self, base_time=None, staleness=None):
         self.sun_times = sun_times
         self.sun_dist = sun_dist
         self.map_q = map_q
-        return True
-
-    def _save(self, filename):
-        import pickle
-
-        # Pickle results of "reset"
-        pickle.dump((self.base_time, self.map_q, self.sun_dist.wcs,
-                     self.sun_dist, self.sun_times),
-                    open(filename, 'wb'))
-
-    def _load(self, filename):
-        import pickle
-        X = pickle.load(open(filename, 'rb'))
-        self.base_time = X[0]
-        self.sun_times = enmap.ndmap(X[4], wcs=X[2])
-        self.sun_dist = enmap.ndmap(X[3], wcs=X[2])
-        self.map_q = X[1]
 
     def _azel_pix(self, az, el, dt=0, round=True, segments=False):
         """Return the pixel indices of the Sun Safety Map that are
@@ -238,7 +234,7 @@ def check_trajectory(self, az, el, t=None, raw=False):
         sun_dists = self.sun_dist[j, i]
 
         # If sun is below horizon, rail sun_dist to 180 deg.
-        if self.get_sun_pos(t=t)['sun_azel'][1] < self.horizon:
+        if self.get_sun_pos(t=t)['sun_azel'][1] < self.policy['el_horizon']:
             sun_dists[:] = 180.
 
         if raw:
@@ -314,7 +310,7 @@ def show_map(self, axes=None, show=True):
         return fig, axes, imgs
 
     def analyze_paths(self, az0, el0, az1, el1, t=None,
-                      plot_file=None, policy=None, dodging=True):
+                      plot_file=None, dodging=True):
         """Design and analyze a number of different paths between (az0, el0)
         and (az1, el1).  Return the list, for further processing and
         choice.
@@ -341,14 +337,15 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
         }
 
         # Suitable list of test els.
+        el_lims = [self.policy[_k] for _k in ['min_el', 'max_el']]
         if el0 == el1:
             el_nodes = [el0]
         else:
             el_nodes = sorted([el0, el1])
-        if dodging and (10. < el_nodes[0]):
-            el_nodes.insert(0, 10.)
-        if dodging and (90. > el_nodes[-1]):
-            el_nodes.append(90.)
+        if dodging and (el_lims[0] < el_nodes[0]):
+            el_nodes.insert(0, el_lims[0])
+        if dodging and (el_lims[1] > el_nodes[-1]):
+            el_nodes.append(el_lims[1])
 
         el_sep = 1.
         el_cands = []
@@ -396,9 +393,7 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
                                       (segments[-1], slice(-1, None), 'x')]:
                     ax.scatter(seg[1][rng], seg[0][rng], marker=mrk, color='blue')
             # Add the selected trajectory in green.
-            selected = None
-            if policy is not None:
-                selected = select_move(all_moves, policy)[0]
+            selected = self.select_move(all_moves)[0]
             if selected is not None:
                 traj = selected['moves'].get_traj()
                 segments = self._azel_pix(*traj, round=True, segments=True)
@@ -410,7 +405,7 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
         return all_moves
 
     def find_escape_paths(self, az0, el0, t=None,
-                          plot_file=None, policy=None):
+                          plot_file=None):
         """Design and analyze a number of different paths that move from (az0,
         el0) to a sun safe position.  Return the list, for further
         processing and choice.
@@ -419,24 +414,88 @@ def find_escape_paths(self, az0, el0, t=None,
         if t is None:
             t = self._now()
 
+        az_cands = []
+        _az = math.ceil(self.policy['min_az'] / 180) * 180
+        while _az <= self.policy['max_az']:
+            az_cands.append(_az)
+            _az += 180.
+
         # Preference is to not change altitude.  But we may need to
         # lower it.
         el1 = el0
         paths = []
-        while len(paths) == 0 and el1 > 0:
-            paths1 = self.analyze_paths(az0, el0, 0., el1, t=t,
-                                        dodging=False)
-            paths2 = self.analyze_paths(az0, el0, 180., el1, t=t,
-                                        dodging=False)
-            best_path1, decisions1 = select_move(paths1, {},
-                                                 escape=True)
-            best_path2, decisions2 = select_move(paths2, {},
-                                                 escape=True)
-            paths = [bp for bp in [best_path1, best_path2] if bp is not None]
+        while len(paths) == 0 and el1 >= self.policy['min_el']:
+            paths = [self.analyze_paths(az0, el0, _az, el1, t=t, dodging=False)
+                     for _az in az_cands]
+            best_paths = [self.select_move(p, escape=True)[0] for p in paths]
+            paths = [bp for bp in best_paths if bp is not None]
             el1 -= 1.
 
         return paths
 
+    def select_move(self, moves, escape=False):
+        _p = self.policy
+
+        decisions = [{'rejected': False,
+                      'reason': None} for m in moves]
+
+        def reject(d, reason):
+            d['rejected'] = True
+            d['reason'] = reason
+
+        # According to policy, reject moves outright.
+        for m, d in zip(moves, decisions):
+            if d['rejected']:
+                continue
+
+            els = m['req_start'][1], m['req_stop'][1]
+
+            if escape and (m['sun_time_start'] < _p['min_sun_time']):
+                if m['sun_dist_min'] < m['sun_dist_start']:
+                    reject(d, 'Path moves even closer to sun.')
+                    continue
+                if m['sun_time_stop'] < _p['min_sun_time']:
+                    reject(d, 'Path does not end in sun-safe location.')
+                    continue
+            else:
+                if m['sun_time'] < _p['min_sun_time']:
+                    reject(d, 'Path too close to sun.')
+                    continue
+
+            if m['travel_el'] < _p['min_el']:
+                reject(d, 'Path goes below minimum el.')
+                continue
+
+            if m['travel_el'] > _p['max_el']:
+                reject(d, 'Path goes above maximum el.')
+                continue
+
+            if not _p['el_dodging']:
+                if m['travel_el'] < min(*els):
+                    reject(d, 'Path dodges (goes below necessary el range).')
+                    continue
+                if m['travel_el'] > max(*els):
+                    reject(d, 'Path dodges (goes above necessary el range).')
+
+        cands = [m for m, d in zip(moves, decisions)
+                 if not d['rejected']]
+        if len(cands) == 0:
+            return None, decisions
+
+        def priority_func(m):
+            # Sorting key for move proposals.
+            els = m['req_start'][1], m['req_stop'][1]
+            return (
+                m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'],
+                m['direct'],
+                m['sun_dist_min'],
+                m['sun_dist_mean'],
+                -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])),
+                m['travel_el'],
+            )
+        cands.sort(key=priority_func)
+        return cands[-1], decisions
+
 
 class MoveSequence:
     def __init__(self, *args, simplify=False):
@@ -481,79 +540,3 @@ def get_traj(self, res=0.5):
             xx.append(np.linspace(x0, x1, n))
             yy.append(np.linspace(y0, y1, n))
         return np.hstack(tuple(xx)), np.hstack(tuple(yy))
-
-
-DEFAULT_POLICY = {
-    'min_el': 0,
-    'max_el': 90,
-    'el_dodging': True,
-    'min_sun_time': HOUR,
-    'response_time': HOUR * 4,
-}
-
-
-def select_move(moves, policy, escape=False):
-    for k in policy.keys():
-        assert k in DEFAULT_POLICY
-    _p = dict(DEFAULT_POLICY)
-    _p.update(policy)
-
-    decisions = [{'rejected': False,
-                  'reason': None} for m in moves]
-
-    def reject(d, reason):
-        d['rejected'] = True
-        d['reason'] = reason
-
-    # According to policy, reject moves outright.
-    for m, d in zip(moves, decisions):
-        if d['rejected']:
-            continue
-
-        els = m['req_start'][1], m['req_stop'][1]
-
-        if escape and (m['sun_time_start'] < _p['min_sun_time']):
-            if m['sun_dist_min'] < m['sun_dist_start']:
-                reject(d, 'Path moves even closer to sun.')
-                continue
-            if m['sun_time_stop'] < _p['min_sun_time']:
-                reject(d, 'Path does not end in sun-safe location.')
-                continue
-        else:
-            if m['sun_time'] < _p['min_sun_time']:
-                reject(d, 'Path too close to sun.')
-                continue
-
-        if m['travel_el'] < _p['min_el']:
-            reject(d, 'Path goes below minimum el.')
-            continue
-
-        if m['travel_el'] > _p['max_el']:
-            reject(d, 'Path goes above maximum el.')
-            continue
-
-        if not _p['el_dodging']:
-            if m['travel_el'] < min(*els):
-                reject(d, 'Path dodges (goes below necessary el range).')
-                continue
-            if m['travel_el'] > max(*els):
-                reject(d, 'Path dodges (goes above necessary el range).')
-
-    cands = [m for m, d in zip(moves, decisions)
-             if not d['rejected']]
-    if len(cands) == 0:
-        return None, decisions
-
-    def priority_func(m):
-        # Sorting key for move proposals.
-        els = m['req_start'][1], m['req_stop'][1]
-        return (
-            m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'],
-            m['direct'],
-            m['sun_dist_min'],
-            m['sun_dist_mean'],
-            -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])),
-            m['travel_el'],
-        )
-    cands.sort(key=priority_func)
-    return cands[-1], decisions

From c6bccefd54ac9197535de17c5be7f61ca19dec37 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Tue, 31 Oct 2023 12:08:32 +0000
Subject: [PATCH 09/21] ACU sun: more clean up; catch edge cases

---
 socs/agents/acu/agent.py     | 170 +++++++++++++++++++++++------------
 socs/agents/acu/avoidance.py |  18 ++--
 2 files changed, 127 insertions(+), 61 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 3cbe5ac23..d95d00e8e 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -40,6 +40,21 @@
 }
 
 
+#: Default Sun avoidance params by platform type (enabled, policy)
+SUN_POLICY = {
+    'ccat': (False, {}),
+    'satp': (True, {
+        'exclusion_radius': 20,
+        'el_horizon': 20,
+        'min_sun_time': 1800,
+        'response_time': 7200,
+    }),
+}
+
+#: How often to refresh to Sun Safety map (valid up to 2x this time)
+SUN_MAP_REFRESH = 6 * avoidance.HOUR
+
+
 class ACUAgent:
     """Agent to acquire data from an ACU and control telescope pointing with the
     ACU.
@@ -105,20 +120,7 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         if len(self.ignore_axes):
             agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes)
 
-        self.solar_params = {
-            'active_avoidance': False,
-            'radius': 0,
-            'next_drill': None,
-            'shift_sun_hours': 0,
-            'do_recompute': False,
-            'disable_until': 0,
-        }
-        if solar_avoidance is not None and solar_avoidance > 0:
-            self.solar_params.update({
-                'active_avoidance': True,
-                'radius': solar_avoidance,
-            })
-        self.avoidance_lockdown = False
+        self.reset_sun_params()
 
         self.exercise_plan = exercise_plan
 
@@ -1187,8 +1189,8 @@ def go_to(self, session, params):
             if not acquired:
                 return False, f"Operation failed: {self.azel_lock.job} is running."
 
-            if self.avoidance_lockdown:
-                return False, "Motion blocked; avoidance in progress."
+            if self._get_sun_policy('motion_blocked'):
+                return False, "Motion blocked; Sun avoidance in progress."
 
             self.log.info('Clearing faults to prepare for motion.')
             yield self.acu_control.clear_faults()
@@ -1528,8 +1530,8 @@ def generate_scan(self, session, params):
           Process .stop method is called)..
 
         """
-        if self.avoidance_lockdown:
-            return False, "Motion blocked; avoidance in progress."
+        if self._get_sun_policy('motion_blocked'):
+            return False, "Motion blocked; Sun avoidance in progress."
 
         self.log.info('User scan params: {params}', params=params)
 
@@ -1784,6 +1786,69 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
     # Sun Safety and Solar Avoidance
     #
 
+    def reset_sun_params(self):
+        _p = {
+            # Global enable (but see "disable_until").
+            'active_avoidance': False,
+
+            # Can be set to a timestamp, in which case Sun Avoidance
+            # is disabled until that time has passed.
+            'disable_until': 0,
+
+            # Flag for indicating normal motions should be blocked
+            # (Sun Escape is active).
+            'block_motion': False,
+
+            # Flag for update_solar to indicate Sun map needs recomputed
+            'recompute_req': False,
+
+            # If set, should be a timestamp at which a seek_to_safe
+            # should be initiated.
+            'next_drill': None,
+
+            # Parameters for the Sun Safety Map computation.
+            'safety_map_kw': {
+                'sun_time_shift': 0,
+            },
+
+            # Avoidance policy, for use in avoidance decisions.
+            'policy': None,
+        }
+
+        # Populate default policy based on platform.
+        _enabled, _policy = SUN_POLICY[self.acu_config['platform']]
+        _p['active_avoidance'] = _enabled
+        _p['policy'] = dict(_policy)
+
+        # And add in platform limits
+        _p['policy'].update({
+            'min_az': self.motion_limits['azimuth']['lower'],
+            'max_az': self.motion_limits['azimuth']['upper'],
+            'min_el': self.motion_limits['elevation']['lower'],
+            'max_el': self.motion_limits['elevation']['upper'],
+        })
+
+        self.sun_params = _p
+
+    def _get_sun_policy(self, key):
+        now = time.time()
+        p = self.sun_params
+        active = (p['active_avoidance'] and (now >= p['disable_until']))
+
+        if key == 'motion_blocked':
+            return active and p['block_motion']
+        elif key == 'sunsafe_moves':
+            return active
+        elif key == 'shelter_enabled':
+            return active
+        elif key == 'map_valid':
+            return (self.sun is not None
+                    and self.sun.base_time is not None
+                    and self.sun.base_time <= now
+                    and self.sun.base_time >= now - 2 * SUN_MAP_REFRESH)
+        else:
+            return p[key]
+
     @inlineCallbacks
     def solar_avoidance(self, session, params):
         """solar_avoidance()
@@ -1799,8 +1864,8 @@ def solar_avoidance(self, session, params):
         def _get_sun_map():
             # To run in thread ...
             start = time.time()
-            new_sun = avoidance.SunTracker(sun_time_shift=self.solar_params['shift_sun_hours'] * 3600.)
-            new_sun.reset()
+            new_sun = avoidance.SunTracker(policy=self.sun_params['policy'],
+                                           **self.sun_params['safety_map_kw'])
             return new_sun, time.time() - start
 
         def _notify_recomputed(result):
@@ -1831,15 +1896,15 @@ def _notify_recomputed(result):
 
             no_map = self.sun is None
             old_map = (not no_map
-                       and self.sun._now() - self.sun.base_time > 12 * avoidance.HOUR)
+                       and self.sun._now() - self.sun.base_time > SUN_MAP_REFRESH)
             do_recompute = (
                 not req_out
-                and (no_map or old_map or self.solar_params['recompute'])
+                and (no_map or old_map or self.sun_params['recompute_req'])
             )
 
             if do_recompute:
                 req_out = True
-                self.solar_params['recompute'] = False
+                self.sun_params['recompute_req'] = False
                 threads.deferToThread(_get_sun_map).addCallback(
                     _notify_recomputed)
 
@@ -1854,11 +1919,11 @@ def _notify_recomputed(result):
             safe = False
             if self.sun is not None:
                 safe_known = True
-                safe = t >= 3600
+                safe = t >= self.sun_params['policy']['min_sun_time']
 
             # Has a drill been requested?
-            drill_req = (self.solar_params['next_drill'] is not None
-                         and self.solar_params['next_drill'] <= time.time())
+            drill_req = (self.sun_params['next_drill'] is not None
+                         and self.sun_params['next_drill'] <= time.time())
 
             # Should we be doing a seek_sun_safety?
             panic_for_real = safe_known and not safe and self._get_sun_policy('shelter_enabled')
@@ -1869,22 +1934,23 @@ def _notify_recomputed(result):
             seek_running = (_session.get('status', 'done') != 'done')
 
             # Block motion as long as we are not sun-safe.
-            self.avoidance_lockdown = panic_for_real or seek_running
+            self.sun_params['block_motion'] = (panic_for_real or seek_running)
 
             session.data.update({
                 'danger_zone': panic_for_real,
-                'lockout': self.avoidance_lockdown,
+                'motion_blocked': self.sun_params['block_motion'],
                 'seek_is_running': seek_running,
             })
 
             if (panic_for_real or panic_for_fun) and (time.time() - last_panic > 60.):
                 self.log.warn('solar_avoidance is requesting seek_sun_safety.')
-                self.solar_params['next_drill'] = None
+                self.sun_params['next_drill'] = None
                 self.agent.start('seek_sun_safety')
                 last_panic = time.time()
 
             yield dsleep(1)
 
+    @ocs_agent.param('reset', type=bool, default=False)
     @ocs_agent.param('trigger_panic', type=bool, default=False)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     @ocs_agent.param('temporary_disable', type=float, default=None)
@@ -1899,17 +1965,21 @@ def update_solar(self, session, params):
                       params={k: v for k, v in params.items()
                               if v is not None})
 
+        if params['reset']:
+            self.reset_sun_params()
+            do_recompute = True
         if params['trigger_panic']:
             self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
-            self.solar_params['next_drill'] = now + 10
+            self.sun_params['next_drill'] = now + 10
         if params['shift_sun_hours'] is not None:
-            self.solar_params['shift_sun_hours'] = params['shift_sun_hours']
+            self.sun_params['safety_map_kw']['sun_time_shift'] = \
+                params['shift_sun_hours'] * 3600
             do_recompute = True
         if params['temporary_disable'] is not None:
-            self.solar_params['disable_until'] = params['temporary_disable'] + now
+            self.sun_params['disable_until'] = params['temporary_disable'] + now
 
         if do_recompute:
-            self.solar_params['recompute'] = True
+            self.sun_params['recompute_req'] = True
 
         return True, 'Params updated.'
 
@@ -1956,12 +2026,13 @@ def seek_sun_safety(self, session, params):
                 state = 'shelter-move'
                 last_move = time.time()
             elif state == 'shelter-move':
-                paths = self.sun.find_escape_paths(az, el)
-                if len(paths) == 0:
-                    print('failed to find escape paths @%.1f, az=%.3f el=%.3f' %
+                escape_path = self.sun.find_escape_paths(az, el)
+                if escape_path is None:
+                    print('failed to find escape path @%.1f, az=%.3f el=%.3f' %
                           (time.time(), az, el))
-
-                legs = paths[0]['moves'].nodes[1:]
+                else:
+                    print('escaping to', escape_path['moves'].nodes[-1])
+                legs = escape_path['moves'].nodes[1:]
                 state = 'shelter-move-legs'
                 leg_d = None
             elif state == 'shelter-move-legs':
@@ -1979,6 +2050,8 @@ def _leg_done(result):
                     else:
                         leg_d = None
                         last_move = time.time()
+                    if not self._get_sun_policy('shelter_enabled'):
+                        state = 'safe-i-guess'
                 if leg_d is None:
                     if len(legs) == 0:
                         state = 'safe-i-guess'
@@ -1999,18 +2072,6 @@ def _leg_done(result):
 
         return True, "Exited."
 
-    def _get_sun_policy(self, key):
-        now = time.time()
-        p = self.solar_params
-
-        if key == 'sunsafe_moves':
-            return p['active_avoidance'] and (now >= p['disable_until'])
-        elif key == 'shelter_enabled':
-            return p['active_avoidance'] and (now >= p['disable_until'])
-
-        else:
-            return p[key]
-
     def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az):
         # Include a bit of buffer for turn-arounds.
         az1, az2 = min(az1, az2), max(az1, az2)
@@ -2021,7 +2082,7 @@ def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az):
         azs = np.linspace(az1, az2, n)
 
         info = self.sun.check_trajectory(azs, azs * 0 + el)
-        safe = info['sun_time'] >= 3600
+        safe = info['sun_time'] >= self.sun_params['policy']['min_sun_time']
         if safe:
             msg = 'Scan is safe for %.1f hours' % (info['sun_time'] / 3600)
         else:
@@ -2036,9 +2097,8 @@ def _get_sunsafe_moves(self, target_az, target_el):
         if not self._get_sun_policy('sunsafe_moves'):
             return [(target_az, target_el)], None
 
-        if self.sun is None or self.sun.base_time is None:
-            return None, 'Sun Safety Map not computed; run the solar_avoidance process.'
-        # check for staleness!
+        if not self._get_sun_policy('map_valid'):
+            return None, 'Sun Safety Map not computed or stale; run the solar_avoidance process.'
 
         # Check the target position and block it outright.
         if self.sun.check_trajectory([target_az], [target_el])['sun_time'] <= 0:
@@ -2054,7 +2114,7 @@ def _get_sunsafe_moves(self, target_az, target_el):
             return None, 'Current position could not be determined.'
 
         moves = self.sun.analyze_paths(az, el, target_az, target_el)
-        move, decisions = avoidance.select_move(moves, {})
+        move, decisions = self.sun.select_move(moves)
         if move is None:
             return None, 'No Sun-Safe moves could be identified!'
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index e422560c7..b72ac8587 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -40,7 +40,7 @@
     'min_az': -45,
     'max_az': 405,
     'el_horizon': 0,
-    'el_dodging': True,
+    'el_dodging': False,
     'min_sun_time': HOUR,
     'response_time': HOUR * 4,
 }
@@ -423,15 +423,17 @@ def find_escape_paths(self, az0, el0, t=None,
         # Preference is to not change altitude.  But we may need to
         # lower it.
         el1 = el0
-        paths = []
-        while len(paths) == 0 and el1 >= self.policy['min_el']:
+        path = None
+        while path is None and el1 >= self.policy['min_el']:
             paths = [self.analyze_paths(az0, el0, _az, el1, t=t, dodging=False)
                      for _az in az_cands]
             best_paths = [self.select_move(p, escape=True)[0] for p in paths]
-            paths = [bp for bp in best_paths if bp is not None]
+            best_paths = [p for p in best_paths if p is not None]
+            if len(best_paths):
+                path = self.select_move(best_paths, escape=True)[0]
             el1 -= 1.
 
-        return paths
+        return path
 
     def select_move(self, moves, escape=False):
         _p = self.policy
@@ -451,7 +453,9 @@ def reject(d, reason):
             els = m['req_start'][1], m['req_stop'][1]
 
             if escape and (m['sun_time_start'] < _p['min_sun_time']):
-                if m['sun_dist_min'] < m['sun_dist_start']:
+                # Test > res, rather than > 0... near the minimum this
+                # can be noisy.
+                if m['sun_dist_start'] - m['sun_dist_min'] > self.res / DEG:
                     reject(d, 'Path moves even closer to sun.')
                     continue
                 if m['sun_time_stop'] < _p['min_sun_time']:
@@ -484,6 +488,7 @@ def reject(d, reason):
 
         def priority_func(m):
             # Sorting key for move proposals.
+            azs = m['req_start'][0], m['req_stop'][0]
             els = m['req_start'][1], m['req_stop'][1]
             return (
                 m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'],
@@ -491,6 +496,7 @@ def priority_func(m):
                 m['sun_dist_min'],
                 m['sun_dist_mean'],
                 -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])),
+                -abs(azs[1] - azs[0]),
                 m['travel_el'],
             )
         cands.sort(key=priority_func)

From 39146f7a06646a9d299a7856c8bfd2084ad8b9f3 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Tue, 31 Oct 2023 20:53:44 +0000
Subject: [PATCH 10/21] ACU sun: args, more

---
 socs/agents/acu/agent.py     | 103 ++++++++++++++++++++++++++---------
 socs/agents/acu/avoidance.py |  50 +++++++++++++++--
 2 files changed, 122 insertions(+), 31 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index d95d00e8e..84720cc39 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -41,14 +41,20 @@
 
 
 #: Default Sun avoidance params by platform type (enabled, policy)
-SUN_POLICY = {
-    'ccat': (False, {}),
-    'satp': (True, {
-        'exclusion_radius': 20,
-        'el_horizon': 20,
-        'min_sun_time': 1800,
-        'response_time': 7200,
-    }),
+SUN_CONFIGS = {
+    'ccat': {
+        'enabled': False,
+        'policy': {},
+    },
+    'satp': {
+        'enabled': True,
+        'policy': {
+            'exclusion_radius': 20,
+            'el_horizon': 10,
+            'min_sun_time': 1800,
+            'response_time': 7200,
+        },
+    },
 }
 
 #: How often to refresh to Sun Safety map (valid up to 2x this time)
@@ -78,13 +84,24 @@ class ACUAgent:
             list should be drawn from "az", "el", and "third".
         disable_idle_reset (bool):
             If True, don't auto-start idle_reset process for LAT.
-        solar_avoidance (float):
+        min_el (float): If not None, override the default configured
+            elevation lower limit.
+        max_el (float): If not None, override the default configured
+            elevation upper limit.
+        solar_avoidance (bool): If set, override the default Sun
+            avoidance setting (i.e. force enable or disable the feature).
+        avoidance_radius (float): If set, override the default Sun
+            avoidance radius (i.e. the radius of the field of view, in
+            degrees, to use for Sun avoidance purposes).
 
     """
 
     def __init__(self, agent, acu_config='guess', exercise_plan=None,
                  startup=False, ignore_axes=None, disable_idle_reset=False,
-                 solar_avoidance=None):
+                 min_el=None, max_el=None,
+                 sun_avoidance=None, avoidance_radius=None):
+        self.log = agent.log
+
         # Separate locks for exclusive access to az/el, and boresight motions.
         self.azel_lock = TimeoutLock()
         self.boresight_lock = TimeoutLock()
@@ -103,6 +120,13 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         self.monitor_fields = status_keys.status_fields[self.acu_config['platform']]['status_fields']
         self.motion_limits = self.acu_config['motion_limits']
 
+        if min_el:
+            self.log.warn(f'Override: min_el={min_el}')
+            self.motion_limits['elevation']['lower'] = min_el
+        if max_el:
+            self.log.warn(f'Override: max_el={max_el}')
+            self.motion_limits['elevation']['upper'] = max_el
+
         # This initializes self.scan_params; these become the default
         # scan params when calling generate_scan.  They can be changed
         # during run time; they can also be overridden when calling
@@ -120,12 +144,11 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         if len(self.ignore_axes):
             agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes)
 
-        self.reset_sun_params()
+        self.reset_sun_params(enabled=sun_avoidance,
+                              radius=avoidance_radius)
 
         self.exercise_plan = exercise_plan
 
-        self.log = agent.log
-
         # self.data provides a place to reference data from the monitors.
         # 'status' is populated by the monitor operation
         # 'broadcast' is populated by the udp_monitor operation
@@ -1786,7 +1809,15 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
     # Sun Safety and Solar Avoidance
     #
 
-    def reset_sun_params(self):
+    def reset_sun_params(self, enabled=None, radius=None):
+        config = SUN_CONFIGS[self.acu_config['platform']]
+
+        # These params update config for the entire run of agent.
+        if enabled is not None:
+            config['enabled'] = enabled
+        if radius is not None:
+            config['policy']['exclusion_radius'] = radius
+
         _p = {
             # Global enable (but see "disable_until").
             'active_avoidance': False,
@@ -1816,9 +1847,8 @@ def reset_sun_params(self):
         }
 
         # Populate default policy based on platform.
-        _enabled, _policy = SUN_POLICY[self.acu_config['platform']]
-        _p['active_avoidance'] = _enabled
-        _p['policy'] = dict(_policy)
+        _p['active_avoidance'] = config['enabled']
+        _p['policy'] = config['policy']
 
         # And add in platform limits
         _p['policy'].update({
@@ -1950,10 +1980,12 @@ def _notify_recomputed(result):
 
             yield dsleep(1)
 
-    @ocs_agent.param('reset', type=bool, default=False)
-    @ocs_agent.param('trigger_panic', type=bool, default=False)
+    @ocs_agent.param('reset', type=bool, default=None)
+    @ocs_agent.param('enable', type=bool, default=None)
+    @ocs_agent.param('trigger_panic', type=bool, default=None)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     @ocs_agent.param('temporary_disable', type=float, default=None)
+    @ocs_agent.param('avoidance_radius', type=float, default=None)
     def update_solar(self, session, params):
         """update_solar()
 
@@ -1968,6 +2000,12 @@ def update_solar(self, session, params):
         if params['reset']:
             self.reset_sun_params()
             do_recompute = True
+
+        if params['enable'] is not None:
+            self.sun_params['active_avoidance'] = params['enable']
+            self.sun_params['disable_until'] = 0
+        if params['temporary_disable'] is not None:
+            self.sun_params['disable_until'] = params['temporary_disable'] + now
         if params['trigger_panic']:
             self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
             self.sun_params['next_drill'] = now + 10
@@ -1975,8 +2013,10 @@ def update_solar(self, session, params):
             self.sun_params['safety_map_kw']['sun_time_shift'] = \
                 params['shift_sun_hours'] * 3600
             do_recompute = True
-        if params['temporary_disable'] is not None:
-            self.sun_params['disable_until'] = params['temporary_disable'] + now
+        if params['avoidance_radius'] is not None:
+            self.sun_params['policy']['exclusion_radius'] = \
+                params['avoidance_radius']
+            do_recompute = True
 
         if do_recompute:
             self.sun_params['recompute_req'] = True
@@ -2255,9 +2295,16 @@ def add_agent_args(parser_in=None):
                         nargs='+', help="One or more axes to ignore.")
     pgroup.add_argument("--disable-idle-reset", action='store_true',
                         help="Disable idle_reset, even for LAT.")
-    pgroup.add_argument("--solar-avoidance", type=float,
-                        help="If set (and > 0), enable active solar avoidance "
-                        "using this radius (in deg) for the exclusion zone.")
+    pgroup.add_argument("--min-el", type=float,
+                        help="Override the minimum el defined in platform config.")
+    pgroup.add_argument("--max-el", type=float,
+                        help="Override the maximum el defined in platform config.")
+    pgroup.add_argument("--sun-avoidance", type=int,
+                        help="Pass 0 or 1 to enable or disable sun-avoidance. "
+                        "Overrides the platform default config.")
+    pgroup.add_argument("--avoidance-radius", type=float,
+                        help="Override the default focal plane radius for "
+                        "Sun avoidance purposes.  Setting to zero disables.")
     return parser_in
 
 
@@ -2266,12 +2313,18 @@ def main(args=None):
     args = site_config.parse_args(agent_class='ACUAgent',
                                   parser=parser,
                                   args=args)
+    avoidance = (None if args.sun_avoidance is None
+                 else (args.sun_avoidance != 0))
+
     agent, runner = ocs_agent.init_site_agent(args)
     _ = ACUAgent(agent, args.acu_config, args.exercise_plan,
                  startup=not args.no_processes,
                  ignore_axes=args.ignore_axes,
                  disable_idle_reset=args.disable_idle_reset,
-                 solar_avoidance=args.solar_avoidance)
+                 sun_avoidance=avoidance,
+                 avoidance_radius=args.avoidance_radius,
+                 min_el=args.min_el,
+                 max_el=args.max_el)
 
     runner.run(agent, auto_reconnect=True)
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index b72ac8587..8168ce771 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -420,20 +420,25 @@ def find_escape_paths(self, az0, el0, t=None,
             az_cands.append(_az)
             _az += 180.
 
-        # Preference is to not change altitude.  But we may need to
-        # lower it.
-        el1 = el0
+        # Clip el0 into the allowed range.
+        el0 = np.clip(el0, self.policy['min_el'], self.policy['max_el'])
+
+        # Preference is to not change altitude; but allow for lowering.
+        n_els = math.ceil(el0 - self.policy['min_el']) + 1
+        els = np.linspace(el0, self.policy['min_el'], n_els)
+
         path = None
-        while path is None and el1 >= self.policy['min_el']:
+        for el1 in els:
             paths = [self.analyze_paths(az0, el0, _az, el1, t=t, dodging=False)
                      for _az in az_cands]
             best_paths = [self.select_move(p, escape=True)[0] for p in paths]
             best_paths = [p for p in best_paths if p is not None]
             if len(best_paths):
                 path = self.select_move(best_paths, escape=True)[0]
-            el1 -= 1.
+            if path is not None:
+                return path
 
-        return path
+        return None
 
     def select_move(self, moves, escape=False):
         _p = self.policy
@@ -546,3 +551,36 @@ def get_traj(self, res=0.5):
             xx.append(np.linspace(x0, x1, n))
             yy.append(np.linspace(y0, y1, n))
         return np.hstack(tuple(xx)), np.hstack(tuple(yy))
+
+
+class RollingMinimum:
+    def __init__(self, window, fallback=None):
+        self.window = window
+        self.subwindow = window / 10
+        self.fallback = fallback
+        self.records = []
+
+    def append(self, val, t=None):
+        if t is None:
+            t = time.time()
+        # Remove old data
+        while len(self.records) and (t - self.records[0][0]) > self.window:
+            self.records.pop(0)
+        # Add this to existing subwindow?
+        if len(self.records):
+            # Consider values up to subwindow ago.
+            _t, _val = self.records[-1]
+            if t - _t < self.subwindow:
+                if val <= _val:
+                    self.records[-1] = (t, val)
+                return
+        # Or start a new subwindow.
+        self.records.append((t, val))
+
+    def get(self, lookback=None):
+        if lookback is None:
+            lookback = self.window
+        recs = [v for t, v in self.records if (time.time() - t < lookback)]
+        if len(recs):
+            return min(recs)
+        return self.fallback

From 63f4c5ce5cf5f3e2b3c4e72504f27c82864bc0f9 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 00:54:12 +0000
Subject: [PATCH 11/21] ACU sun: consistencyize, reterminologize.

---
 socs/agents/acu/agent.py     | 220 +++++++++++++++++++++--------------
 socs/agents/acu/avoidance.py |   2 +-
 2 files changed, 136 insertions(+), 86 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 84720cc39..0c61c50a8 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -88,9 +88,9 @@ class ACUAgent:
             elevation lower limit.
         max_el (float): If not None, override the default configured
             elevation upper limit.
-        solar_avoidance (bool): If set, override the default Sun
+        avoid_sun (bool): If set, override the default Sun
             avoidance setting (i.e. force enable or disable the feature).
-        avoidance_radius (float): If set, override the default Sun
+        fov_radius (float): If set, override the default Sun
             avoidance radius (i.e. the radius of the field of view, in
             degrees, to use for Sun avoidance purposes).
 
@@ -99,7 +99,7 @@ class ACUAgent:
     def __init__(self, agent, acu_config='guess', exercise_plan=None,
                  startup=False, ignore_axes=None, disable_idle_reset=False,
                  min_el=None, max_el=None,
-                 sun_avoidance=None, avoidance_radius=None):
+                 avoid_sun=None, fov_radius=None):
         self.log = agent.log
 
         # Separate locks for exclusive access to az/el, and boresight motions.
@@ -144,8 +144,8 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         if len(self.ignore_axes):
             agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes)
 
-        self.reset_sun_params(enabled=sun_avoidance,
-                              radius=avoidance_radius)
+        self.reset_sun_params(enabled=avoid_sun,
+                              radius=fov_radius)
 
         self.exercise_plan = exercise_plan
 
@@ -191,8 +191,8 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
                                self._simple_process_stop,
                                blocking=False,
                                startup=startup)
-        agent.register_process('solar_avoidance',
-                               self.solar_avoidance,
+        agent.register_process('monitor_sun',
+                               self.monitor_sun,
                                self._simple_process_stop,
                                blocking=False,
                                startup=startup)
@@ -264,11 +264,11 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         agent.register_task('clear_faults',
                             self.clear_faults,
                             blocking=False)
-        agent.register_task('update_solar',
-                            self.update_solar,
+        agent.register_task('update_sun',
+                            self.update_sun,
                             blocking=False)
-        agent.register_task('seek_sun_safety',
-                            self.seek_sun_safety,
+        agent.register_task('escape_sun_now',
+                            self.escape_sun_now,
                             blocking=False,
                             aborter=self._simple_task_abort)
 
@@ -1806,10 +1806,15 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
         return True, 'Scan ended cleanly'
 
     #
-    # Sun Safety and Solar Avoidance
+    # Sun Safety Monitoring and Active Avoidance
     #
 
     def reset_sun_params(self, enabled=None, radius=None):
+        """Resets self.sun_params based on defaults for this platform.  Note
+        if enabled or radius are specified here, they update the
+        defaults (so they endure for the life of the agent).
+
+        """
         config = SUN_CONFIGS[self.acu_config['platform']]
 
         # These params update config for the entire run of agent.
@@ -1830,11 +1835,11 @@ def reset_sun_params(self, enabled=None, radius=None):
             # (Sun Escape is active).
             'block_motion': False,
 
-            # Flag for update_solar to indicate Sun map needs recomputed
+            # Flag for update_sun to indicate Sun map needs recomputed
             'recompute_req': False,
 
-            # If set, should be a timestamp at which a seek_to_safe
-            # should be initiated.
+            # If set, should be a timestamp at which escape_sun_now
+            # will be initiated.
             'next_drill': None,
 
             # Parameters for the Sun Safety Map computation.
@@ -1869,7 +1874,7 @@ def _get_sun_policy(self, key):
             return active and p['block_motion']
         elif key == 'sunsafe_moves':
             return active
-        elif key == 'shelter_enabled':
+        elif key == 'escape_enabled':
             return active
         elif key == 'map_valid':
             return (self.sun is not None
@@ -1879,18 +1884,25 @@ def _get_sun_policy(self, key):
         else:
             return p[key]
 
+    @ocs_agent.param('_')
     @inlineCallbacks
-    def solar_avoidance(self, session, params):
-        """solar_avoidance()
+    def monitor_sun(self, session, params):
+        """monitor_sun()
 
-        **Process** - Avoid the Sun.
+        **Process** - Monitors and reports the position of the Sun;
+        maintains a Sun Safety Map for verifying that moves and scans
+        are Sun-safe; triggers a "Sun escape" if the boresight enters
+        an unsafe position.
 
-        """
-        # The main jobs of this process are:
-        # - track Sun and report its position
-        # - maintain a Sun Safety map for other ops to query.
-        # - guide the platform to Sun Safe position, if needed.
+        The monitoring functions are always active (as long as this
+        process is running).  But the escape functionality must be
+        explicitly enabled (through the default platform
+        configuration, command line arguments, or the update_sun
+        task).
 
+        Session data contains lots of good stuff::
+
+        """
         def _get_sun_map():
             # To run in thread ...
             start = time.time()
@@ -1955,79 +1967,103 @@ def _notify_recomputed(result):
             drill_req = (self.sun_params['next_drill'] is not None
                          and self.sun_params['next_drill'] <= time.time())
 
-            # Should we be doing a seek_sun_safety?
-            panic_for_real = safe_known and not safe and self._get_sun_policy('shelter_enabled')
+            # Should we be doing a escape_sun_now?
+            panic_for_real = safe_known and not safe and self._get_sun_policy('escape_enabled')
             panic_for_fun = drill_req
 
-            # Is seek_sun_safe running?
-            ok, msg, _session = self.agent.status('seek_sun_safety')
-            seek_running = (_session.get('status', 'done') != 'done')
+            # Is escape_sun_now task running?
+            ok, msg, _session = self.agent.status('escape_sun_now')
+            escape_in_progress = (_session.get('status', 'done') != 'done')
 
             # Block motion as long as we are not sun-safe.
-            self.sun_params['block_motion'] = (panic_for_real or seek_running)
+            self.sun_params['block_motion'] = (panic_for_real or escape_in_progress)
 
             session.data.update({
                 'danger_zone': panic_for_real,
                 'motion_blocked': self.sun_params['block_motion'],
-                'seek_is_running': seek_running,
+                'escape_in_progress': escape_in_progress,
             })
 
             if (panic_for_real or panic_for_fun) and (time.time() - last_panic > 60.):
-                self.log.warn('solar_avoidance is requesting seek_sun_safety.')
+                self.log.warn('monitor_sun is requesting escape_sun_now.')
                 self.sun_params['next_drill'] = None
-                self.agent.start('seek_sun_safety')
+                self.agent.start('escape_sun_now')
                 last_panic = time.time()
 
             yield dsleep(1)
 
     @ocs_agent.param('reset', type=bool, default=None)
     @ocs_agent.param('enable', type=bool, default=None)
-    @ocs_agent.param('trigger_panic', type=bool, default=None)
-    @ocs_agent.param('shift_sun_hours', type=float, default=None)
     @ocs_agent.param('temporary_disable', type=float, default=None)
+    @ocs_agent.param('escape', type=bool, default=None)
     @ocs_agent.param('avoidance_radius', type=float, default=None)
-    def update_solar(self, session, params):
-        """update_solar()
+    @ocs_agent.param('shift_sun_hours', type=float, default=None)
+    def update_sun(self, session, params):
+        """update_sun()
+
+        **Task** - Update Sun monitoring and avoidance parameters.
+
+        Args:
+
+          reset (bool): If True, reset all sun_params to the platform
+            defaults.  (The "defaults" includes any overrides
+            specified on Agent command line.)
+          enable (bool): If True, enable active Sun avoidance.  If
+            avoidance was temporarily disable,d it is re-enabled.  If
+            False, disable active Sun avoidance (non-temporarily).
+          temporary_disable (float): If set, disable Sun avoidance for
+            this number of seconds.
+          escape (bool): If True, schedule an escape drill for 10
+            seconds from now.
+          avoidance_radius (float): If set, change the FOV radius
+            (degrees), for Sun avoidance purposes, to this number.
+          shift_sun_hours (float): If set, compute the Sun position as
+            though it were this many hours in the future.  This is for
+            debugging, testing, and work-arounds.  Pass zero to
+            cancel.
 
-        **Task** - Update solar avoidance parameters.
         """
         do_recompute = False
         now = time.time()
-        self.log.info('update_solar params: {params}',
+        self.log.info('update_sun params: {params}',
                       params={k: v for k, v in params.items()
                               if v is not None})
 
         if params['reset']:
             self.reset_sun_params()
             do_recompute = True
-
         if params['enable'] is not None:
             self.sun_params['active_avoidance'] = params['enable']
             self.sun_params['disable_until'] = 0
         if params['temporary_disable'] is not None:
             self.sun_params['disable_until'] = params['temporary_disable'] + now
-        if params['trigger_panic']:
-            self.log.warn('Triggering solar avoidance panic drill in 10 seconds.')
+        if params['escape']:
+            self.log.warn('Setting sun escape drill to start in 10 seconds.')
             self.sun_params['next_drill'] = now + 10
-        if params['shift_sun_hours'] is not None:
-            self.sun_params['safety_map_kw']['sun_time_shift'] = \
-                params['shift_sun_hours'] * 3600
-            do_recompute = True
         if params['avoidance_radius'] is not None:
             self.sun_params['policy']['exclusion_radius'] = \
                 params['avoidance_radius']
             do_recompute = True
+        if params['shift_sun_hours'] is not None:
+            self.sun_params['safety_map_kw']['sun_time_shift'] = \
+                params['shift_sun_hours'] * 3600
+            do_recompute = True
 
         if do_recompute:
             self.sun_params['recompute_req'] = True
 
         return True, 'Params updated.'
 
+    @ocs_agent.param('_')
     @inlineCallbacks
-    def seek_sun_safety(self, session, params):
-        """seek_sun_safety()
+    def escape_sun_now(self, session, params):
+        """escape_sun_now()
 
-        **Task** - Move the platform to a Sun-Safe position.
+        **Task** - Take control of the platform, and move it to a
+        Sun-Safe position.  This will abort/stop any current go_to or
+        generate_scan, identify the safest possible path to North or
+        South (without changing elevation, if possible), and perform
+        the moves to get there.
 
         """
         state = 'init'
@@ -2037,76 +2073,78 @@ def seek_sun_safety(self, session, params):
                         'timestamp': time.time()}
         session.set_status('running')
 
-        while session.status in ['starting', 'running'] and state not in ['safe-i-guess']:
+        while session.status in ['starting', 'running'] and state not in ['escape-done']:
             az, el = [self.data['status']['summary'][f'{ax}_current_position']
                       for ax in ['Azimuth', 'Elevation']]
 
             if state == 'init':
-                state = 'shelter-abort'
-            elif state == 'shelter-abort':
+                state = 'escape-abort'
+            elif state == 'escape-abort':
                 # raise stop flags and issue stop on motion ops
                 for op in ['generate_scan', 'go_to']:
                     self.agent.stop(op)
                     self.agent.abort(op)
-                state = 'shelter-wait-idle'
+                state = 'escape-wait-idle'
                 timeout = 30
-            elif state == 'shelter-wait-idle':
+            elif state == 'escape-wait-idle':
                 for op in ['generate_scan', 'go_to']:
                     ok, msg, _session = self.agent.status(op)
                     if _session.get('status', 'done') != 'done':
                         break
                 else:
-                    state = 'shelter-move'
+                    state = 'escape-move'
                     last_move = time.time()
                 timeout -= 1
                 if timeout < 0:
-                    state = 'shelter-stop'
-            elif state == 'shelter-stop':
+                    state = 'escape-stop'
+            elif state == 'escape-stop':
                 yield self._stop()
-                state = 'shelter-move'
+                state = 'escape-move'
                 last_move = time.time()
-            elif state == 'shelter-move':
+            elif state == 'escape-move':
                 escape_path = self.sun.find_escape_paths(az, el)
                 if escape_path is None:
-                    print('failed to find escape path @%.1f, az=%.3f el=%.3f' %
-                          (time.time(), az, el))
+                    self.log.error('failed to find best escape path @(t, az, el) '
+                                   '= (%.1f, %.3f, %.3f) !' % (time.time(), az, el))
+                    self.log.info('Trying fallback, due South, low elevation.')
+                    legs = [(180., max(self.sun_params['policy']['min_el'], 0))]
                 else:
-                    print('escaping to', escape_path['moves'].nodes[-1])
-                legs = escape_path['moves'].nodes[1:]
-                state = 'shelter-move-legs'
+                    legs = escape_path['moves'].nodes[1:]
+                self.log.info('escaping to (az, el)={pos}', pos=legs[-1])
+                state = 'escape-move-legs'
                 leg_d = None
-            elif state == 'shelter-move-legs':
+            elif state == 'escape-move-legs':
                 def _leg_done(result):
                     nonlocal state, last_move, leg_d
                     all_ok, msg = result
                     if not all_ok:
-                        print('leg failed:', leg_az, leg_el)
+                        self.log.error('Leg failed.')
                         # Recompute the escape path.
                         if time.time() - last_move > 60:
-                            print('giving up for now')
-                            state = 'safe-i-guess'
+                            self.log.error('Too many failures -- giving up for now')
+                            state = 'escape-done'
                         else:
-                            state = 'shelter-move'
+                            state = 'escape-move'
                     else:
                         leg_d = None
                         last_move = time.time()
-                    if not self._get_sun_policy('shelter_enabled'):
-                        state = 'safe-i-guess'
+                    if not self._get_sun_policy('escape_enabled'):
+                        state = 'escape-done'
                 if leg_d is None:
                     if len(legs) == 0:
-                        state = 'safe-i-guess'
+                        state = 'escape-done'
                     else:
                         leg_az, leg_el = legs.pop(0)
                         leg_d = self._go_to_axes(session, az=leg_az, el=leg_el,
                                                  clear_faults=True)
                         leg_d.addCallback(_leg_done)
-
-            elif state == 'safe-i-guess':
+            elif state == 'escape-done':
+                # This block won't run -- loop will exit.
                 pass
 
             session.data['state'] = state
             if state != last_state:
-                self.log.info('solar_avoidance: state is now "{state}"', state=state)
+                self.log.info('escape_sun_now: state is now "{state}"', state=state)
                 last_state = state
             yield dsleep(1)
 
@@ -2134,11 +2172,25 @@ def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az):
             return True, 'Sun-safety not active; %s' % msg
 
     def _get_sunsafe_moves(self, target_az, target_el):
+        """Given a target position, find a Sun-safe way to get there.  This
+        will either be a direct move, or else an ordered slew in az
+        before el (or vice versa).
+
+        Returns (legs, msg).  If legs is None, it indicates that no
+        Sun-safe path could be found; msg is an error message.  If a
+        path can be found, the legs is a list of intermediate move
+        targets, ``[(az0, el0), (az1, el1) ...]``, terminating on
+        ``(target_az, target_el)``.  msg is None in that case.
+
+        When Sun avoidance is not enabled, this function returns as
+        though the direct path to the target is a safe one.
+
+        """
         if not self._get_sun_policy('sunsafe_moves'):
             return [(target_az, target_el)], None
 
         if not self._get_sun_policy('map_valid'):
-            return None, 'Sun Safety Map not computed or stale; run the solar_avoidance process.'
+            return None, 'Sun Safety Map not computed or stale; run the monitor_sun process.'
 
         # Check the target position and block it outright.
         if self.sun.check_trajectory([target_az], [target_el])['sun_time'] <= 0:
@@ -2299,12 +2351,12 @@ def add_agent_args(parser_in=None):
                         help="Override the minimum el defined in platform config.")
     pgroup.add_argument("--max-el", type=float,
                         help="Override the maximum el defined in platform config.")
-    pgroup.add_argument("--sun-avoidance", type=int,
-                        help="Pass 0 or 1 to enable or disable sun-avoidance. "
+    pgroup.add_argument("--avoid-sun", type=int,
+                        help="Pass 0 or 1 to enable or disable Sun avoidance. "
                         "Overrides the platform default config.")
-    pgroup.add_argument("--avoidance-radius", type=float,
-                        help="Override the default focal plane radius for "
-                        "Sun avoidance purposes.  Setting to zero disables.")
+    pgroup.add_argument("--fov-radius", type=float,
+                        help="Override the default field of view (radius in "
+                        "degrees, for Sun avoidance purposes.")
     return parser_in
 
 
@@ -2313,16 +2365,14 @@ def main(args=None):
     args = site_config.parse_args(agent_class='ACUAgent',
                                   parser=parser,
                                   args=args)
-    avoidance = (None if args.sun_avoidance is None
-                 else (args.sun_avoidance != 0))
 
     agent, runner = ocs_agent.init_site_agent(args)
     _ = ACUAgent(agent, args.acu_config, args.exercise_plan,
                  startup=not args.no_processes,
                  ignore_axes=args.ignore_axes,
                  disable_idle_reset=args.disable_idle_reset,
-                 sun_avoidance=avoidance,
-                 avoidance_radius=args.avoidance_radius,
+                 avoid_sun=args.avoid_sun,
+                 fov_radius=args.fov_radius,
                  min_el=args.min_el,
                  max_el=args.max_el)
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 8168ce771..5f6f04b51 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -265,7 +265,7 @@ def get_sun_pos(self, az=None, el=None, t=None):
 
         results = {
             'sun_radec': (v.ra / DEG, v.dec / DEG),
-            'sun_azel': (-neg_zen_az / DEG, zen_el / DEG),
+            'sun_azel': ((-neg_zen_az / DEG) % 360., zen_el / DEG),
         }
         if self.sun_time_shift != 0:
             results['WARNING'] = 'Fake Sun Position is in use!'

From b12092c318c37797cc768e92ab5fe156c1978c8f Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 14:43:57 +0000
Subject: [PATCH 12/21] ACU sun: one more tweak to escape path computation

---
 socs/agents/acu/avoidance.py | 33 +++++++++++++++++++++++++++------
 1 file changed, 27 insertions(+), 6 deletions(-)

diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 5f6f04b51..ffa06a1b9 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -405,7 +405,7 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
         return all_moves
 
     def find_escape_paths(self, az0, el0, t=None,
-                          plot_file=None):
+                          debug=False):
         """Design and analyze a number of different paths that move from (az0,
         el0) to a sun safe position.  Return the list, for further
         processing and choice.
@@ -435,12 +435,15 @@ def find_escape_paths(self, az0, el0, t=None,
             best_paths = [p for p in best_paths if p is not None]
             if len(best_paths):
                 path = self.select_move(best_paths, escape=True)[0]
+                if debug:
+                    cands, _ = self.select_move(best_paths, escape=True, raw=True)
+                    return cands
             if path is not None:
                 return path
 
         return None
 
-    def select_move(self, moves, escape=False):
+    def select_move(self, moves, escape=False, raw=False):
         _p = self.policy
 
         decisions = [{'rejected': False,
@@ -491,20 +494,38 @@ def reject(d, reason):
         if len(cands) == 0:
             return None, decisions
 
-        def priority_func(m):
-            # Sorting key for move proposals.
+        def metric_func(m):
+            # Sorting key for move proposals.  More preferable paths
+            # should have higher sort order.
             azs = m['req_start'][0], m['req_stop'][0]
             els = m['req_start'][1], m['req_stop'][1]
             return (
+                # Low sun_time is bad, though anything longer
+                # than response_time is equivalent.
                 m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'],
+
+                # Single leg moves are preferred, for simplicity.
                 m['direct'],
+
+                # Higher minimum sun distance is preferred.
                 m['sun_dist_min'],
-                m['sun_dist_mean'],
+
+                # Shorter paths (less total az / el motion) are preferred.
                 -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])),
                 -abs(azs[1] - azs[0]),
+
+                # Larger mean Sun distance is preferred.  But this is
+                # subdominant to path length; otherwise spinning
+                # around a bunch of times can be used to lower the
+                # mean sun dist!
+                m['sun_dist_mean'],
+
+                # Prefer higher elevations for the move, all else being equal.
                 m['travel_el'],
             )
-        cands.sort(key=priority_func)
+        cands.sort(key=metric_func)
+        if raw:
+            return [(c, metric_func(c)) for c in cands], decisions
         return cands[-1], decisions
 
 

From b6a418b3be760f5bafa4cf670680a00817c63b02 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 14:48:01 +0000
Subject: [PATCH 13/21] ACU sun: monitor_sun session.data + docs

---
 socs/agents/acu/agent.py | 128 ++++++++++++++++++++++++++++++++-------
 1 file changed, 106 insertions(+), 22 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 0c61c50a8..5c475ca3d 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -1819,7 +1819,7 @@ def reset_sun_params(self, enabled=None, radius=None):
 
         # These params update config for the entire run of agent.
         if enabled is not None:
-            config['enabled'] = enabled
+            config['enabled'] = bool(enabled)
         if radius is not None:
             config['policy']['exclusion_radius'] = radius
 
@@ -1900,7 +1900,67 @@ def monitor_sun(self, session, params):
         configuration, command line arguments, or the update_sun
         task).
 
-        Session data contains lots of good stuff::
+        Session data looks like this::
+
+          {
+            "timestamp": 1698848292.5579932,
+            "active_avoidance": false,
+            "disable_until": 0,
+            "block_motion": false,
+            "recompute_req": false,
+            "next_drill": null,
+            "safety_map_kw": {
+              "sun_time_shift": 0
+            },
+            "policy": {
+              "exclusion_radius": 20,
+              "el_horizon": 10,
+              "min_sun_time": 1800,
+              "response_time": 7200,
+              "min_az": -90,
+              "max_az": 450,
+              "min_el": 18.5,
+              "max_el": 90
+            },
+            "sun_pos": {
+              "map_exists": true,
+              "map_is_old": false,
+              "map_ref_time": 1698848179.1123455,
+              "platform_azel": [
+                90.0158,
+                20.0022
+              ],
+              "sun_radec": [
+                216.50815789438036,
+                -14.461844389380719
+              ],
+              "sun_azel": [
+                78.24269024936028,
+                60.919554369324096
+              ],
+              "sun_dist": 41.75087242151837,
+              "sun_safe_time": 71760
+            },
+            "avoidance": {
+              "safety_unknown": false,
+              "warning_zone": false,
+              "danger_zone": false,
+              "escape_triggered": false,
+              "escape_active": false,
+              "last_escape_time": 0,
+              "sun_is_real": true
+            }
+          }
+
+        In debugging, the Sun position might be falsified.  In that
+        case the "sun_pos" subtree will contain an entry like this::
+
+          "WARNING": "Fake Sun Position is in use!",
+
+        and "avoidance": "sun_is_real" will be set to false.  (No
+        other functionality is changed when using a falsified Sun
+        position; flags are computed and actions decided based on the
+        false position.)
 
         """
         def _get_sun_map():
@@ -1926,15 +1986,18 @@ def _notify_recomputed(result):
         session.set_status('running')
 
         while session.status in ['starting', 'running']:
+            new_data = {
+                'timestamp': time.time(),
+            }
+            new_data.update(self.sun_params)
+
             try:
                 az, el = [self.data['status']['summary'][f'{ax}_current_position']
                           for ax in ['Azimuth', 'Elevation']]
                 if az is None or el is None:
                     raise KeyError
             except KeyError:
-                session.data = {}
-                yield dsleep(1)
-                continue
+                az, el = None, None
 
             no_map = self.sun is None
             old_map = (not no_map
@@ -1950,25 +2013,37 @@ def _notify_recomputed(result):
                 threads.deferToThread(_get_sun_map).addCallback(
                     _notify_recomputed)
 
+            new_data.update({
+                'sun_pos': {
+                    'map_exists': not no_map,
+                    'map_is_old': old_map,
+                    'map_ref_time': None if no_map else self.sun.base_time,
+                    'platform_azel': (az, el),
+                },
+            })
+
+            sun_is_real = True  # flags time shift during debugging.
             if self.sun is not None:
                 info = self.sun.get_sun_pos(az, el)
-                session.data.update(info)
-                t = self.sun.check_trajectory([az], [el])['sun_time']
-                session.data['sun_safe_time'] = t if t > 0 else 0
+                sun_is_real = ('WARNING' not in info)
+                new_data['sun_pos'].update(info)
+                if az is not None:
+                    t = self.sun.check_trajectory([az], [el])['sun_time']
+                    new_data['sun_pos']['sun_safe_time'] = t if t > 0 else 0
 
             # Are we currently in safe position?
-            safe_known = False
-            safe = False
+            safety_known, danger_zone, warning_zone = False, False, False
             if self.sun is not None:
-                safe_known = True
-                safe = t >= self.sun_params['policy']['min_sun_time']
+                safety_known = True
+                danger_zone = (t < self.sun_params['policy']['min_sun_time'])
+                warning_zone = (t < self.sun_params['policy']['response_time'])
 
             # Has a drill been requested?
             drill_req = (self.sun_params['next_drill'] is not None
                          and self.sun_params['next_drill'] <= time.time())
 
             # Should we be doing a escape_sun_now?
-            panic_for_real = safe_known and not safe and self._get_sun_policy('escape_enabled')
+            panic_for_real = safety_known and danger_zone and self._get_sun_policy('escape_enabled')
             panic_for_fun = drill_req
 
             # Is escape_sun_now task running?
@@ -1978,11 +2053,15 @@ def _notify_recomputed(result):
             # Block motion as long as we are not sun-safe.
             self.sun_params['block_motion'] = (panic_for_real or escape_in_progress)
 
-            session.data.update({
-                'danger_zone': panic_for_real,
-                'motion_blocked': self.sun_params['block_motion'],
-                'escape_in_progress': escape_in_progress,
-            })
+            new_data['avoidance'] = {
+                'safety_unknown': not safety_known,
+                'warning_zone': warning_zone,
+                'danger_zone': danger_zone,
+                'escape_triggered': panic_for_real,
+                'escape_active': escape_in_progress,
+                'last_escape_time': last_panic,
+                'sun_is_real': sun_is_real,
+            }
 
             if (panic_for_real or panic_for_fun) and (time.time() - last_panic > 60.):
                 self.log.warn('monitor_sun is requesting escape_sun_now.')
@@ -1990,6 +2069,9 @@ def _notify_recomputed(result):
                 self.agent.start('escape_sun_now')
                 last_panic = time.time()
 
+            # Update session.
+            session.data.update(new_data)
+
             yield dsleep(1)
 
     @ocs_agent.param('reset', type=bool, default=None)
@@ -2102,15 +2184,17 @@ def escape_sun_now(self, session, params):
                 state = 'escape-move'
                 last_move = time.time()
             elif state == 'escape-move':
+                self.log.info('Getting escape path for (t, az, el) = '
+                              '(%.1f, %.3f, %.3f)' % (time.time(), az, el))
                 escape_path = self.sun.find_escape_paths(az, el)
                 if escape_path is None:
-                    self.log.error('failed to find best escape path @(t, az, el) '
-                                   '= (%.1f, %.3f, %.3f) !' % (time.time(), az, el))
-                    self.log.info('Trying fallback, due South, low elevation.')
+                    self.log.error('Failed to find acceptable path; using '
+                                   'failsafe (South, low el).')
                     legs = [(180., max(self.sun_params['policy']['min_el'], 0))]
                 else:
                     legs = escape_path['moves'].nodes[1:]
-                self.log.info('escaping to (az, el)={pos}', pos=legs[-1])
+                self.log.info('Escaping to (az, el)={pos} ({n} moves)',
+                              pos=legs[-1], n=len(legs))
                 state = 'escape-move-legs'
                 leg_d = None
             elif state == 'escape-move-legs':

From d70ac29df8918fa63af2721787ef2a28109cb57f Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 15:13:33 +0000
Subject: [PATCH 14/21] ACU sun: fix direct path bug; remove "escape=True"
 switch

---
 socs/agents/acu/avoidance.py | 32 ++++++++++++++++++++------------
 1 file changed, 20 insertions(+), 12 deletions(-)

diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index ffa06a1b9..e1371672e 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -214,7 +214,7 @@ def check_trajectory(self, az, el, t=None, raw=False):
         minimum value of the Sun Distance map.
 
         This requires the Sun Safety Map to have been computed with a
-        base_time of t - 24 hours or later.
+        base_time in the 24 hours before t.
 
         Returns a dict with entries:
 
@@ -376,12 +376,17 @@ def analyze_paths(self, az0, el0, az1, el1, t=None,
                         a, = ax.plot(i, j, color=c, lw=1)
                 last_el = iel
 
-        # Include the "direct" path.
+        # Include the direct path, but put in "worst case" details
+        # based on all "confined" paths computed above.
         direct = dict(base)
         direct['moves'] = MoveSequence(az0, el0, az1, el1)
         traj_info = self.check_trajectory(*direct['moves'].get_traj(), t=t)
         direct.update(traj_info)
-        all_moves.append(direct)
+        conf = [m for m in all_moves if m['travel_el_confined']]
+        if len(conf):
+            for k in ['sun_time', 'sun_dist_min', 'sun_dist_mean']:
+                direct[k] = min([m[k] for m in conf])
+            all_moves.append(direct)
 
         if plot_file:
             # Add the direct traj, in blue.
@@ -431,19 +436,19 @@ def find_escape_paths(self, az0, el0, t=None,
         for el1 in els:
             paths = [self.analyze_paths(az0, el0, _az, el1, t=t, dodging=False)
                      for _az in az_cands]
-            best_paths = [self.select_move(p, escape=True)[0] for p in paths]
+            best_paths = [self.select_move(p)[0] for p in paths]
             best_paths = [p for p in best_paths if p is not None]
             if len(best_paths):
-                path = self.select_move(best_paths, escape=True)[0]
+                path = self.select_move(best_paths)[0]
                 if debug:
-                    cands, _ = self.select_move(best_paths, escape=True, raw=True)
+                    cands, _ = self.select_move(best_paths, raw=True)
                     return cands
             if path is not None:
                 return path
 
         return None
 
-    def select_move(self, moves, escape=False, raw=False):
+    def select_move(self, moves, raw=False):
         _p = self.policy
 
         decisions = [{'rejected': False,
@@ -460,7 +465,10 @@ def reject(d, reason):
 
             els = m['req_start'][1], m['req_stop'][1]
 
-            if escape and (m['sun_time_start'] < _p['min_sun_time']):
+            if (m['sun_time_start'] < _p['min_sun_time']):
+                # If the path is starting in danger zone, then only
+                # enforce that the move takes the platform to a better place.
+
                 # Test > res, rather than > 0... near the minimum this
                 # can be noisy.
                 if m['sun_dist_start'] - m['sun_dist_min'] > self.res / DEG:
@@ -469,10 +477,10 @@ def reject(d, reason):
                 if m['sun_time_stop'] < _p['min_sun_time']:
                     reject(d, 'Path does not end in sun-safe location.')
                     continue
-            else:
-                if m['sun_time'] < _p['min_sun_time']:
-                    reject(d, 'Path too close to sun.')
-                    continue
+
+            elif m['sun_time'] < _p['min_sun_time']:
+                reject(d, 'Path too close to sun.')
+                continue
 
             if m['travel_el'] < _p['min_el']:
                 reject(d, 'Path goes below minimum el.')

From 496d07af9b7cc83a36edb0d9c66665ae47664129 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 16:47:20 +0000
Subject: [PATCH 15/21] ACU sun: generate_scan initial seek must be sun-safe
 too!

---
 socs/agents/acu/agent.py | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 5c475ca3d..a104e6482 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -1639,10 +1639,14 @@ def generate_scan(self, session, params):
 
         # Seek to starting position
         self.log.info(f'Moving to start position, az={plan["init_az"]}, el={init_el}')
-        ok, msg = yield self._go_to_axes(session, az=plan['init_az'], el=init_el)
-
-        if not ok:
-            return False, f'Start position seek failed with message: {msg}'
+        legs, msg = yield self._get_sunsafe_moves(plan['init_az'], init_el)
+        if msg is not None:
+            self.log.error(msg)
+            return False, msg
+        for leg_az, leg_el in legs:
+            ok, msg = yield self._go_to_axes(session, az=leg_az, el=leg_el)
+            if not ok:
+                return False, f'Start position seek failed with message: {msg}'
 
         # Prepare the point generator.
         g = sh.generate_constant_velocity_scan(az_endpoint1=az_endpoint1,

From 8844a616e6fec3e01426a50e2d03ae75c0359af3 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Fri, 3 Nov 2023 15:13:24 +0000
Subject: [PATCH 16/21] ACU: fix bug where empty blocks were pushed to feed

This wasn't causing any trouble other than lots of log messages in
influxpublisher.
---
 socs/agents/acu/agent.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index a104e6482..a8fcc4a4a 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -640,7 +640,9 @@ def monitor(self, session, params):
                 del new_influx_blocks[k]
 
             for block in new_influx_blocks.values():
-                self.agent.publish_to_feed('acu_status_influx', block)
+                # Check that we have data (commands and corotator often don't)
+                if len(block['data']) > 0:
+                    self.agent.publish_to_feed('acu_status_influx', block)
             influx_blocks.update(new_influx_blocks)
 
             # Assemble data for aggregator ...

From 857c1664f499817e7130431fc9bc339b4f5c65d8 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 19:39:48 +0000
Subject: [PATCH 17/21] ACU sun: docs and requirements

---
 docs/agents/acu_agent.rst    |  58 ++++++++++++++
 requirements.txt             |   2 +
 socs/agents/acu/agent.py     |  19 ++---
 socs/agents/acu/avoidance.py | 145 +++++++++++++++++++++++++++++------
 4 files changed, 193 insertions(+), 31 deletions(-)

diff --git a/docs/agents/acu_agent.rst b/docs/agents/acu_agent.rst
index c08f3cbeb..dcde6f975 100644
--- a/docs/agents/acu_agent.rst
+++ b/docs/agents/acu_agent.rst
@@ -88,6 +88,61 @@ example configuration block is below::
     }
 
 
+Sun Avoidance
+-------------
+
+The Sun's position, and the potential danger of the Sun to the
+equipment, is monitored and reported by the ``monitor_sun`` Process.
+If enabled to do so, this Process can trigger the ``escape_sun_now``
+Task, which will cause the platform to move to a Sun-safe position.
+
+The parameters used by an Agent instance for Sun Avoidance are
+determined like this:
+
+- Default parameters for each platform (LAT and SATP) are in the Agent
+  code.
+- On start-up the default parameters for platform are modified
+  according to any command-line parameters passed in by the user.
+- Some parameters can be altered using the command line.
+
+The avoidance policy is defined by a few key parameters and concepts.
+
+.. automodule:: socs.agents.acu.avoidance
+
+
+The ``exclusion_radius`` can be configured from the Agent command
+line, and also through the ``update_sun`` Task.
+
+When Sun Avoidance is active (``active_avoidance`` is ``True``), the
+following will be enforced:
+
+- When a user initiates the ``go_to`` Task, the target point of the
+  motion will be checked.  If it is not Sun-safe, the Task will exit
+  immediately with an error.  If the Task cannot find a set of moves
+  that are Sun-safe and that do not violate other requirements
+  (azimuth and elevation limits; the ``el_dodging`` policy), then the
+  Task will exit with error.  The move may be executed as a series of
+  separate legs (e.g. the Task may move first to an intermediate
+  elevation, then slew in azimuth, then continue to the final
+  elevation) rather than simulataneously commanding az and el motion.
+- When a user starts the ``generate_scan`` Process, the sweep of the
+  scan will be checked for Sun-safety, and the Process will exit with
+  error if it is not.  Furthermore, any movement required prior to
+  starting the scan will be checked in the same way as for the
+  ``go_to`` Task.
+- If the platform, at any time, enters a position that is not
+  Sun-safe, then an Escape will be Initiated.  During an Escape, any
+  running ``go_to`` or ``generate_scan`` operations will be cancelled,
+  and further motions are blocked.  The platform will be driven to a
+  position at due North or due South.  The current elevation of the
+  platform will be preserved, unless that is not Sun-safe (in which
+  case lower elevations will be attempted).  The Escape feature is
+  active, even when motions are not in progress, as long as the
+  ``monitor_sun`` Process is running.  However -- the Escape operation
+  requires that the platform be in Remote operation mode, with no
+  persistent faults.
+
+
 Exercisor Mode
 --------------
 
@@ -151,3 +206,6 @@ Supporting APIs
 
 .. automodule:: socs.agents.acu.drivers
     :members:
+
+.. automodule:: socs.agents.acu.avoidance
+    :members:
diff --git a/requirements.txt b/requirements.txt
index 6c02ae1dd..1f74fe79b 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -16,6 +16,8 @@ pyyaml
 
 # acu agent
 soaculib @ git+https://github.com/simonsobs/soaculib.git@master
+so3g
+pixell
 
 # holography agent - python 3.8 only!
 # -r requirements/holography.txt
diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index a8fcc4a4a..6e1fdd805 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -144,8 +144,8 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None,
         if len(self.ignore_axes):
             agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes)
 
-        self.reset_sun_params(enabled=avoid_sun,
-                              radius=fov_radius)
+        self._reset_sun_params(enabled=avoid_sun,
+                               radius=fov_radius)
 
         self.exercise_plan = exercise_plan
 
@@ -1815,7 +1815,7 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
     # Sun Safety Monitoring and Active Avoidance
     #
 
-    def reset_sun_params(self, enabled=None, radius=None):
+    def _reset_sun_params(self, enabled=None, radius=None):
         """Resets self.sun_params based on defaults for this platform.  Note
         if enabled or radius are specified here, they update the
         defaults (so they endure for the life of the agent).
@@ -2087,7 +2087,8 @@ def _notify_recomputed(result):
     @ocs_agent.param('avoidance_radius', type=float, default=None)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     def update_sun(self, session, params):
-        """update_sun()
+        """update_sun(reset, enable, temporary_disable, escape,
+                      avoidance_radius, shift_sun_hours)
 
         **Task** - Update Sun monitoring and avoidance parameters.
 
@@ -2097,7 +2098,7 @@ def update_sun(self, session, params):
             defaults.  (The "defaults" includes any overrides
             specified on Agent command line.)
           enable (bool): If True, enable active Sun avoidance.  If
-            avoidance was temporarily disable,d it is re-enabled.  If
+            avoidance was temporarily disabled it is re-enabled.  If
             False, disable active Sun avoidance (non-temporarily).
           temporary_disable (float): If set, disable Sun avoidance for
             this number of seconds.
@@ -2118,7 +2119,7 @@ def update_sun(self, session, params):
                               if v is not None})
 
         if params['reset']:
-            self.reset_sun_params()
+            self._reset_sun_params()
             do_recompute = True
         if params['enable'] is not None:
             self.sun_params['active_avoidance'] = params['enable']
@@ -2442,11 +2443,11 @@ def add_agent_args(parser_in=None):
     pgroup.add_argument("--max-el", type=float,
                         help="Override the maximum el defined in platform config.")
     pgroup.add_argument("--avoid-sun", type=int,
-                        help="Pass 0 or 1 to enable or disable Sun avoidance. "
+                        help="Pass 0 or 1 to disable or enable Sun avoidance. "
                         "Overrides the platform default config.")
     pgroup.add_argument("--fov-radius", type=float,
-                        help="Override the default field of view (radius in "
-                        "degrees, for Sun avoidance purposes.")
+                        help="Override the default field-of-view (radius in "
+                        "degrees) for Sun avoidance purposes.")
     return parser_in
 
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index e1371672e..c1690afa7 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -1,17 +1,72 @@
-"""Sun Avoidance
-
-This module provides code to support Sun Avoidance in the ACU Agent.
-The basic idea is to create a map in equatorial coordinates where the
-value of the map indicates when that region of the sky will next be
-within the Sun Exclusion Zone (likely defined as some radius around
-the Sun).
-
-Using that pre-computed map, any az-el pointings can be checked for
-Sun safety (i.e. whether they are safe positoins), at least for the
-following 24 hours.  The map can be used to identify safest routes
-between two az-el pointings.
+# Sun Avoidance
+#
+# The docstring below is intended for injection into the documentation
+# system.
+
+"""When considering Sun Safety of a boresight pointing, we consider an
+exclusion zone around the Sun with a user-specified radius. This is
+called the ``exclusion_radius`` or the field-of-view radius.
+
+The Safety of the instrument at any given moment is parametrized
+by two numbers:
+
+  ``sun_dist``
+    The separation between the Sun and the boresight (az, el), in
+    degrees.
+
+  ``sun_time``
+    The minimum time, in seconds, which must elapse before the current
+    (az, el) pointing of the boresight will lie within the exclusion
+    radius of the Sun.
+
+While the ``sun_dist`` is an important indicator of whether the
+instrument is currently in immediate danger, the ``sun_time`` is
+helpful with looking forward and avoiding positions that will soon be
+dangerous.
+
+The user-defined policy for Sun Safety is captured in the following
+settings:
+
+  ``exclusion_radius``
+    The radius, in degres, of a disk centered on the Sun that must be
+    avoided by the boresight.
+
+  ``min_sun_time``
+    An (az, el) position is considered unsafe (danger zone) if the
+    ``sun_time`` is less than the ``min_sun_time``. (Expressed in
+    seconds.)
+
+  ``response_time``
+    An (az, el) position is considered vulnerable (warning zone) if
+    the ``sun_time`` is less than the ``response_time``. This is
+    intended to represent the maximum amount of time it could take an
+    operator to reach the instrument and secure it, were motion to
+    stall unexpectedly. (Expressed in seconds.)
+
+  ``el_horizon``
+    The elevation (in degrees) below which the Sun may be considered
+    as invisible to the instrument.
+
+  ``el_dodging``
+    This setting affects how point-to-point motions are executed, with
+    respect to what elevations may be used in intermediate legs of the
+    trajectory. When this is False, the platform is restricted to
+    travel only at elevations that lie between the initial and the
+    target elevation. When True, the platform is permitted to travel
+    at other elevations, all the way up to the limits of the
+    platform. Using True is helpful to find Sun-safe trajectories in
+    some circumstances. But False is helpful if excess elevation
+    changes are potentially disturbing to the cryogenics.  This
+    setting only affects point-to-point motions; "escape" paths will
+    always consider all available elevations.
+
+
+A "Sun-safe" position is a pointing of the boresight that currently
+has a ``sun_time`` that meets or exceeds the ``min_sun_time``
+parameter.
 
 """
+
 import datetime
 import math
 import time
@@ -47,26 +102,37 @@
 
 
 class SunTracker:
-    """Provide guidance on what horizion coordinate positions are
-    sun-safe.
-
-    Key concepts:
-    - Sun Safety Map
-    - Az-el trajectory
+    """Provide guidance on what horizion coordinate positions and
+    trajectories are sun-safe.
 
     Args:
-      exclusion_radius (float, deg): radius of circle around the Sun
-        to consider as "unsafe".
+      policy (dict): Exclusion policy parameters.  See module
+        docstring, and DEFAULT_POLICY.  The policy should also include
+        {min,max}\\_{el,az}, giving the limits supported by those axes.
+      site (EarthlySite or None): Site to use; default is the SO LAT.
+        If not None, pass an so3g.proj.EarthlySite or compatible.
       map_res (float, deg): resolution to use for the Sun Safety Map.
-      site (str or None): Site to use (so3g site, defaults to so_lat).
+      sun_time_shift (float, seconds): For debugging and testing,
+        compute the Sun's position as though it were this manys
+        seconds in the future.  If None or zero, this feature is
+        disabled.
+      fake_now (float, seconds): For debugging and testing, replace
+        the tracker's computation of the current time (time.time())
+        with this value.  If None, this testing feature is disabled.
+      compute (bool): If True, immediately compute the Sun Safety Map
+        by calling .reset().
+      base_time (unix timestamp): Store this base_time and, if compute
+        is True, pass it to .reset().
 
     """
 
     def __init__(self, policy=None, site=None,
-                 map_res=.5, sun_time_shift=0., fake_now=None,
+                 map_res=.5, sun_time_shift=None, fake_now=None,
                  compute=True, base_time=None):
         # Note res is stored in radians.
         self.res = map_res * DEG
+        if sun_time_shift is None:
+            sun_time_shift = 0.
         self.sun_time_shift = sun_time_shift
         self.fake_now = fake_now
         self.base_time = base_time
@@ -449,6 +515,28 @@ def find_escape_paths(self, az0, el0, t=None,
         return None
 
     def select_move(self, moves, raw=False):
+        """Given a list of possible "moves", select the best one.
+        The "moves" should be like the ones returned by
+        ``analyze_paths``.
+
+        The best move is determined by first screening out dangerous
+        paths (ones that pass close to Sun, move closer to Sun
+        unnecessarily, violate axis limits, etc.) and then identifying
+        paths that minimize danger (distance to Sun; Sun time) and
+        path length.
+
+        If raw=True, a debugging output is returned; see code.
+
+        Returns:
+          best_move (dict): The element of moves that is safest.  If
+            no safe move was found, None is returned.
+          decisions (list): The items in this list are dicts that
+            correspond one-to-one with the entries in moves.  Each
+            decision dict has entries 'rejected' (True or False) and
+            'reason' (string description of why the move was rejected
+            outright).
+
+        """
         _p = self.policy
 
         decisions = [{'rejected': False,
@@ -539,6 +627,19 @@ def metric_func(m):
 
 class MoveSequence:
     def __init__(self, *args, simplify=False):
+        """Container for a series of (az, el) positions.  Pass the
+        positions to the constructor as (az, el) tuples::
+
+          MoveSequence((60, 180), (60, 90), (50, 90))
+
+        or equivalently as individual arguments::
+
+          MoveSequence(60, 180, 60, 90, 50, 90)
+
+        If simplify=True is passed, then any immediate position
+        repetitions are deleted.
+
+        """
         self.nodes = []
         if len(args) == 0:
             return

From f4effc8fdc055cdaf6ffbb21fa7769695cc25468 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Wed, 1 Nov 2023 16:14:28 -0400
Subject: [PATCH 18/21] ACU sun: add tests, cleanup

---
 socs/agents/acu/avoidance.py   | 33 -------------------------------
 tests/agents/test_acu_agent.py | 36 ++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 33 deletions(-)

diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index c1690afa7..271cf005c 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -681,36 +681,3 @@ def get_traj(self, res=0.5):
             xx.append(np.linspace(x0, x1, n))
             yy.append(np.linspace(y0, y1, n))
         return np.hstack(tuple(xx)), np.hstack(tuple(yy))
-
-
-class RollingMinimum:
-    def __init__(self, window, fallback=None):
-        self.window = window
-        self.subwindow = window / 10
-        self.fallback = fallback
-        self.records = []
-
-    def append(self, val, t=None):
-        if t is None:
-            t = time.time()
-        # Remove old data
-        while len(self.records) and (t - self.records[0][0]) > self.window:
-            self.records.pop(0)
-        # Add this to existing subwindow?
-        if len(self.records):
-            # Consider values up to subwindow ago.
-            _t, _val = self.records[-1]
-            if t - _t < self.subwindow:
-                if val <= _val:
-                    self.records[-1] = (t, val)
-                return
-        # Or start a new subwindow.
-        self.records.append((t, val))
-
-    def get(self, lookback=None):
-        if lookback is None:
-            lookback = self.window
-        recs = [v for t, v in self.records if (time.time() - t < lookback)]
-        if len(recs):
-            return min(recs)
-        return self.fallback
diff --git a/tests/agents/test_acu_agent.py b/tests/agents/test_acu_agent.py
index 921fe550c..9afa6f280 100644
--- a/tests/agents/test_acu_agent.py
+++ b/tests/agents/test_acu_agent.py
@@ -1 +1,37 @@
+from socs.agents.acu import avoidance as av
 from socs.agents.acu.agent import ACUAgent  # noqa: F401
+
+
+def test_avoidance():
+    az0, el0 = 72, 67
+    t0 = 1698850000
+
+    sun = av.SunTracker(fake_now=t0)
+    pos = sun.get_sun_pos()
+    az, el = pos['sun_azel']
+    assert abs(az - az0) < .5 and abs(el - el0) < .5
+
+    # Zenith should be about 23 deg away.
+    assert abs(sun.get_sun_pos(0, 90)['sun_dist'] - 23) < 0.5
+
+    # Unsafe positions.
+    assert sun.check_trajectory([90], [60])['sun_time'] == 0
+
+    # Safe positions
+    assert sun.check_trajectory([90], [20])['sun_time'] > 0
+    assert sun.check_trajectory([270], [60])['sun_time'] > 0
+
+    # No safe moves to Sun position.
+    paths = sun.analyze_paths(180, 20, az0, el0)
+    path, analysis = sun.select_move(paths)
+    assert path is None
+
+    # Escape paths.
+    path = sun.find_escape_paths(az0, el0 - 5)
+    assert path is not None
+    path = sun.find_escape_paths(az0, el0 + 5)
+    assert path is not None
+    path = sun.find_escape_paths(az0 - 10, el0)
+    assert path is not None
+    path = sun.find_escape_paths(az0 + 10, el0)
+    assert path is not None

From ef96a664e0c7306b5579f699560c3503949aa512 Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Mon, 6 Nov 2023 19:03:19 +0000
Subject: [PATCH 19/21] ACU sunvoidance: more docs cleanup

---
 docs/agents/acu_agent.rst    | 14 ++++++++++----
 socs/agents/acu/agent.py     |  3 +--
 socs/agents/acu/avoidance.py |  4 +++-
 3 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/docs/agents/acu_agent.rst b/docs/agents/acu_agent.rst
index dcde6f975..3ff79266c 100644
--- a/docs/agents/acu_agent.rst
+++ b/docs/agents/acu_agent.rst
@@ -105,10 +105,10 @@ determined like this:
   according to any command-line parameters passed in by the user.
 - Some parameters can be altered using the command line.
 
-The avoidance policy is defined by a few key parameters and concepts.
-
-.. automodule:: socs.agents.acu.avoidance
-
+The avoidance policy is defined by a few key parameters and concepts;
+please see the descriptions of ``sun_dist``, ``sun_time``,
+``exclusion_radius``, and more in the :mod:`socs.agents.acu.avoidance`
+module documentation.
 
 The ``exclusion_radius`` can be configured from the Agent command
 line, and also through the ``update_sun`` Task.
@@ -204,8 +204,14 @@ acquisition processes are running::
 Supporting APIs
 ---------------
 
+drivers (Scanning support)
+``````````````````````````
+
 .. automodule:: socs.agents.acu.drivers
     :members:
 
+avoidance (Sun Avoidance)
+`````````````````````````
+
 .. automodule:: socs.agents.acu.avoidance
     :members:
diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 6e1fdd805..a2087653b 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -2087,8 +2087,7 @@ def _notify_recomputed(result):
     @ocs_agent.param('avoidance_radius', type=float, default=None)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     def update_sun(self, session, params):
-        """update_sun(reset, enable, temporary_disable, escape,
-                      avoidance_radius, shift_sun_hours)
+        """update_sun(reset, enable, temporary_disable, escape, avoidance_radius, shift_sun_hours)
 
         **Task** - Update Sun monitoring and avoidance parameters.
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index 271cf005c..f8c1bed74 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -87,7 +87,9 @@
 DAY = 86400
 NO_TIME = DAY * 2
 
-
+#: Default policy to apply when evaluating Sun-safety and planning
+#: trajectories.  Note the Agent code may apply different defaults,
+#: based on known platform details.
 DEFAULT_POLICY = {
     'exclusion_radius': 20,
     'min_el': 0,

From 330be8644e35f51f5ec9be5f001cba674d698b1b Mon Sep 17 00:00:00 2001
From: Brian Koopman <brian.koopman@yale.edu>
Date: Mon, 6 Nov 2023 17:37:03 -0500
Subject: [PATCH 20/21] ACU sun: Add new dependencies to setup.py

---
 docs/agents/acu_agent.rst  |  6 ++++++
 docs/user/installation.rst |  2 ++
 setup.py                   | 15 +++++++++------
 3 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/docs/agents/acu_agent.rst b/docs/agents/acu_agent.rst
index 3ff79266c..c02faa020 100644
--- a/docs/agents/acu_agent.rst
+++ b/docs/agents/acu_agent.rst
@@ -26,6 +26,12 @@ installed to use this Agent. This can be installed via:
 
     $ pip install 'soaculib @ git+https://github.com/simonsobs/soaculib.git@master'
 
+Additionally, ``socs`` should be installed with the ``acu`` group:
+
+.. code-block:: bash
+
+    $ pip install -U socs[acu]
+
 Configuration File Examples
 ---------------------------
 Below are configuration examples for the ocs config file and for soaculib.
diff --git a/docs/user/installation.rst b/docs/user/installation.rst
index 4cd0114a1..ce1f0f580 100644
--- a/docs/user/installation.rst
+++ b/docs/user/installation.rst
@@ -22,6 +22,8 @@ The different groups, and the agents they provide dependencies for are:
      - Supporting Agents
    * - ``all``
      - All Agents
+   * - ``acu``
+     - ACU Agent
    * - ``labjack``
      - Labjack Agent
    * - ``magpie``
diff --git a/setup.py b/setup.py
index ea348ef17..37b6f2274 100644
--- a/setup.py
+++ b/setup.py
@@ -7,7 +7,11 @@
 
 # Optional Dependencies
 # ACU Agent
-# acu_deps = ['soaculib @ git+https://github.com/simonsobs/soaculib.git@master']
+acu_deps = [
+    # 'soaculib @ git+https://github.com/simonsobs/soaculib.git@master',
+    'pixell',
+    'so3g',
+]
 
 # Holography FPGA and Synthesizer Agents
 # holography_deps = [  # Note: supports python 3.8 only!
@@ -53,10 +57,9 @@
 #     'xy_stage_control @ git+https://github.com/kmharrington/xy_stage_control.git@main',
 # ]
 
-# Note: Not including the holograph deps, which are Python 3.8 only
-# all_deps = acu_deps + labjack_deps + magpie_deps + pfeiffer_deps + \
-#    pysmurf_deps + smurf_sim_deps + synacc_deps + xy_stage_deps
-all_deps = labjack_deps + magpie_deps + pfeiffer_deps + \
+# Note: Not including the holograph deps, which are Python 3.8 only. Also not
+# including any dependencies with only direct references.
+all_deps = acu_deps + labjack_deps + magpie_deps + pfeiffer_deps + \
     smurf_sim_deps + synacc_deps + timing_master_deps
 all_deps = list(set(all_deps))
 
@@ -111,7 +114,7 @@
     ],
     extras_require={
         'all': all_deps,
-        # 'acu': acu_deps,
+        'acu': acu_deps,
         # 'holography': holography_deps,
         'labjack': labjack_deps,
         'magpie': magpie_deps,

From e2faba83c55d0504ff8267d035711547c1e31d4f Mon Sep 17 00:00:00 2001
From: Matthew Hasselfield <mhasself@users.noreply.github.com>
Date: Tue, 7 Nov 2023 14:06:31 +0000
Subject: [PATCH 21/21] ACU sunvoidance: couple more docs fixes

---
 socs/agents/acu/agent.py     |  3 ++-
 socs/agents/acu/avoidance.py | 16 +++++++++-------
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index a2087653b..5ed66a231 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -2087,7 +2087,8 @@ def _notify_recomputed(result):
     @ocs_agent.param('avoidance_radius', type=float, default=None)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     def update_sun(self, session, params):
-        """update_sun(reset, enable, temporary_disable, escape, avoidance_radius, shift_sun_hours)
+        """update_sun(reset, enable, temporary_disable, escape, \
+                      avoidance_radius, shift_sun_hours)
 
         **Task** - Update Sun monitoring and avoidance parameters.
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index f8c1bed74..a65b7b3e0 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -530,13 +530,15 @@ def select_move(self, moves, raw=False):
         If raw=True, a debugging output is returned; see code.
 
         Returns:
-          best_move (dict): The element of moves that is safest.  If
-            no safe move was found, None is returned.
-          decisions (list): The items in this list are dicts that
-            correspond one-to-one with the entries in moves.  Each
-            decision dict has entries 'rejected' (True or False) and
-            'reason' (string description of why the move was rejected
-            outright).
+          (dict, list): (best_move, decisions)
+
+          ``best_move`` -- the element of moves that is safest.  If no
+          safe move was found, None is returned.
+
+          ``decisions`` - List of dicts, in one-to-one correspondence
+          with ``moves``.  Each decision dict has entries 'rejected'
+          (True or False) and 'reason' (string description of why the
+          move was rejected outright).
 
         """
         _p = self.policy