# -*- coding: utf-8 -*-

""" v0.2
"""

import struct
import SocketServer
import threading
import socket
import time

import serial

# Stellarium server params
HOST, PORT = "localhost", 9999
PACKET_SIZE = 20

# Merlin/Orion params
ENCODER_ZERO = 0x800000

# Driver params
SERIAL_PORT = "/dev/ttyS0"
SERIAL_BAUDRATE = 9600


class AbstractDriver(object):
    """ Base (abstract) class for bus drivers.
    """
    def __init__(self):
        """ Init the driver.
        """
        self._lock = threading.RLock()

    def init(self):
        """ Init the connection.
        """
        raise NotImplementedError("AbstractDriver.init() must be overidden")

    def shutdown(self):
        """ Shutdown the connection.
        """
        raise NotImplementedError("AbstractDriver.shutdown() must be overidden")

    def acquireBus(self):
        """ Acquire and lock the bus.
        """
        #print "AbstractDriver.acquireBus()"
        self._lock.acquire()

    def releaseBus(self):
        """ Unlock and release the bus.
        """
        #print "AbstractDriver.releaseBus()"
        self._lock.release()

    def setTimeout(self, timeout):
        """ Set the timeout delay.

        @param timeout: timeout delay, in s
        @type timeout: float
        """
        raise NotImplementedError("AbstractDriver.setTimeout() must be overloaded")

    def empty(self):
        """ Empty buffer.
        """
        raise NotImplementedError("AbstractDriver.empty() must be overloaded")

    def write(self, data):
        """ Write data.

        @param data: data to write
        @type data: str

        @return: size of data written
        @rtype: int
        """
        raise NotImplementedError("AbstractDriver.write() must be overloaded")

    def read(self, size):
        """ Read data.

        @param size: size of the data to read
        @type size: int

        @return: read data
        @rtype: str
        """
        raise NotImplementedError("AbstractDriver.read() must be overloaded")


class SerialDriver(AbstractDriver):
    """ Driver for serial connection.
    """
    def init(self):
        try:
            self._serial = serial.Serial(port=SERIAL_PORT, baudrate=SERIAL_BAUDRATE, timeout=3)
            self.empty()
        except:
            print "SerialDriver._init()"
            raise

    def shutdown(self):
        self._serial.close()

    def setRTS(self, level):
        """ Set RTS signal to specified level.

        @param level: level to set to RTS signal
        @type level: int
        """
        self._serial.setRTS(level)

    def setDTR(self, level):
        """ Set DTR signal to specified level.

        @param level: level to set to DTR signal
        @type level: int
        """
        self._serial.setDTR(level)

    def setTimeout(self, timeout):
        self._serial.timeout = timeout

    def empty(self):
        if hasattr(self._serial, "inWaiting"):
            self.read(self._serial.inWaiting())

    def write(self, data):
        #print "SerialDriver.write(): data=%s" % repr(data)
        size = self._serial.write(data)
        return size

    def read(self, size):
        data = self._serial.read(size)
        #print "SerialDriver.read(): data=%s" % repr(data)
        if size and not data:
            raise IOError("Timeout while reading on serial bus (size=%d, data=%s)" % (size, repr(data)))
        else:
            return data


class EthernetDriver(AbstractDriver):
    """ Driver for TCP ethernet connection.
    """
    def init(self):
        print "EthernetDriver._init(): trying to connect to %s:%d..." % ("localhost", 7165)
        try:
            #import time
            #time.sleep(3)
            self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self._sock.connect(("localhost", 7165))
            self._sock.settimeout(3)
            self.empty()
        except Exception, msg:
            print "EthernetDriver.init()"
            raise Exception(msg)
        else:
            print "EthernetDriver._init(): successfully connected to %s:%d" % ("localhost", 7165)

    def shutdown(self):
        self._sock.close()

    def setTimeout(self, timeout):
        self._sock.settimeout(timeout)

    def empty(self):
        pass

    def write(self, data):
        #Logger().debug("EthernetDriver.write(): data=%s" % repr(data))
        try:
            size = self._sock.send(data)
            #if size != len(data) + 2:
                #raise IOError("Failed to send data on ethernet")
            #else:
                #return size
            return size
        except socket.timeout:
            raise IOError("Timeout while writing on ethernet")
        except socket.error, msg:
            raise IOError(msg)

    def read(self, size):
        try:
            data = self._sock.recv(size)
            #print "EthernetDriver.read(): data=%s" % repr(data)
            if not data:
                raise IOError("Connection lost")
            else:
                return data
        except socket.timeout:
            raise IOError("Timeout while reading on ethernet bus")
        except socket.error, msg:
            raise IOError(msg)


