#!/usr/bin/env python

import time
from telnetlib import Telnet
import sys


def write_telnet(msg, tosleep=0.3):
    global tn
    print(msg)
    tn.write(msg + "\r\n")
    time.sleep(tosleep)
    return read_telnet()


def read_telnet():
    global tn
    res = tn.read_very_eager()
    print(res)
    return res

def run_procedure():
    global tn
    if len(sys.argv) == 3:
        arg1 = eval(sys.argv[1])
        arg2 = eval(sys.argv[2])
        if arg1 < -100 or arg1 > 100 or arg2 < 0 or arg2 > 100:
            if arg1 < -100:
                print('First argument is BIAS and must be -100 <= arg1 <= 100 ~ Current value %d' % arg1)
            if arg2 < 0 or arg2 > 100:
                print('Second argument is Duration of current and must be 0 <= arg2 <= 100 ~ Current value %d' % arg2)
            return -4
    else:
        print('Please specify BIAS and duration es. \"./shutter_openclose_lars_patch.py -10 20\" to power the motor at 10% of voltage RMS in the negative direction for 20 s')
        return -5
    read_telnet()
    write_telnet('1 21 0 user')
    write_telnet('1 22 0 mpia')
    res = write_telnet('1 23 8')
    if res == '1 23 8 -15\r\n':
        print( "Error module 8 cannot be reserved. Unlock it then retry")
        write_telnet('1 29 0')
        return -1
    elif res == '1 23 8 -18\r\n':
        print("Error moudule 8 already reserved")
        write_telnet('1 29 0')
        return -2
    elif res == '1 23 8 1\r\n':
        # print('1 113 8 8 %d' % arg1)
        # print("Arg2 = %d" % arg2)
        # write_telnet('1 24 8')
        # write_telnet('1 29 0')
        # return 0
        ###### Setting PID to 0 ######
        write_telnet('1 113 8 1 0')
        write_telnet('1 113 8 2 0')
        write_telnet('1 113 8 3 0')
        write_telnet('1 120 8', 5)
        ###### Move of 1 step ######
        write_telnet('1 123 8 1', 2)
        ###### Move arg1% ######
        write_telnet('1 113 8 8 %d' % arg1)
        write_telnet('1 120 8', 5)
        ###### Sleep arg2 seconds ######
        time.sleep(arg2)
        ###### Stop Current ######
        write_telnet('1 113 8 8 0')
        write_telnet('1 120 8', 5)
        ###### close connection ######
        write_telnet('1 24 8')
        write_telnet('1 29 0')

        print("\nShutter patch procedure successfully completed")
        return 0


def main():
    global tn
    mocon_opened = False
    tn = Telnet('192.168.61.52', 4000)
    try:
        tn.open('192.168.61.52', 4000)
        print("MoCon connected")
        time.sleep(1)
        mocon_opened = True
    except exception as e:
        print(e.what())
        raise e
    except StandardError as e:
        print(e.what())
        raise e
    if mocon_opened:
        run_procedure()


if __name__ == "__main__":
    main()
