#! /usr/bin/env python
# -*- Encoding: utf-8 -*-
# ==================================================================
#
# DICE / StarDICE@OHP
#
# Serial interface (minimal) to the Takahashi EM-200 mount (LUPM)
#
# Authors: Johann Cohen-Tanugi, Laurent Le Guillou, Kelly Joaquina
#
# Code History:
# Merged code from Temma*.py from J. Cohen-Tanugi with standard
# "robustified" RS-232 device code from L. Le Guillou.
# Extensive tests of the code with the Takahashi EM-200
# in June 2016 (LLG, KJ)
#
# ==================================================================

import sys
import os, os.path
import time
import serial

# ==================================================================
# Useful conversion for the Temma language

def dms2temmadec(deg, arcmin, arcsec):
    """
    Return dms into temma dec format :
    example : +48:05:30 -> +48055
    """ 

    # DEC [space] when Dec = 00000
    # Degrees (0 - 89)
    # Minutes (0 - 59)
    # Seconds (0 - 9) 1/10 Minute

    if deg>=0:
        dec_temma = "+%02d%02d%01d"%(deg,arcmin,arcsec/6.)
    else:
        dec_temma = "-%02d%02d%01d"%(abs(deg),arcmin,arcsec/6.)
    return dec_temma


def temmadec2dms(temmadec):
    """
    Convert back from temma dec format :
    example : +48055 -> +48:05:30
    example : -48055 -> -48:05:30
    """ 

    # DEC [space] when Dec = 00000
    # Degrees (0 - 89)
    # Minutes (0 - 59)
    # Seconds (0 - 9) 1/10 Minute

    if len(temmadec) != 6:
        raise ValueError("Wrong format for temma dec value[%s]: " +
                         "should be '+99999'" % temmadec)

    sign = 1
    if temmadec[0] == '-':
        sign = -1

    deg = sign * int(temmadec[1:3])
    arcmin = int(temmadec[3:5])
    arcsec = int(temmadec[5]) * 6
        
    return deg, arcmin, arcsec


def hms2temmara(hour, minute, sec, decimal=True):
    """
    Return dms into temma ra format :
    example : +48:05:30 -> +48055 if decimal is True
    or 480530 if decimal is False (lst format) 
    """

    # RA :
    # Hour (0 - 23)
    # Minutes (0 - 59)
    # Seconds (0 - 99) 1/100 Minute

    if decimal:
        return "%02d%02d%02d"%(hour, minute, (sec*1.666666667)) 
    else:
        return "%02u%02u%02u"%(hour, minute, sec)


def temmara2hms(temmara):
    """
    Convert back from temma ra format :
    """ 
    # RA :
    # Hour (0 - 23)
    # Minutes (0 - 59)
    # Seconds (0 - 99) 1/100 Minute

    if len(temmara) != 6:
        raise ValueError("Wrong format for temma ra value[%s]: " +
                         "should be '999999'" % temmara)

    hour = int(temmara[0:2])
    minute = int(temmara[2:4])
    sec = int(temmara[4:6])*0.6
        
    return hour, minute, sec

def dms2temmadec(deg, arcmin, arcsec):
    """
    Return dms into temma dec format :
    example : +48:05:30 -> +48055
    """ 

    # DEC [space] when Dec = 00000
    # Degrees (0 - 89)
    # Minutes (0 - 59)
    # Seconds (0 - 9) 1/10 Minute

    if deg>=0:
        dec_temma = "+%02d%02d%01d"%(deg,arcmin,arcsec/6.)
    else:
        dec_temma = "-%02d%02d%01d"%(abs(deg),arcmin,arcsec/6.)
    return dec_temma


def temmadec2dms(temmadec):
    """
    Convert back from temma dec format :
    example : +48055 -> +48:05:30
    example : -48055 -> -48:05:30
    """ 

    # DEC [space] when Dec = 00000
    # Degrees (0 - 89)
    # Minutes (0 - 59)
    # Seconds (0 - 9) 1/10 Minute

    if len(temmadec) != 6:
        raise ValueError("Wrong format for temma dec value[%s]: " +
                         "should be '+99999'" % temmadec)

    sign = 1
    if temmadec[0] == '-':
        sign = -1

    deg = sign * int(temmadec[1:3])
    arcmin = int(temmadec[3:5])
    arcsec = int(temmadec[5]) * 6
        
    return deg, arcmin, arcsec


def hms2temmara(hour, minute, sec, decimal=True):
    """
    Return dms into temma ra format :
    example : +48:05:30 -> +48055 if decimal is True
    or 480530 if decimal is False (lst format) 
    """

    # RA :
    # Hour (0 - 23)
    # Minutes (0 - 59)
    # Seconds (0 - 99) 1/100 Minute

    if decimal:
        return "%02d%02d%02d"%(hour, minute, (sec*1.666666667)) 
    else:
        return "%02u%02u%02u"%(hour, minute, sec)


