planewave_mountd

#!/usr/bin/env python3
#
# This file is part of the Robotic Observatory Control Kit (rockit)
#
# rockit is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# rockit is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with rockit.  If not, see <http://www.gnu.org/licenses/>.

"""Daemon for controlling a Planewave L mount through its http interface via Pyro"""

import argparse
import threading
import time
from astropy.coordinates import AltAz, FK5, ICRS, SkyCoord, EarthLocation, get_moon, get_sun
from astropy.time import Time
import astropy.units as u
import requests
import Pyro4

from rockit.common import log, TryLock
from rockit.common.helpers import pyro_client_matches
from rockit.mount.planewave import CommandStatus, Config, MountState


def _parse_mountstate(status, is_homing=False):
    """Parse a MountState value from the PWI status dictionary"""
    if status is None or status['mount.is_connected'] != 'true':
        return MountState.Disabled

    # Work around is_slewing being false while homing
    if is_homing:
        ha_moving = status['mount.axis0.setpoint_velocity_degs_per_sec'] != '0'
        dec_moving = status['mount.axis1.setpoint_velocity_degs_per_sec'] != '0'
        if ha_moving or dec_moving:
            return MountState.Homing

    if status['mount.axis0.is_homed'] != 'true' or status['mount.axis1.is_homed'] != 'true':
        return MountState.NotHomed

    # Simplify the external API we expose by treating unpowered axes as parked
    if status['mount.axis0.is_enabled'] != 'true' or status['mount.axis1.is_enabled'] != 'true':
        return MountState.Parked

    if status['mount.is_slewing'] == 'true':
        return MountState.Slewing

    if status['mount.is_tracking'] == 'true':
        return MountState.Tracking

    return MountState.Stopped


def _parse_location(status):
    """Parse an astropy EarthLocation value from the PWI status dictionary"""
    return EarthLocation(
        lat=status['site.latitude_degs'],
        lon=status['site.longitude_degs'],
        height=status['site.height_meters'])


