#!/usr/bin/env python
# ==================================================================
# DESI
#
# Emulator for the Pollux Micos RS232 linear stage controller
#
# Author: Laurent Le Guillou 
#
#--------------------------
# For tests without a raspberry:
#   socat -d -d PTY,raw,echo=0,b19200 PTY,raw,echo=0,b19200
#
#--------------------------
# Raspberry config
#
# in /etc/inittab
# #Spawn a getty on Raspberry Pi serial line
# #T0:23:respawn:/sbin/getty -L ttyAMA0 115200 vt100
# T0:23:respawn:/usr/local/sbin/pollux-server ttyAMA0 19200

# in /boot/cmdline.txt
# # dwc_otg.lpm_enable=0 console=ttyAMA0,115200 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline rootwait
# dwc_otg.lpm_enable=0 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline rootwait
#
# ==================================================================

import sys
import os
import threading
import time
import datetime
import serial
import logging

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

device = "/dev/ttyAMA0" # raspberry serial UART
baudrate = 19200
parity = serial.PARITY_NONE
bytesize = serial.EIGHTBITS
stopbits = serial.STOPBITS_ONE

if len(sys.argv) >= 2:
    device = sys.argv[1]

if len(sys.argv) >= 2:
    device = sys.argv[1]


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

now = datetime.datetime.utcnow()
logdir = os.path.join("var", "log")
logname = os.path.join(logdir,
                       ( "pollux-server-%s.log" % 
                         now.isoformat().split('T')[0] ))
# print logdir
# print logname
# # recreate the symlink sbig-server.log 
# logsymlink = os.path.join(logdir, "pollux-server.log")
# print logsymlink
# if os.path.islink(logsymlink):
#     try:
#         os.unlink(logsymlink)
#     except OSError:
#         pass
# os.symlink(logname, logsymlink)

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

class PolluxMotorEmulator(object):

    def __init__(self):
        self.lower = 0.0
        self.upper = 151.75
        self.start_position = 0.0
        self.end_position = 0.0
        self.start_move_time = -1.0
        self.end_move_time = 0.0
        self.speed = 10.0 # 10 mm/s  -> to fix
        
    def getpos(self):
        t = time.time() 
        if t > self.end_move_time:
            # not moving anymore
            return self.end_position
        #
        # A move is going on, lets compute the current position

        ### ICI INCOHERENT !!!!
        if self.end_position > self.start_position:
            speed = self.speed
        else:
            speed = -self.speed

        position = ( self.end_position -
                     (self.end_move_time - t) * speed )

        return position

    def move(self, position):
        t = time.time() 
        self.start_move_time = t
        self.start_position = self.end_position
        self.end_position = position
        self.end_move_time = (
            self.start_move_time +
            abs(self.end_position - self.start_position) / self.speed )
        
    def is_moving(self):
        t = time.time() 
        if t < self.end_move_time:
            return True
        return False

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

class PolluxEmulator(object):

    def __init__(self):
        self.serialno = "31415926"
        self.axis = 1
        self.EOL = "\n"
        self.motor = PolluxMotorEmulator()
        
    def execute(self, request):

        # --- first parse the command (only a small subset is emulated)

        noanswer = self.EOL
        
        cmd = request.strip()
        parts = cmd.split()

        # --- How many elements were provided?
         
        if len(parts) == 3:
            try:
                value = float(parts[0])
                axis = int(parts[1])
                keyword = parts[2]
            except ValueError:
                return noanswer

        elif len(parts) == 2:
            try:
                axis = int(parts[0])
                keyword = parts[1]
            except ValueError:
                return noanswer
            
        else:
            return noanswer


        # --- Are we the right axis?
        
        if axis != self.axis:
            # This request is not for us
            return noanswer

        # --- Now specific action depending of the keyword
        
        # ----------------------------
        # nm/nmove
        # nr/nrmove
            
        if keyword in ["nm", "nmove"]:
            # absolute move
            self.motor.move(value)
            return noanswer

        elif keyword in ["nr", "nrmove"]:
            # relative move
            self.motor.move(self.motor.getpos() + value)
            return noanswer

        # ----------------------------
        # getserialno

        elif keyword == "getserialno":
            return self.serialno + self.EOL

        # ----------------------------
        # np/npos

        elif keyword in ["np", "npos"]:
            return ("%f" % self.motor.getpos()) + self.EOL

        # ----------------------------
        # nst/nstatus

        elif keyword in ["nst", "nstatus"]:
            moving = 0
            if self.motor.is_moving(): moving = 1
            return ("%d" % moving) + self.EOL

        # ----------------------------
        # ncal/ncalibrate

        elif keyword in ["ncal", "ncalibrate"]:
            # relative move
            self.motor.move(self.motor.lower)
            return noanswer
        
        # ----------------------------
        # nrm/nrangemeasure

        elif keyword in ["nrm", "nrangemeasure"]:
            # relative move
            self.motor.move(self.motor.upper)
            return noanswer

        else:
            return noanswer

        return noanswer
        

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

s = serial.Serial(device,
                  baudrate = baudrate,
                  parity = parity,
                  bytesize = bytesize,
                  stopbits = stopbits)

emulator = PolluxEmulator()

alive = True
while alive:
    data = s.readline()
    answer = emulator.execute(data)
    s.write(answer)
        
# ==================================================================