#!/usr/bin/env python

import time
from telnetlib import Telnet


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
    read_telnet()
    write_telnet('1 21 0 user')
    write_telnet('1 22 0 mpia')
    res = write_telnet('1 23 5')
    if res == '1 23 5 -15\r\n':
        print( "Error module 5 cannot be reserved. Unlock it then retry")
        write_telnet('1 29 0')
        return
    elif res == '1 23 5 -18\r\n':
        print("Error moudule already reserved")
        write_telnet('1 29 0')
        return
    elif res == '1 23 5 1\r\n':
        res = write_telnet('1 23 6')
        if res != '1 23 6 1\r\n':
            print("Error module 5 cannot be reserved")
            tn.write('1 29 0\r\n')
            return
        ###### Activate Lars' modification ######
        write_telnet('1 112 6 3 1')
        write_telnet('1 120 6', 5)
        ###### Move of 65536 steps twice ######
        write_telnet('1 123 5 65536', 2)
        write_telnet('1 123 5 65536', 2)
        ###### Deactivate Lars' modification ######
        write_telnet('1 112 6 3 0')
        write_telnet('1 120 6', 5)
        ###### release modules 5 and 6 ######
        write_telnet('1 24 5')
        write_telnet('1 24 6')
        ###### close connection ######
        write_telnet('1 29 0')

        print("\nAntibacklash procedure successfully completed")


def main():
    global tn
    mocon_opened = False
    tn = Telnet('192.168.61.50', 4000)
    try:
        tn.open('192.168.61.50', 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()