class MountDaemon:
    """Daemon interface for talon subsystems"""
    def __init__(self, config):
        self._config = config
        self._is_homing = False

        self._ha_positive_limit = self._config.ha_soft_limits[1] * u.deg
        self._ha_negative_limit = self._config.ha_soft_limits[0] * u.deg
        self._dec_positive_limit = self._config.dec_soft_limits[1] * u.deg
        self._dec_negative_limit = self._config.dec_soft_limits[0] * u.deg

        self._pointing_condition = threading.Condition()
        self._force_stopped = False

        # Only one command may be active at a time (except for stop)
        self._command_lock = threading.Lock()

    def _request_with_status(self, command, **kwargs):
        """
        Send a command to PWI4 and return its status response
        Returns None if the command failed or a dictionary of the mount status
        """
        try:
            url = f'http://{self._config.pwi_host}:{self._config.pwi_port}/{command}'
            response = requests.get(url, timeout=self._config.pwi_timeout, params=kwargs)
            response.raise_for_status()

            data = response.text.split("\n")
            status = {}
            for line in data:
                fields = line.split("=", 1)
                if len(fields) == 2:
                    status[fields[0]] = fields[1]
            return status
        except Exception:
            return None

    def _validate_pointing(self, coords, log_failed_prefix):
        """
        Check whether the given coordinate is within our soft HA and Dec limits.
        Returns CommandStatus.Succeeded if valid, otherwise logs and returns the appropriate error status
        """
        icrs = coords.icrs
        lst = Time(coords.obstime, location=coords.location).sidereal_time('apparent')
        ha = (lst - icrs.ra).wrap_at(12 * u.hourangle)
        dec = icrs.dec

        if ha < self._ha_negative_limit or ha > self._ha_positive_limit:
            log.error(self._config.log_name, log_failed_prefix + f'; ha {ha} outside limit ' +
                      f'({self._ha_negative_limit}, {self._ha_positive_limit})')
            return CommandStatus.OutsideHALimits

        if dec < self._dec_negative_limit or dec > self._dec_positive_limit:
            log.error(self._config.log_name, log_failed_prefix + f'; dec {dec} outside limit ' +
                      f'({self._dec_negative_limit}, {self._dec_positive_limit})')
            return CommandStatus.OutsideDecLimits
        return CommandStatus.Succeeded

    def _set_axis_power(self, enabled):
        """
        Send a mount/(enable|disable) command.
        Returns True on success or logs and returns False on error
        """
        command = 'enable' if enabled else 'disable'
        if self._request_with_status('mount/' + command, axis=-1) is None:
            log.error(self._config.log_name, 'Failed to ' + command + ' motor power')
            return False

        # Wait momentarily for the state to change
        time.sleep(1)
        return True

    def _point(self, command, expected_state, is_homing=False, **kwargs):
        """
        Send a mount/goto_* command and wait for slewing to complete.
        Returns True on success or logs and returns False on error
        """
        if is_homing:
            poll_interval = self._config.home_poll_interval
            timeout = self._config.home_timeout
            slewing_state = MountState.Homing
        else:
            poll_interval = self._config.slew_poll_interval
            timeout = self._config.slew_timeout
            slewing_state = MountState.Slewing

        with self._pointing_condition:
            if self._request_with_status(command, **kwargs) is None:
                log.error(self._config.log_name, 'Pointing failed: failed to issue ' + command)

            start = Time.now()
            while True:
                # Note: it can take a few milliseconds before the slew begins
                # Wait an interval period so we don't exit too early
                self._pointing_condition.wait(poll_interval)

                if self._force_stopped:
                    break

                status = self._request_with_status('status')
                state = _parse_mountstate(status, is_homing)
                if state != slewing_state or (Time.now() - start).to_value(u.s) > timeout:
                    break

        if self._force_stopped:
            log.error(self._config.log_name, 'Pointing failed: aborted')
            return False

        if state == expected_state:
            return True

        self._request_with_status('mount/stop')

        if state == MountState.Slewing:
            log.error(self._config.log_name, 'Pointing failed: timed out')
        else:
            log.error(self._config.log_name, f'Pointing failed: completed with {MountState.label(state)} instead of ' +
                      MountState.label(expected_state))

        return False

    @Pyro4.expose
    def report_status(self):
        """Returns a dictionary containing the current telescope state"""

        status = self._request_with_status('status')
        if status is None:
            return {
                'date': Time.now().strftime('%Y-%m-%dT%H:%M:%S.%fZ'),
                'state': MountState.Disabled,
                'state_label': MountState.label(MountState.Disabled),
            }

        state = _parse_mountstate(status, self._is_homing)
        location = _parse_location(status)

        # Prefer the mount timestamp if we have it - this was when the coordinates were calculated
        # If the mount is not connected we can safely fall back to the request timestamp
        if state == MountState.Disabled:
            timestamp = status['response.timestamp_utc']
        else:
            timestamp = status['mount.timestamp_utc']

        date = Time(Time.strptime(timestamp, '%Y-%m-%d %H:%M:%S.%f'), location=location)
        data = {
            'date': date.strftime('%Y-%m-%dT%H:%M:%S.%fZ'),
            'state': state,
            'state_label': MountState.label(state),
            'pwi_version': status['pwi4.version'],
            'site_latitude': location.lat.to_value(u.deg),
            'site_longitude': location.lon.to_value(u.deg),
            'site_elevation': location.height.to_value(u.m),
        }

        if state != MountState.Disabled:
            lst = u.Quantity(status['site.lmst_hours'], unit=u.hourangle)

            data.update({
                'lst': lst.to_value(u.deg),
            })

            if state >= MountState.Parked:
                pointing = SkyCoord(
                    ra=status['mount.ra_j2000_hours'],
                    dec=status['mount.dec_j2000_degs'],
                    unit=(u.hourangle, u.deg),
                    frame='icrs')

                data.update({
                    'ra': pointing.ra.to_value(u.deg),
                    'dec': pointing.dec.to_value(u.deg),
                    'offset_ra': 0,
                    'offset_dec': 0,

                    # Note: this field depends on a custom patched version of pwi4
                    'ha': u.Quantity(status['mount.ha_hours'], unit=u.hourangle).to_value(u.deg),

                    'alt': u.Quantity(status['mount.altitude_degs'], unit=u.deg).to_value(u.deg),
                    'az': u.Quantity(status['mount.azimuth_degs'], unit=u.deg).to_value(u.deg),

                    'moon_separation': get_moon(date).separation(pointing).to_value(u.deg),
                    'sun_separation': get_sun(date).separation(pointing).to_value(u.deg),
                })

        return data

    @Pyro4.expose
    def find_homes(self):
        """Finds the mount home positions"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.Disabled:
                return CommandStatus.MountNotInitialized

            # Enable axes if needed
            axes_powered = status['mount.axis0.is_enabled'] == 'true' and status['mount.axis1.is_enabled'] == 'true'
            if not axes_powered and not self._set_axis_power(True):
                return CommandStatus.Failed

            log.info(self._config.log_name, 'Homing axes')

            try:
                self._is_homing = True
                if not self._point('mount/find_home', MountState.Stopped, is_homing=True):
                    log.error(self._config.log_name, 'Failed to issue home command')
                    return CommandStatus.Failed
            finally:
                self._is_homing = False

            # Restore previous power state
            if not axes_powered and not self._set_axis_power(False):
                return CommandStatus.Failed

            log.info(self._config.log_name, 'Homing complete')
            return CommandStatus.Succeeded

    @Pyro4.expose
    def initialize(self):
        """Connect to mount and enable motor power"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            if status is None:
                return CommandStatus.MountControlNotRunning

            state = _parse_mountstate(status)
            if state != MountState.Disabled:
                return CommandStatus.MountNotDisabled

            if self._request_with_status('mount/connect') is None:
                log.error(self._config.log_name, 'Failed to connect to mount')
                return CommandStatus.Failed

            # Wait a second for the mount to respond before double checking that things are how we expect
            time.sleep(1)
            status = self._request_with_status('status')
            state = _parse_mountstate(status)

            return CommandStatus.Failed if state == MountState.Disabled else CommandStatus.Succeeded

    @Pyro4.expose
    def shutdown(self):
        """Disables motor power and disconnects from mount"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.Disabled:
                return CommandStatus.MountNotInitialized

            if state != MountState.Parked and not self._set_axis_power(False):
                return CommandStatus.Failed

            if self._request_with_status('mount/disconnect') is None:
                log.error(self._config.log_name, 'Failed to disconnect from mount')
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def stop(self):
        """Stops any active telescope movement"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with self._pointing_condition:
            self._force_stopped = True

            status = self._request_with_status('mount/stop')
            state = _parse_mountstate(status)
            if state == MountState.Disabled:
                return CommandStatus.MountNotInitialized

            self._pointing_condition.notify_all()

        # Block until any other pointing commands have terminated before cleaning up
        with self._command_lock:
            self._force_stopped = False

        return CommandStatus.Succeeded

    @Pyro4.expose
    def slew_altaz(self, alt_deg, az_deg):
        """Moves the telescope to a specified alt, az"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            # Check against telescope limits
            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            location = _parse_location(status)
            coords = SkyCoord(alt=alt_deg, az=az_deg, unit=u.deg, frame='altaz',
                              location=location, obstime=Time.now())

            valid_status = self._validate_pointing(coords, f'failed to move to alt {alt_deg} az {az_deg}')
            if valid_status != CommandStatus.Succeeded:
                return valid_status

            success = self._point('mount/goto_alt_az', MountState.Stopped, alt_degs=alt_deg, az_degs=az_deg)
            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def park(self, position):
        """Moves the telescope to a named park position and disables axis power"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            if position not in self._config.park_positions:
                return CommandStatus.UnknownParkPosition

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            park = self._config.park_positions[position]
            if not self._point('mount/goto_alt_az', MountState.Stopped, alt_degs=park['alt'], az_degs=park['az']):
                return CommandStatus.Failed

            if not self._set_axis_power(False):
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def slew_hadec(self, ha_deg, dec_deg):
        """Moves the telescope to a specified apparent ha, dec"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            # Convert apparent HA, Dec to Alt, Az for pointing
            now = Time.now()
            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            location = _parse_location(status)
            coords = SkyCoord(
                ra=Time(now, location=location).sidereal_time('apparent') - ha_deg * u.deg,
                dec=dec_deg * u.deg,
                frame=FK5(equinox=now),
                obstime=now).transform_to(AltAz(obstime=now, location=location))

            # Check against telescope limits
            valid_status = self._validate_pointing(coords, f'failed to move to ha {ha_deg} dec {dec_deg}')
            if valid_status != CommandStatus.Succeeded:
                return valid_status

            success = self._point('mount/goto_alt_az', MountState.Stopped,
                                  alt_degs=coords.alt.to_value(u.deg),
                                  az_degs=coords.az.to_value(u.deg))
            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def slew_radec(self, ra_deg, dec_deg):
        """Moves the telescope to a specified J2000 ra, dec"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            # Convert apparent RA, Dec to Alt, Az for pointing
            now = Time.now()
            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            location = _parse_location(status)
            coords = SkyCoord(
                ra=ra_deg * u.deg,
                dec=dec_deg * u.deg,
                frame='icrs').transform_to(AltAz(obstime=now, location=location))

            # Check against telescope limits
            valid_status = self._validate_pointing(coords, f'failed to move to ra {ra_deg} dec {dec_deg}')
            if valid_status != CommandStatus.Succeeded:
                return valid_status

            success = self._point('mount/goto_alt_az', MountState.Stopped,
                                  alt_degs=coords.alt.to_value(u.deg),
                                  az_degs=coords.az.to_value(u.deg))
            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def track_radec(self, ra_deg, dec_deg):
        """Moves the telescope to a specified J2000 ra, dec and track at the sidereal rate"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            location = _parse_location(status)
            coords = SkyCoord(
                ra=ra_deg * u.deg,
                dec=dec_deg * u.deg,
                frame='icrs',
                obstime=Time(Time.now(), location=location))

            # Check against telescope limits
            valid_status = self._validate_pointing(coords, f'failed to move to ra {ra_deg} dec {dec_deg}')
            if valid_status != CommandStatus.Succeeded:
                return valid_status

            success = self._point('mount/goto_ra_dec_j2000', MountState.Tracking,
                                  ra_hours=coords.ra.to_value(u.hourangle),
                                  dec_degs=coords.dec.to_value(u.deg))
            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def offset_radec(self, ra_delta_deg, dec_delta_deg):
        """Offsets the telescope relative to the current position"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            location = _parse_location(status)
            now = Time(Time.now(), location=location)

            if state == MountState.Tracking:
                coords = SkyCoord(
                    ra=u.Quantity(status['mount.ra_j2000_hours'], unit=u.hourangle) + ra_delta_deg * u.deg,
                    dec=u.Quantity(status['mount.dec_j2000_degs'], unit=u.deg) + dec_delta_deg * u.deg,
                    frame='icrs',
                    obstime=now)

                # Check against telescope limits
                valid_status = self._validate_pointing(coords,
                                                       f'failed to offset by ra {ra_delta_deg} dec {dec_delta_deg}')
                if valid_status != CommandStatus.Succeeded:
                    return valid_status

                success = self._point('mount/offset', state,
                                      ra_add_arcsec=3600 * ra_delta_deg,
                                      dec_add_arcsec=3600 * dec_delta_deg)
            else:
                # The mount/offset command only works when the mount is tracking,
                # so issue a mount/goto_alt_az command instead
                coords = SkyCoord(
                    alt=status['mount.altitude_degs'],
                    az=status['mount.azimuth_degs'],
                    unit=u.deg,
                    frame='altaz',
                    obstime=now,
                    location=location).transform_to(ICRS)
                coords = SkyCoord(
                    ra=coords.ra + ra_delta_deg * u.deg,
                    dec=coords.dec + dec_delta_deg * u.deg,
                    frame='icrs').transform_to(AltAz(obstime=now, location=location))

                # Check against telescope limits
                valid_status = self._validate_pointing(coords,
                                                       f'failed to offset by ra {ra_delta_deg} dec {dec_delta_deg}')
                if valid_status != CommandStatus.Succeeded:
                    return valid_status

                success = self._point('mount/goto_alt_az', MountState.Stopped,
                                      alt_degs=coords.alt.to_value(u.deg),
                                      az_degs=coords.az.to_value(u.deg))
            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def track_tle(self, line1, line2, line3):
        """Tracks a TLE
           WARNING: Telescope limits are not enforced: it is up to the caller to ensure we don't point at the floor!
        """
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            state = _parse_mountstate(status)
            if state == MountState.NotHomed:
                return CommandStatus.MountNotHomed
            if state == MountState.Parked and not self._set_axis_power(True):
                return CommandStatus.Failed

            success = self._point('mount/follow_tle', MountState.Tracking,
                                  line1=line1, line2=line2, line3=line3)

            return CommandStatus.Succeeded if success else CommandStatus.Failed

    @Pyro4.expose
    def add_pointing_model_point(self, ra_j2000_deg, dec_j2000_deg):
        """
        Add a point to PWI4's pointing model, mapping the current
        pointing direction to the specified J2000 coordinates.
        """
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        with TryLock(self._command_lock) as success:
            if not success:
                return CommandStatus.Blocked

            status = self._request_with_status('status')
            if _parse_mountstate(status) != MountState.Tracking:
                return CommandStatus.Failed

            status = self._request_with_status('mount/model/add_point',
                                               ra_j2000_hours=ra_j2000_deg / 15,
                                               dec_j2000_degs=dec_j2000_deg)

            return CommandStatus.Failed if status is None else CommandStatus.Succeeded

    @Pyro4.expose
    def ping(self):
        """Returns immediately with a success status"""
        return CommandStatus.Succeeded


if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Telescope Server')
    parser.add_argument('config', help='Path to configuration json file')
    args = parser.parse_args()
    c = Config(args.config)
    c.daemon.launch(MountDaemon(c))