class MerlinOrionHardware(object):
    """ Merlin/Orion low-level hardware.
    """
    def __init__(self, axis, driver):
        """
        """
        self._axis = axis
        self._driver = driver

        self._encoderFullCircle = None

    def _decodeAxisValue(self, strValue):
        """ Decode value from axis.

        Values (position, speed...) returned by axis are
        32bits-encoded strings, low byte first.

        @param strValue: value returned by axis
        @type strValue: str

        @return: value
        @rtype: int
        """
        value = 0
        for i in xrange(3):
            value += eval("0x%s" % strValue[i*2:i*2+2]) * 2 ** (i * 8)

        return value

    def _encodeAxisValue(self, value):
        """ Encode value for axis.

        Values (position, speed...) to send to axis must be
        32bits-encoded strings, low byte first.

        @param value: value
        @type value: int

        @return: value to send to axis
        @rtype: str
        """
        strHexValue = "000000%s" % hex(value)[2:]
        strValue = strHexValue[-2:] + strHexValue[-4:-2] + strHexValue[-6:-4]

        return strValue.upper()

    def _encoderToAngle(self, codPos):
        """ Convert encoder value to degres.

        @param codPos: encoder position
        @type codPos: int

        @return: position, in °
        @rtype: float
        """
        return (codPos - ENCODER_ZERO) * 360. / self._encoderFullCircle

    def _angleToEncoder(self, pos):
        """ Convert degres to encoder value.

        @param pos: position, in °
        @type pos: float

        @return: encoder position
        @rtype: int
        """
        return int(pos * self._encoderFullCircle / 360. + ENCODER_ZERO)

    def _sendCmd(self, cmd, param=""):
        """ Send a command to the axis.

        @param cmd: command to send
        @type cmd: str

        @return: answer
        @rtype: str
        """
        cmd = ":%s%d%s\r" % (cmd, self._axis, param)
        #print "MerlinOrionHardware._sendCmd(): axis %d cmd=%s" % (self._axis, repr(cmd))
        for nbTry in xrange(3):
            try:
                answer = ""
                self._driver.empty()
                self._driver.write(cmd)
                c = ''
                while c not in ('=', '!'):
                    c = self._driver.read(1)
                    #print "MerlinOrionHardware._sendCmd(): c=%s" % repr(c)
                if c == '!':
                    c = self._driver.read(1) # Get error code
                    raise IOError("Error in command %s (err=%s)" % (repr(cmd), c))
                answer = ""
                while True:
                    c = self._driver.read(1)
                    #print "MerlinOrionHardware._sendCmd(): c=%s" % repr(c)
                    if c == '\r':
                        break
                    answer += c

            except IOError:
                print "MerlinOrionHardware._sendCmd(): axis %d can't sent command %s. Retrying..." % (self._axis, repr(cmd))
            else:
                break
        else:
            raise Exception("axis %d can't send command %s" % (self._axis, repr(cmd)))
        #print "MerlinOrionHardware._sendCmd(): axis %d ans=%s" % (self._axis, repr(answer))

        return answer

    def init(self):
        """ Init the MerlinOrion hardware.

        Done only once per axis.
        """
        self._driver.acquireBus()
        try:

            # Stop motor
            self._sendCmd("L")

            # Check motor?
            self._sendCmd("F")

            # Get firmeware version
            value = self._sendCmd("e")
            print "MerlinOrionHardware.init(): firmeware version=%s" % value

            # Get encoder full circle
            value = self._sendCmd("a")
            self._encoderFullCircle = self._encoderFullCircle_ = self._decodeAxisValue(value)
            print "MerlinOrionHardware.init(): encoder full circle=%s" % hex(self._encoderFullCircle)

            # Get sidereal rate
            value = self._sendCmd("D")
            print "MerlinOrionHardware.init(): sidereal rate=%s" % hex(self._decodeAxisValue(value))

        finally:
            self._driver.releaseBus()

    def overwriteEncoderFullCircle(self, encoderFullCircle):
        """ Overwrite firmware value.
        """
        print "MerlinOrionHardware.overwriteEncoderFullCircle(): encoderFullCircle=%x" % encoderFullCircle
        self._encoderFullCircle = encoderFullCircle

    def useFirmwareEncoderFullCircle(self):
        """ Use value from firmware.
        """
        self._encoderFullCircle = self._encoderFullCircle_
        print "MerlinOrionHardware.useFirmwareEncoderFullCircle(): encoderFullCircle=%x" % self._encoderFullCircle

    def read(self):
        """ Read the axis position.

        @return: axis position, in °
        @rtype: float
        """
        self._driver.acquireBus()
        try:
            value = self._sendCmd("j")
        finally:
            self._driver.releaseBus()
        pos = self._encoderToAngle(self._decodeAxisValue(value))
        return pos

    def drive(self, pos):
        """ Drive the axis.

        @param pos: position to reach, in °
        @type pos: float
        """
        strValue = self._encodeAxisValue(self._angleToEncoder(pos))
        self._driver.acquireBus()
        try:
            self._sendCmd("L")
            self._sendCmd("G", "00")
            self._sendCmd("S", strValue)
            self._sendCmd("J")
        finally:
            self._driver.releaseBus()

    def stop(self):
        """ Stop the axis.
        """
        self._driver.acquireBus()
        try:
            self._sendCmd("L")
        finally:
            self._driver.releaseBus()

    def startJog(self, dir_, speed):
        """ Start the axis.

        @param dir_: direction ('+', '-')
        @type dir_: str

        @param speed: speed
        @type speed: int
        """
        self._driver.acquireBus()
        try:
            self._sendCmd("L")
            if dir_ == '+':
                self._sendCmd("G", "30")
            elif dir_ == '-':
                self._sendCmd("G", "31")
            else:
                raise ValueError("Axis %d dir. must be in ('+', '-')" % self._axis)
            self._sendCmd("I", self._encodeAxisValue(speed))
            self._sendCmd("J")
        finally:
            self._driver.releaseBus()

    def getStatus(self):
        """ Get axis status.

        @return: axis status
        @rtype: str
        """
        self._driver.acquireBus()
        try:
            return self._sendCmd("f")
        finally:
            self._driver.releaseBus()

    def setOutput(self, state):
        """ Set output state.

        @param state: new state of the the output
        @type state: bool
        """
        self._driver.acquireBus()
        try:
            self._sendCmd("O", str(int(state)))
        finally:
            self._driver.releaseBus()