def temmara2hms(temmara, decimal=True):
    """
    Convert back from temma ra format :
    """ 
    # RA :
    # Hour (0 - 23)
    # Minutes (0 - 59)
    # Seconds (0 - 99) 1/100 Minute

    if len(temmara) != 6:
        raise ValueError("Wrong format for temma ra value[%s]: " +
                         "should be '999999'" % temmara)

    hour = int(temmara[0:2])
    minute = int(temmara[2:4])
    if decimal:
        sec = int(temmara[4:6])*0.6
    else:
        sec = int(temmara[4:6])
        
    return hour, minute, sec

    
# ==================================================================

from exceptions import Exception

class MountError(Exception):
    pass


# ============ Class Temma controller ==============================

class Temma(object):
    """
    The Temma class represents the Temma controller 
    for the Takahashi EM-200 mount.
    """

    # ---------- Constructor ---------------------------------

    def __init__(self,
                 # port = '/dev/ttyUSB0',
                 port = '/dev/temma',
                 serial_port = None, # to provide if serial port already created
                 debug = True):

        self.port = port
        self.serial_port = serial_port
        self.baudrate = 19200
        self.timeout = 0.5 # Non-blocking & non waiting mode
        self.repr_mode = 0
        self.parity = serial.PARITY_EVEN
        self.bytesize = serial.EIGHTBITS
        self.stopbits = serial.STOPBITS_ONE
        self.rtscts = True
        self.serial_port = None
        self.EOL = '\r\n'
    
        # ---- debug mode
        self.debug = debug

        # ---- action timeout 
        self.action_timeout = 10


    # ---------- Open the controller device ------------------

    def open(self):
        """
        Open the Temma device. 
        Check if there's something connected (echotest).
        """

        #create the internal serial port instance
        if self.serial_port == None:
            if self.debug: print("Opening port %s ..." % self.port, file=sys.stderr)
     
            self.serial_port = serial.Serial(port = self.port, 
                                             baudrate = self.baudrate,
                                             bytesize = self.bytesize, 
                                             parity = self.parity,
                                             stopbits = self.stopbits,
                                             rtscts = self.rtscts,
                                             timeout = self.timeout)
        
            if ( (self.serial_port == None) or
                 not(self.serial_port.isOpen()) ):
                raise IOError("Failed to open serial port %s" % self.port)


        # if serial port has already been created
        # check whether it is open, and if not reopen it:
        elif self.serial_port != None and not self.serial_port.isOpen():
            self.serial_port.open()
            #paranoid flush
            self.serial_port.flushOutput()

        #port is already open
        else:
                if self.debug: print(( "port %s already open"%self.port), file=sys.stderr)
            
        
        if not(self.echotest()):
            raise IOError(("Not echoing on serial port %s") % 
                          self.port)
           
        if self.debug: 
            print(( "Opening port %s done." % 
                                  self.port ), file=sys.stderr)
        
        # self.clear()


    # ---------- Close the controller device ----------------- 

    def close(self):
 
        if ( self.serial_port and
             self.serial_port.isOpen() ):
            self.serial_port.close()

        self.serial_port = None
        
    # ----------------- write command  ----------------------- 

    def write(self, command):
        """
        Send a command through the serial port.
        """
        if not( self.serial_port and
                self.serial_port.isOpen() ):
            raise IOError("Port %s not yet opened" % self.port)

        if self.debug: print("Sending command [" + command + "]", file=sys.stderr)
        # A trailing space is required
        self.serial_port.write(command + self.EOL)

    # ----------------- read command  ----------------------- 

    def read(self, timeout = None):
        """
        Read the answer from the serial port.
        Return it as a string.
        If <timeout> is specified, the function will wait
        for data with the specified timeout (instead of the default one). 
        """
        
        if not( self.serial_port and
                self.serial_port.isOpen() ):
            raise IOError("Port %s not yet opened" % self.port)

        if self.debug: print("Reading serial port buffer", file=sys.stderr)

        if timeout != None:
            self.serial_port.timeout = timeout
            if self.debug: print("Timeout specified: ", timeout, file=sys.stderr)
            
        answer = self.serial_port.readline() # return buffer
        
        # Restoring timeout to default one
        self.serial_port.timeout = self.timeout
        # answer = answer.strip()
        if self.debug: print("Received [" + str(answer) + "]", file=sys.stderr)

        return answer

    # ----------------- Purge the serial port --------------- 

    def purge(self):
        """
        Purge the serial port to avoid framing errors.
        """
        self.serial_port.flushOutput()
        self.serial_port.flushInput()
        self.serial_port.readlines()

    # ---------- Echo test ---------------------------------- 

    def echotest(self):
        cmd = "v" + self.EOL
        print(cmd)
        self.serial_port.write(cmd)
        time.sleep(1)
        answer = self.serial_port.readline()
        print("echotest: answer=[%s]" % answer)
        # if (len(answer) < 3) or (answer[0:3] != 'ver'):
        if answer == '':
            return False
        return True

    # ---------- Send a command and get the answer -----------

    def send(self, cmd):
        """
        Send a command (Temma language) to the motor and
        return the answer (if any).

        @param cmd: the command to send.
        """
        command = cmd 
        # Now send it
        self.write(command)
        # Parsing the answer (to detect errors)
        answer = self.read()

        return answer

    # ---------- Get the Temma2 version -----------------

    def get_version(self):
        """
        Return the version string.
        """
        answer = self.send("v")
        serial = answer.strip()[4:]
        return serial

    serial = property(get_version, doc="Version")

    # ---------- Setup: set latitude ------------------------- 

    def set_latitude(self, d, m, s):
        lat_temma = dms2temmadec(d, m, s)
        #  '+43559'
        answer = self.send("I%s" % lat_temma)
        return True
    
    def get_latitude(self):
        # DEC [space] when Dec = 00000
        # Degrees (0 - 89)
        # Minutes (0 - 59)
        # Seconds (0 - 9) 1/10 Minute
        answer = self.send("i")
        #returns 'i+<DMD>\r\n'
        #in dmd format, e.g. DMD=+48050=+48d05.5'
        # print "answer = ", answer
        # print str(answer)
        if answer[0] == 'i':
            lat_temma = answer.strip()[1:7]
        else:
            lat_temma = answer.strip()[0:6]
        # return lat_temma
        return temmadec2dms(lat_temma)

    # ---------- Setup: set sideral time (LST) --------------- 

    def set_LST(self, hour, minute, sec):
        lst_temma = hms2temmara(hour, minute, sec)
        # RA :
        # Hour (0 - 23)
        # Minutes (0 - 59)
        # Seconds (0 - 99) 1/100 Minute
        answer = self.send("T%s" %str(lst_temma))
        return True
        
    def get_LST(self):
        answer = self.send("g")
        # if len(answer) < 7:
        #     raise IOError(("Not responding to E command" +
        #                    "on serial port %s") %
        #                   self.port)

        print("answer = ", answer)
        print(str(answer))
        ## TODO: conversion!!!
        # return lat.strip()[1:]
        if answer[0] == 'g':
            lst_temma = answer.strip()[1:7]
        else:
            lst_temma = answer.strip()[0:6]
        # return lat_temma

        hour,minute,sec = temmara2hms(lst_temma)
        return hour, minute, sec
    
    # ---------- Init at Zenith ------------------------------ 

    def zenith(self):
        """
        Initialisation: put the telescope to zenith
        and call this method.
        """
        answer = self.send("Z")
        return True

    # ---------- Current motor position ---------------------- 

    def get_radec(self):
        """
        Return current RA, DEC, E/W, H???
        """
        answer = self.send("E")
        if len(answer) < 12:
            raise IOError(("Not responding to E command" +
                           "on serial port %s") %
                          self.port)
        print(answer)
        temmara = answer[1:7]
        temmadec = answer[7:13]

        hour, minute, sec = temmara2hms(temmara)
        deg, arcmin, arcsec = temmadec2dms(temmadec)
        
        return hour, minute, sec, deg, arcmin, arcsec

    # position = property(get_position, doc="Axis current position")

    # ---------- Define current position (???)---------------- 

    def define_radec(self,
                     hour, minute, sec,
                     deg, arcmin, arcsec):
        """
        Set current RA, DEC (initialisation).
        """

        temmara = hms2temmara(hour, minute, sec)
        temmadec = dms2temmadec(deg, arcmin, arcsec)

        command = "D%s%s" % (temmara, temmadec)

        answer = self.send(command)
        answer = answer.strip()

        # R *
        # ### retour si ok : R0\r\n sinon R1 ou R2 ou R3 ou R4 ou R5
        # R0 = Ok
        # R1 = RA Error
        # R2 = Dec Error
        # R3 = Too many digits
        # R4 = objet sous ligne d'horizon
        # R5 = etat standby ON (soit moteurs arretes)
        # Note : la monture gere elle-meme les retournements du telescope
        # afin d'eviter au mieux les rapprochements materiel critiques.
        # Le firmware provoque ainsi un retournement du telescope cote Est
        # de la monture en cas de goto cote Ouest du meridien.
        
        if answer == "R0":
            return True

        if answer == "R1":
            raise MountError("RA Error")

        if answer == "R2":
            raise MountError("DEC Error")

        if answer == "R3":
            raise MountError("Too many digits")

        if answer == "R4":
            raise MountError("Target is below horizon")

        if answer == "R5":
            raise MountError("RA motors in standby (standby on)")

        return answer


    # ---------- Goto RA, DEC (current epoch) ---------------- 

    def goto_radec(self,
                   hour, minute, sec,
                   deg, arcmin, arcsec):
        """
        Point to RA, DEC ('goto')
        """

        if self.is_pointing():
            raise MountError("Already moving (goto command ongoing)")
        
        temmara = hms2temmara(hour, minute, sec)
        temmadec = dms2temmadec(deg, arcmin, arcsec)

        command = "P%s%s" % (temmara, temmadec)

        answer = self.send(command)
        answer = answer.strip()

        # R *
        # ### retour si ok : R0\r\n sinon R1 ou R2 ou R3 ou R4 ou R5
        # R0 = Ok
        # R1 = RA Error
        # R2 = Dec Error
        # R3 = Too many digits
        # R4 = objet sous ligne d'horizon
        # R5 = etat standby ON (soit moteurs arretes)
        # Note : la monture gere elle-meme les retournements du telescope
        # afin d'eviter au mieux les rapprochements materiel critiques.
        # Le firmware provoque ainsi un retournement du telescope cote Est
        # de la monture en cas de goto cote Ouest du meridien.
        
        if answer == "R0":
            return True

        if answer == "R1":
            raise MountError("RA Error")

        if answer == "R2":
            raise MountError("DEC Error")

        if answer == "R3":
            raise MountError("Too many digits")

        if answer == "R4":
            raise MountError("Target is below horizon")

        if answer == "R5":
            raise MountError("RA motors in standby (standby on)")

        return answer

    # ---------- Is it on E/W side ? ------------------------- 

    def get_side(self):
        """
        Tell us if the telescope is on the East(E) or West(W) side.
        """
        # We should ask get_raded (E) at least 4 times !!!
        # After a goto it will answer 3 times 'F'
        # !!!

        for i in range(4):
            answer = self.send("E")

        return answer[13]

    # ---------- Tell the mount to change side (E/W) --------- 

    def flip_side(self):
        """
        Switch the mount side (E/W). Does not move the mount,
        but tell it on which side it actually is.
        IMPORTANT to avoid collision of the telescope tube.
        See also the zenith() method.
        When turned on, the mount believes it is on the West side.
        Please check with get_side() after!
        """
        answer = self.send("PT")
        return answer

    # ---------- Stop goto ----------------------------------- 
    
    def stop(self):
        """
        Stop an on-going command.
        """
        answer = self.send("PS")
        return answer
    
    # ---------- Is the telescope doing a goto ? ------------- 

    def is_pointing(self):
        """
        A goto command is ongoing ?
        """
        answer = self.send("s")
        if len(answer) < 2:
            raise IOError(("Not responding to 's' on serial port %s") % 
                          self.port)
        if answer[1] == '1':
            return True
        else:
            return False
        
    # ---------- Small movements (simulating handset) -------- 

    def move(self, direction, fast = False):
        ''' 
        move N/S/W/E as when using the handset.
        speed is defined by the correction speed.
        Depending on the E/W position of the scope,
        the S/N direction is reversed.
        '''
        # Check that no goto is currently executed

        if self.is_pointing():
            raise MountError('GOTO running, you need to stop GOTO first')

        direction = direction.upper()

        # Are we on the East or West side

        north = 0
        south = 0
        east  = 0
        west  = 0
        high_speed = 0

        total=64; #010abcdy
        # throttle high speed (HS)
        if fast:
            high_speed = 1
        # if self.encoder == 0:
        #     total+=32 #011abcdy

        side = self.get_side()

        if side == "W":
            if direction == "N":
                south = 1
            elif direction == "S":
                north = 1
        elif side == "E":
            if direction == "N":
                north = 1
            elif direction == "S":
                south = 1

        if direction == "E":
            east = 1
        elif direction == "W":
            west = 1
        
        total += 16*north + 8*south + 4*east + 2*west + high_speed
        answer = self.send("M%c" % total)
        return answer

    # ---------- Standby mode (on = RA motors off) ----------- 

    def standby(self, on=True):
        """
        Initialisation: put the telescope RA motors on standby
        standby ON  = RA OFF
        standby OFF = RA ON
        """
        if on:
            answer = self.send("STN-ON")
        else:
            answer = self.send("STN-OFF")

        return answer

    def is_standby(self):
        """
        Is the telescope RA motors in stand-by ?
        """
        answer = self.send("STN-COD")
        answer = answer.strip()

        if answer == "stn-off":
            return False

        return True
    
# ===================================================================
if __name__ == "__main__":
    mount = Temma(port='/dev/temma')