talon_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 telescope through Talon/telescoped/csimcd via Pyro"""

import argparse
import glob
import math
import os
import signal
import subprocess
import threading
import time
import sysv_ipc
from astropy.coordinates import SkyCoord, EarthLocation, get_moon, get_sun
from astropy.time import Time
import astropy.units as u
import Pyro4

from rockit.common import log, TryLock
from rockit.common.helpers import pyro_client_matches
from rockit.mount.talon import (
    CommandStatus, TelState, FocusState,
    Config, ShmOffsets, shm_read_double, shm_read_int, shm_read_ushort)


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

        # Motor offsets for jogging and autoguiding
        self._offset_ra = 0 * u.deg
        self._offset_dec = 0 * u.deg

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

        # Status data from shared memory
        self._talon_shm = None
        self._talon_shm_lock = threading.Lock()

        self._observatory = None
        self._current_ra_j2000 = 0 * u.rad
        self._current_dec_j2000 = 0 * u.rad
        self._current_ha_apparent = 0 * u.rad
        self._current_dec_apparent = 0 * u.rad
        self._current_alt = 0 * u.rad
        self._current_az = 0 * u.rad
        self._current_lst = 0 * u.rad

        self._last_pointing_state = TelState.Absent
        self._pointing_state = TelState.Absent
        self._last_pointing_idx = 0
        self._pointing_idx = 0
        self._axes_homed = False

        self._last_telescope_focus_state = FocusState.Absent
        self._telescope_focus_state = FocusState.Absent
        self._telescope_focus_um = 0
        self._last_telescope_focus_um = 0

        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._last_talon_mjd = 0
        self._talon_mjd = 0
        self._talon_pid = 0

        tel_status_thread = threading.Thread(target=self.__poll_tel_status)
        tel_status_thread.daemon = True
        tel_status_thread.start()

    def __update_state_from_talon_shm(self):
        """Update local copies of state variables from talon shared memory segment"""
        try:
            if self._talon_shm is None:
                self._talon_shm = sysv_ipc.SharedMemory(ShmOffsets.Key)

            with self._talon_shm_lock:
                self._last_talon_mjd = self._talon_mjd
                self._talon_mjd = shm_read_double(self._talon_shm, ShmOffsets.MJD)

                # Talon stops updating the shared memory if the hardware crashes
                # We use this as a proxy for checking that the daemons are operating correctly
                talon_alive = 0 < self._talon_mjd != self._last_talon_mjd

                # The first stage of talon boot up zeros the shared memory.
                # Wait for it to be updated with sensible data
                if self._talon_mjd > 0:
                    self._last_pointing_state = self._pointing_state
                    self._last_pointing_idx = self._pointing_idx
                    pointing_state = shm_read_int(self._talon_shm, ShmOffsets.TelState)
                    if not talon_alive:
                        pointing_state = TelState.Absent

                    self._pointing_state = pointing_state
                    self._pointing_idx = shm_read_int(self._talon_shm, ShmOffsets.TelStateIdx)

                    self._current_ra_j2000 = shm_read_double(self._talon_shm, ShmOffsets.RAJ2000) * u.rad
                    self._current_dec_j2000 = shm_read_double(self._talon_shm, ShmOffsets.DecJ2000) * u.rad

                    self._current_ha_apparent = shm_read_double(self._talon_shm, ShmOffsets.HAApparent) * u.rad
                    self._current_dec_apparent = shm_read_double(self._talon_shm, ShmOffsets.DecApparent) * u.rad
                    self._current_lst = shm_read_double(self._talon_shm, ShmOffsets.LST) * u.rad

                    self._current_alt = shm_read_double(self._talon_shm, ShmOffsets.Alt) * u.rad
                    self._current_az = shm_read_double(self._talon_shm, ShmOffsets.Az) * u.rad

                    ra_flags = shm_read_ushort(self._talon_shm, ShmOffsets.RAFlags)
                    dec_flags = shm_read_ushort(self._talon_shm, ShmOffsets.DecFlags)
                    focus_flags = shm_read_ushort(self._talon_shm, ShmOffsets.FocusFlags)
                    focus_step = shm_read_int(self._talon_shm, ShmOffsets.FocusStep)
                    focus_pos = shm_read_double(self._talon_shm, ShmOffsets.FocusCPos)
                    focus_df = shm_read_double(self._talon_shm, ShmOffsets.FocusDF)

                    self._last_telescope_focus_state = self._telescope_focus_state
                    self._telescope_focus_state = FocusState.Absent
                    if focus_flags & 0x01 == 1:
                        self._telescope_focus_state = FocusState.NotHomed
                        if focus_flags & 0x80 != 0:
                            self._telescope_focus_state = FocusState.Homing
                        if focus_flags & 0x80 != 0:
                            self._telescope_focus_state = FocusState.Homing
                        elif focus_flags & 0x100 != 0:
                            self._telescope_focus_state = FocusState.Limiting
                        elif focus_flags & 0x200 != 0:
                            self._telescope_focus_state = FocusState.Ready

                    self._last_telescope_focus_um = self._telescope_focus_um
                    self._telescope_focus_um = focus_step * focus_pos / (2 * math.pi * focus_df)

                    self._axes_homed = ra_flags & 0x200 != 0 and dec_flags & 0x200 != 0 and \
                                       (focus_flags & 0x01 == 0 or focus_flags & 0x200 != 0)

        except sysv_ipc.ExistentialError:
            # Talon has not been initialized since last boot
            talon_alive = False
        except Exception as e:
            # Some other unexpected error occurred - log it
            log.error(self._config.log_name, 'Failed parsing shared memory: ' + str(e))
            talon_alive = False

        if not talon_alive and self._last_pointing_state != TelState.Absent:
            # Talon has just died on us!
            log.info(self._config.log_name, 'Talon is offline')

            # Kill telescoped and csimcd
            log.info(self._config.log_name, 'Killing rund-spawned daemons')
            subprocess.call(['/usr/bin/killall', 'rund'])

            # Clear interprocess-communication channels
            for f in glob.glob('/usr/local/telescope/comm/*'):
                print('Removing ' + f)
                try:
                    os.unlink(f)
                except Exception:
                    pass

            self._talon_pid = 0

            # Wake up any active commands so that they can fail
            self._pointing_condition.notify()
        elif talon_alive and self._last_pointing_state == TelState.Absent:
            # Either we or talon has been restarted... find current status
            log.info(self._config.log_name, 'Talon is online')

            with self._talon_shm_lock:
                self._talon_pid = shm_read_int(self._talon_shm, ShmOffsets.PID)

                self._observatory = EarthLocation(
                    lat=shm_read_double(self._talon_shm, ShmOffsets.Latitude) * u.rad,
                    lon=shm_read_double(self._talon_shm, ShmOffsets.Longitude) * u.rad,
                    height=shm_read_double(self._talon_shm, ShmOffsets.Elevation) * 6.37816e6 * u.m)

    def __poll_tel_status(self):
        """Background thread that polls shared memory for the current telescope status"""
        while True:
            # Acquire all condition locks to avoid races with commands
            with self._pointing_condition:
                with self._telescope_focus_condition:
                    self.__update_state_from_talon_shm()

                    if self._last_pointing_idx != self._pointing_idx:
                        last = TelState.label(self._last_pointing_state)
                        label = TelState.label(self._pointing_state)
                        print(f'Pointing: {last} ({self._last_pointing_idx}) -> {label} ({self._pointing_idx})')
                        self._pointing_condition.notify_all()

                    if self._last_telescope_focus_um != self._telescope_focus_um:
                        print(f'tel focus: {self._last_telescope_focus_um:2f} -> {self._telescope_focus_um:2f}')
                        self._telescope_focus_condition.notify_all()

                    if self._telescope_focus_state != self._last_telescope_focus_state:
                        last = FocusState.label(self._last_telescope_focus_state)
                        label = FocusState.label(self._telescope_focus_state)
                        print(f'Focus: {last} -> {label}')
                        self._telescope_focus_condition.notify_all()

            time.sleep(self._config.query_delay)

    def __reset_pointing_offset(self):
        """Resets the pointing xdelta"""
        if not fifo_write('Tel.in', 'xdelta(0,0)'):
            log.error(self._config.log_name, 'Failed to write to Tel.in')
            return False

        self._offset_ra = 0 * u.deg
        self._offset_dec = 0 * u.deg
        return True

    def __point(self, fifo_command, active_state, final_state, timeout):
        """Issues a pointing command and blocks until the final state is reached (or timeout)"""
        with self._pointing_condition:
            if not fifo_write('Tel.in', fifo_command):
                log.error(self._config.log_name, 'Failed to write to Tel.in')
                return False

            while True:
                self._pointing_condition.wait(timeout)
                stopped = self._force_stopped or self._pointing_state == TelState.Absent

                # Ignore the initial state change to <active_state>, which we may
                # or may not receive depending on the timing of the telstat loop
                if self._pointing_state == active_state and not stopped:
                    continue
                break

            if self._pointing_state != final_state or stopped:
                return False

        return True

    def __focus(self, fifo_command, active_state, final_state, timeout):
        """Issues a focus command and blocks until the final state is reached (or timeout)"""
        if self._telescope_focus_state == FocusState.Absent:
            return False

        with self._telescope_focus_condition:
            if not fifo_write('Focus.in', fifo_command):
                log.error(self._config.log_name, 'Failed to write to Focus.in')
                return False

            while True:
                self._telescope_focus_condition.wait(timeout)
                stopped = self._force_stopped or self._pointing_state == TelState.Absent

                # Ignore the initial state change to <active_state>, which we may
                # or may not receive depending on the timing of the telstat loop
                if self._telescope_focus_state == active_state and not stopped:
                    continue
                break

            if self._telescope_focus_state != final_state or stopped:
                return False

        return True

    @Pyro4.expose
    def report_status(self):
        """Returns a dictionary containing the current telescope state"""
        data = {
            'state': self._pointing_state,
            'state_label': TelState.label(self._pointing_state)
        }

        if self._observatory is not None:
            data.update({
                'site_latitude': self._observatory.lat.to_value(u.deg),
                'site_longitude': self._observatory.lon.to_value(u.deg),
                'site_elevation': self._observatory.height.to_value(u.m),
            })

        if self._pointing_state != TelState.Absent:
            data.update({
                'axes_homed': self._axes_homed,
                'lst': self._current_lst.to_value(u.deg),
            })

            if self._config.is_onemetre:
                data['telescope_focus_state'] = self._telescope_focus_state

            if self._axes_homed:
                timenow = Time(Time.now(), format='mjd', location=self._observatory)
                pointing = SkyCoord(self._current_ra_j2000, self._current_dec_j2000, frame='fk5')
                data.update({
                    'ra': self._current_ra_j2000.to_value(u.deg),
                    'dec': self._current_dec_j2000.to_value(u.deg),
                    'offset_ra': self._offset_ra.to_value(u.deg),
                    'offset_dec': self._offset_dec.to_value(u.deg),

                    'ha': self._current_ha_apparent.to_value(u.deg),

                    'alt': self._current_alt.to_value(u.deg),
                    'az': self._current_az.to_value(u.deg),

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

                if self._telescope_focus_state != FocusState.Absent:
                    data.update({
                        'telescope_focus_um': self._telescope_focus_um
                    })

        return data

    @Pyro4.expose
    def initialize(self):
        """Turns on the power and starts the lower level hardware daemons"""
        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 self._pointing_state != TelState.Absent:
                return CommandStatus.TelescopeNotUninitialized

            # Check hard limits (security system)
            if self._config.is_onemetre:
                try:
                    with self._config.security_system_daemon.connect() as daemon:
                        status = daemon.last_measurement()
                        key = self._config.security_system_key
                        if key not in status or not status[key]:
                            return CommandStatus.SecuritySystemTripped
                except Pyro4.errors.CommunicationError:
                    return CommandStatus.CannotCommunicateWithSecuritySystem

            # systemd starts talond with a clean environment
            # load the talon-specific environment just for talon
            talon_env = environment_from_source('/etc/profile.d/talon.sh')
            with open(os.devnull, 'w') as devnull:
                daemon_args = ['/usr/local/telescope/bin/rund', 'telescoped']
                if self._config.virtual:
                    daemon_args.append('-v')

                subprocess.call(daemon_args, env=talon_env, stdout=devnull, stderr=devnull)

            with self._pointing_condition:
                self._pointing_condition.wait(self._config.initialization_timeout)

            if self._pointing_state == TelState.Absent:
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def shutdown(self):
        """Disables and powers off the telescope"""
        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 self._pointing_state == TelState.Absent:
                return CommandStatus.TelescopeNotInitialized

            # Send a shutdown signal to telescoped, which will
            # stop any motion and gracefully exit
            # The main daemon thread will detect the shm data becoming stale
            # and clean up our internal state in talond
            os.kill(self._talon_pid, signal.SIGINT)

            return CommandStatus.Succeeded

    @Pyro4.expose
    def find_homes(self):
        """Finds the axis 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

            if self._pointing_state == TelState.Absent:
                return CommandStatus.TelescopeNotInitialized

            log.info(self._config.log_name, 'Homing HA axis')
            if not self.__point('homeH', TelState.Homing, TelState.Stopped, self._config.homing_timeout):
                return CommandStatus.Failed

            log.info(self._config.log_name, 'Homing Dec axis')
            if not self.__point('homeD', TelState.Homing, TelState.Stopped, self._config.homing_timeout):
                return CommandStatus.Failed

            if self._telescope_focus_state != FocusState.Absent:
                log.info(self._config.log_name, 'Homing Focus axis')
                if not self.__focus('home', FocusState.Homing, FocusState.Ready, self._config.homing_timeout):
                    return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def find_limits(self):
        """Performs the home and limit calibrations"""
        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 not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            slew_to_zenith = 'Alt: 1.570796 Az: 0'
            if not self.__point(slew_to_zenith, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            log.info(self._config.log_name, 'Finding HA limits')
            if not self.__point('limitsH', TelState.Limiting, TelState.Stopped, self._config.limit_timeout):
                return CommandStatus.Failed

            if not self.__point(slew_to_zenith, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            log.info(self._config.log_name, 'Finding Dec limits')
            if not self.__point('limitsD', TelState.Limiting, TelState.Stopped, self._config.limit_timeout):
                return CommandStatus.Failed

            if not self.__point(slew_to_zenith, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            if self._telescope_focus_state != FocusState.Absent:
                log.info(self._config.log_name, 'Finding Focus limits')
                if not self.__focus('limits', FocusState.Limiting, FocusState.Ready, self._config.limit_timeout):
                    return CommandStatus.Failed

                # TODO: Return to the nominal focus point (-1950 um)

            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

        if self._pointing_state == TelState.Absent:
            return CommandStatus.TelescopeNotInitialized

        # The stop command overrides all other commands
        self._force_stopped = True

        tel_fifo_succeeded = fifo_write('Tel.in', 'Stop')
        focus_fifo_succeeded = fifo_write('Focus.in', 'Stop')

        if not tel_fifo_succeeded:
            log.error(self._config.log_name, 'Failed to write to Tel.in')
            return CommandStatus.Failed

        if not focus_fifo_succeeded:
            log.error(self._config.log_name, 'Failed to write to Focus.in')
            return CommandStatus.Failed

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

        # TODO: Wait for state to update?

        return CommandStatus.Succeeded

    @Pyro4.expose
    def slew_altaz(self, alt_deg, az_deg):
        """Moves the telescope to a target"""
        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 not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            # Check against telescope limits
            timenow = Time(Time.now(), location=self._observatory)
            coords = SkyCoord(alt=alt_deg, az=az_deg, unit=u.deg, frame='altaz',
                              location=self._observatory, obstime=Time.now()).icrs
            ha = (timenow.sidereal_time('apparent') - coords.ra).wrap_at(12 * u.hourangle)
            dec = coords.dec

            if ha < self._ha_negative_limit or ha > self._ha_positive_limit:
                log.error(self._config.log_name,
                          f'failed to move to alt {alt_deg} az {az_deg}; ' +
                          f'ha {ha} outside limit ({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,
                          f'failed to move to alt {alt_deg} az {az_deg}; ' +
                          f'dec {dec} outside limit ({self._dec_negative_limit}, {self._dec_positive_limit})')
                return CommandStatus.OutsideDecLimits

            command = f'Alt: {(alt_deg * u.deg).to_value(u.rad)} Az: {(az_deg * u.deg).to_value(u.rad)}'
            if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            if not self.__reset_pointing_offset():
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def slew_hadec(self, ha_deg, dec_deg):
        """Moves the telescope to a target"""
        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 not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            # Check against telescope limits
            ha = ha_deg * u.deg
            dec = dec_deg * u.deg

            if ha < self._ha_negative_limit or ha > self._ha_positive_limit:
                log.error(self._config.log_name,
                          f'failed to move to ha {ha_deg} dec {dec_deg}; ' +
                          f'ha {ha_deg} outside limit ({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,
                          f'failed to move to ha {ha_deg} dec {dec_deg}; ' +
                          f'dec {dec_deg} outside limit ({self._dec_negative_limit}, {self._dec_positive_limit})')
                return CommandStatus.OutsideDecLimits

            command = f'HA: {ha.to_value(u.rad)} Dec: {dec.to_value(u.rad)}'
            if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            if not self.__reset_pointing_offset():
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def slew_radec(self, ra_deg, dec_deg):
        """Moves the telescope to a target"""
        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 not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            # Check against telescope limits
            timenow = Time(Time.now(), location=self._observatory)
            coords = SkyCoord(ra=ra_deg, dec=dec_deg, unit=u.deg, frame='icrs')
            ha = (timenow.sidereal_time('apparent') - coords.ra).wrap_at(12 * u.hourangle)
            dec = coords.dec.to(u.deg)

            if ha < self._ha_negative_limit or ha > self._ha_positive_limit:
                log.error(self._config.log_name,
                          f'failed to move to ra {ra_deg} dec {dec_deg}; ' +
                          f'ha {ha} outside limit ({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,
                          f'failed to move to ra {ra_deg} dec {dec_deg}; ' +
                          f'dec {dec_deg} outside limit ({self._dec_negative_limit}, {self._dec_positive_limit})')
                return CommandStatus.OutsideDecLimits

            command = f'HA: {ha.to_value(u.rad)} Dec: {dec.to_value(u.rad)}'
            if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            if not self.__reset_pointing_offset():
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def track_radec(self, ra_deg, dec_deg):
        """Moves the telescope to a target and starts tracking"""
        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 not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            # Check against telescope limits
            timenow = Time(Time.now(), location=self._observatory)
            coords = SkyCoord(ra=ra_deg, dec=dec_deg, unit=u.deg, frame='icrs')
            ha = (timenow.sidereal_time('apparent') - coords.ra).wrap_at(12 * u.hourangle)
            ra = coords.ra.to(u.deg)
            dec = coords.dec.to(u.deg)

            if ha < self._ha_negative_limit or ha > self._ha_positive_limit:
                log.error(self._config.log_name,
                          f'failed to move to ra {ra} dec {dec}; ' +
                          f'ha {ha} outside limit ({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,
                          f'failed to move to ra {ra} dec {dec}; ' +
                          f'dec {dec} outside limit ({self._dec_negative_limit}, {self._dec_positive_limit})')
                return CommandStatus.OutsideDecLimits

            # Slewing is much faster than tracking
            command = f'HA: {ha.to_value(u.rad)} Dec: {dec.to_value(u.rad)}'
            if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            # Do the final adjustment and start tracking
            command = f'RA: {ra.to_value(u.rad)} Dec: {dec.to_value(u.rad)} Epoch: 2000'
            if not self.__point(command, TelState.Hunting, TelState.Tracking, self._config.slew_timeout):
                return CommandStatus.Failed

            if not self.__reset_pointing_offset():
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @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

            if not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            if self._pointing_state in [TelState.Tracking, TelState.Hunting]:
                # Offsets are absolute relative to the original tracking coordinate, not cumulative
                offset_ra = self._offset_ra + ra_delta_deg * u.deg
                offset_dec = self._offset_dec + dec_delta_deg * u.deg

                # xdelta expects angles in degrees
                fifo_command = f'xdelta({offset_ra.to_value(u.deg)},{offset_dec.to_value(u.deg)})'
                if not fifo_write('Tel.in', fifo_command):
                    log.error(self._config.log_name, 'Failed to write to Tel.in')
                    return CommandStatus.Failed

                self._offset_ra = offset_ra
                self._offset_dec = offset_dec
            elif self._pointing_state == TelState.Stopped:
                # Otherwise just slew to the updated position
                with self._talon_shm_lock:
                    offset_ha_rad = (self._current_ha_apparent + ra_delta_deg * u.deg).to_value(u.rad)
                    offset_dec_rad = (self._current_dec_apparent + dec_delta_deg * u.deg).to_value(u.rad)

                command = f'HA: {offset_ha_rad} Dec: {offset_dec_rad}'
                if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                    return CommandStatus.Failed

                self._offset_ra += ra_delta_deg * u.deg
                self._offset_dec += dec_delta_deg * u.deg
            else:
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def park(self, position_name):
        """Parks the telescope in a named 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

            if not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            position = self._config.park_positions.get(position_name, None)
            if position is None:
                return CommandStatus.Failed

            if not self.__reset_pointing_offset():
                return CommandStatus.Failed

            command = f'park {position["ha_enc"]} {position["dec_enc"]}'
            if not self.__point(command, TelState.Slewing, TelState.Stopped, self._config.slew_timeout):
                return CommandStatus.Failed

            return CommandStatus.Succeeded

    @Pyro4.expose
    def telescope_focus(self, focus_um):
        """Moves the W1m's focus to a specified position"""
        if not pyro_client_matches(self._config.control_ips):
            return CommandStatus.InvalidControlIP

        if not self._config.is_onemetre:
            return CommandStatus.Failed

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

            if not self._axes_homed:
                return CommandStatus.TelescopeNotHomed

            if self._telescope_focus_state == FocusState.Absent:
                return CommandStatus.Failed

            # TODO: merge this into the __focus command
            with self._telescope_focus_condition:
                focus_offset = focus_um - self._telescope_focus_um
                if math.fabs(focus_offset) < self._config.focus_tolerance:
                    return CommandStatus.Succeeded

                if not fifo_write('Focus.in', str(focus_offset)):
                    log.error(self._config.log_name, 'Failed to write to Focus.in')
                    return CommandStatus.Failed

                while True:
                    self._telescope_focus_condition.wait(self._config.focus_timeout)
                    stopped = self._force_stopped or self._pointing_state == TelState.Absent
                    moved = self._telescope_focus_um != self._last_telescope_focus_um
                    complete = math.fabs(focus_um - self._telescope_focus_um) < self._config.focus_tolerance

                    # Keep waiting until we reach the requested focus, timeout (haven't moved),
                    # or have been explicitly stopped
                    if complete or stopped or not moved:
                        break

                return CommandStatus.Succeeded if complete else CommandStatus.Failed

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


def environment_from_source(source_path):
    """Returns a dictionary of environment variables obtained after sourcing a file
       into a clean environment"""
    try:
        command = ['bash', '-c', 'source ' + source_path + ' && env']
        output = subprocess.check_output(command, universal_newlines=True, timeout=5)
        ret = dict()
        for line in output.split('\n'):
            (key, _, value) = line.partition('=')
            ret[key] = value
        return ret
    except Exception:
        return dict()


def fifo_write(fifo_name, fifo_data):
    """Writes a string into the named fifo"""
    try:
        # Don't create a new file if the fifo doesn't exist
        fd = os.open('/usr/local/telescope/comm/' + fifo_name, os.O_WRONLY)
    except OSError:
        return False

    with os.fdopen(fd, 'w') as fifo:
        print('Writing to ' + fifo_name + ': ' + fifo_data)
        fifo.write(fifo_data + '\n')
        return True


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(TelescopeDaemon(c))