#!/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 read_position():
    global tn
    position = 0
#    res = write_telnet('1 166 5')
    tn.write('1 166 5\r\n')
    time.sleep(0.3)
    res = tn.read_very_eager()
    result = [eval(x) for x in res.split()]
    if len(result) == 9 and result[0] == 1 and result[1] == 166 and result[2] == 5 and result[3] == 2:
        position = result[4]
    return position

def read_mc_time():
    global tn
    mc_time = 0
#    res = write_telnet('1 173 5')
    tn.write('1 173 5\r\n')
    time.sleep(0.3)
    res = tn.read_very_eager()
    result = [eval(x) for x in res.split()]
    if len(result) == 9 and result[0] == 1 and result[1] == 173 and result[2] == 5 and result[3] == 2:
        mc_time = result[4]
    return mc_time

def read_data_command(command, printCom):
    global tn
    global profile_target
    global last_position
    mc_time = 0
#    res = write_telnet('1 173 5')
    telnet_command = '1 %d 5\r\n' % command
    tn.write(telnet_command)
    time.sleep(0.3)
    res = tn.read_very_eager()
    answer = res.split('\r\n')
    proper_result = ' '
    result = proper_result.join([x for x in answer if x and eval(x.split()[1])==command]).split()
    result2 = [x for x in answer if x and eval(x.split()[1]) != command]
    if printCom:
        print(telnet_command, res)
    if len(result2) > 0:
        for el in result2:
            split_el = el.split()
            if len(split_el) > 3:
                if eval(split_el[3]) == 3:
                    print("Event %s for command %s" % (split_el[4], split_el[1]))
                    if eval(split_el[1]) == 222:
                        print("Profile Target was : " , profile_target, "; current position : " , last_position, "; Difference is " , profile_target - last_position)
                elif eval(split_el[3]) == 4:
                    print("Info %s for command %s" % (split_el[4], split_el[1]))
                else:
                    print(el)
            else:
                print(el)
    if len(result) == 9 and eval(result[0]) == 1 and eval(result[1]) == command and eval(result[2]) == 5 and eval(result[3]) == 2:
        mc_time = eval(result[4])
    return mc_time

def run_procedure():
    global tn
    global profile_target
    global last_position
    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':
        ###### Read Current Position ####
        position = read_data_command(166, True)
#        position = read_position()
        if position != 0:
            ###### Allocate Bufer ######
            write_telnet('1 220 5 6000 1')
            duration = 40698
            velocity = -1269821 
            acceleration = -7
            jerk = 2
            ###### Write Profile  ######
            write_telnet('1 221 5 0 %d %d %d %d %d' % (duration, velocity, position, acceleration, jerk))
            for i in range(duration - 1):
                position += (velocity/65536. + acceleration/(2*65536) + jerk/(6*65536.*65536))
                velocity += (acceleration + jerk/(2*65536.))
                acceleration += (jerk/65536.)
                if i == duration - 2:
                    print("delta Pos : " , (velocity/65536. + acceleration/(2*65536) + jerk/(6*65536.*65536)))
            profile_target = position
            write_telnet('1 221 5 1 0 0 %d 0 0' % int(position))
            ###### SetExternalProfileStart  ######
            write_telnet('1 222 5 0')
            start_time = time.time()
            start_mc_time = read_mc_time()
            mc_time = start_mc_time
            if start_mc_time != 0:
                while mc_time - start_mc_time < (duration + 2000):
                #    position = read_position()
                    position = read_data_command(166, True)
                    last_position = position
                #    mc_time = read_mc_time()
                    mc_time = read_data_command(173, False)
                    if position != 0 and mc_time != 0:
                        print(time.time() - start_time, position, mc_time)
            ###### SetExternalProfileStop  #####
            write_telnet('1 224 5 0')
            ###### SetExternalProfileClearBuffer  ######
            write_telnet('1 226 5 0')
        else :
            print("Error reading current position")
            print(res)
            print(result)
            print(len(result))

        print("\nExternal profile test successfully completed")
    else:
        print("Error module 5 cannot be reserved")
    ###### release modules 5 ######
    write_telnet('1 24 5')
    ###### close connection ######
    write_telnet('1 29 0')
    return



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()
