#! /usr/bin/env python
#
# This is open-source software licensed under a BSD license.
# Please see the file LICENSE.txt for details.
#
from __future__ import print_function, absolute_import, unicode_literals, division
import sys

import numpy as np
from astropy.coordinates.matrix_utilities import rotation_matrix

from hcam_widgets.gtc.corba import get_telescope_server
from hcam_widgets.gtc.headers import create_header_from_telpars


def calculate_sky_offset(xoff, yoff, sky_pa):
    """
    Convert pixel offset to sky offset in arcseconds

    Arguments
    ---------
    xoff, yoff : float
        desired pixel offsets
    sky_pa : float
        current instrument PA
    """
    px_scale = 0.081  # arcseconds per pixel
    flipEW = True  # is E to the right in the image?
    EofN = True  # does increasing rotator PA move us to E?
    paOff = 76.97  # pa when rotator = 0

    if EofN:
        theta = sky_pa - paOff
    else:
        theta = -sky_pa - paOff
    # only take 3d of 3d rotation matrix
    rmat = rotation_matrix(theta)[:2, :2]

    # +ve shifts should move stars right and up
    # offset for skyPA = 0 are:
    ra_shift_arcsecs = -xoff*px_scale if flipEW else xoff*px_scale
    dec_shift_arcsecs = -yoff*px_scale
    pix_shift = np.array([ra_shift_arcsecs, dec_shift_arcsecs])
    return rmat.dot(pix_shift)


if __name__ == "__main__":

    carry_on = True
    s = get_telescope_server()
    try:
        s.getTelescopeParams()
    except:
        print('Cannot communicate with telescope - is GTC telescope server running?')
        sys.exit(1)

    msg = 'Send pointing offsets to GTC:\n'
    msg += '(+ve offset moves stars right and up)'
    while carry_on:
        try:
            xoff, yoff = input('> Enter desired x and y offset ')
            xoff, yoff = float(xoff), float(yoff)

            hdr = create_header_from_telpars(s.getTelescopeParams())
            sky_pa = float(hdr['INSTRPA'])
            raoff, decoff = calculate_sky_offset(xoff, yoff, sky_pa)

            s.requestTelescopeOffset(raoff, decoff)
        except KeyboardInterrupt:
            carry_on = False