class IncomingPacket(object):
    """ Incoming packet from Stellarium.
    """
    def __init__(self, message):
        """ Create the packet
        """
        self.message = message

        length, type_, time_, ra, dec =  struct.unpack("<HHQIi", message)
        self.length = length
        self.type_ = type_
        self.time_ = time_
        self.ra = ra * 90. / 0x40000000
        self.dec = dec * 90. / 0x40000000


class MyTCPHandler(SocketServer.BaseRequestHandler):
    """ The RequestHandler class for our server.
    """
    def handle(self):
        while True:

            # self.request is the TCP socket connected to the client
            self.packet = IncomingPacket(self.request.recv(PACKET_SIZE).strip())
            print "MyTCPHandler.handle(): {} wrote:".format(self.client_address[0])
            print "MyTCPHandler.handle(): %s" % repr(self.packet.message)
            print "MyTCPHandler.handle(): length=%d, time=%d, RA=%.2f, DEC=%.2f" % (self.packet.length, self.packet.time_, self.packet.ra, self.packet.dec)
            self.server.yaw.drive(self.packet.dec)
            self.server.pitch.drive(self.packet.ra)
            while self.server.yaw.getStatus()[1] != '0' and self.server.pitch.getStatus()[1] != '0':
                time.sleep(0.1)
            print "MyTCPHandler.handle(): done"

            # just send back the reached position
            self.request.sendall(struct.pack("<HHQIii", self.packet.length+4, self.packet.type_, time.time(), self.packet.ra, self.packet.dec, 0))


class MyServer(SocketServer.TCPServer):
    allow_reuse_address = True



def main():

    #driver = EthernetDriver()
    driver = SerialDriver()
    driver.init()

    # Create the server, binding to localhost on port 9999
    server = MyServer((HOST, PORT), MyTCPHandler)

    server.yaw = MerlinOrionHardware(1, driver)
    server.yaw.init()
    server.pitch = MerlinOrionHardware(2, driver)
    server.pitch.init()

    # Activate the server; this will keep running until you
    # interrupt the program with Ctrl-C
    server.serve_forever()


if __name__ == "__main__":
    main()

# Stellarium server params
HOST, PORT = "localhost", 9999  <<<<<<<<< do not change anything here...
PACKET_SIZE = 20

# Merlin/Orion params
ENCODER_ZERO = 0x800000

# Driver params
SERIAL_PORT = "/dev/ttyS0"  <<<<<<<<<<<<<< change your Merlin serial port here
SERIAL_BAUDRATE = 9600