import KollmorgenInverter3 as drive
import DAQLABJACK2 as AqCard
import datetime
import time
import numpy as np
import matplotlib.pyplot as plt
import json
import os
import multiprocessing as mp



def AKD(v_desired) : # this Thread is to config and control the Motor 
    inv.opMode(1)               #config
    inv.drv_cmdsource(0)        #config
    inv.unit_vrotary(2) # Deg/s #config Unit 
    time.sleep(0.1)
    #Analog output of Inverter config 
    inv.Aout_mode(1)      #config
    time.sleep(0.1)
    inv.Aout_vscale(100) #  Max v 1000 deg/s = 10V 
    time.sleep(0.1)
    inv.mtSave()            #config
    time.sleep(0.1)
    #Start
    inv.vl_cmdu(v_desired)     # Desired Speed  
    answer = input("Do you want to change the speed or stop ? Y/N ")
    while answer=="Y" :
        v = float(input("New speed : "))
        inv.vl_cmdu(v) # new speed 
        time.sleep(0.1)
        print("speed changed ")
        time.sleep(0.1)
        answer = input("Do you want to change the speed  or stop ? Y/N ")
    time.sleep(0.1)
    inv.stop() # Stop motor
    print("Motor stop")
    time.sleep(5)
    print('DONE')
    inv.dis()

def Stream(key,scanRate,scans) :  # I use the basic stream like the example / i wraped it to daq.acquireData 
    #set the mode for the data aquisition
    if key == 'read':
        timer, positionFB, ain = daq.acquireData('read', scanRate, scans)
    else:
        timer, positionFB, ain = daq.acquireData('stream',scanRate,scans)

    # get the actual Speed , Torque   
    Actual_Speed = [first[0] for first in ain]
    Torque = [first[1] for first in ain]
    
    # dump the data with json
    with open('./'+folder+'/'+fileName+'.dat', 'w') as datfile:
        json.dump((timer, positionFB, Actual_Speed , Torque), datfile)

    b = open('./'+folder+'/'+fileName+'.dat', 'r')
    data =json.load(b)
    b.close() 
    v = data[2] # Only  show speed 
    print(v)

    # close the connection
    daq.close()

if __name__=="__main__" : 

    folder = 'Data_' + datetime.datetime.now().strftime("%Y%m%d")  # where to store
    debug = True               # 'True' -> additional information printed on screen
    key = None        # set 'read' aquisition is in read mode; default: stream mode
    namedMeasurement = False           # set to True the script will ask for a name
    scanRate = 1000                                        # stream frequency in Hz
    scans = 1000                                                  # number of scans


    print("Please give the desired Speed  ")
    v_desired = float(input("v_desired = "))

    try:
        os.mkdir(folder)
    except OSError as e:
        print('OSError:', e, 'file exists')

    if namedMeasurement is True:

        name = input('measurementName:')
        fileName = name + datetime.datetime.now().strftime("%Y%m%d-%H%M%S")

    else:
        name = 'LJvalues'
        fileName = name + datetime.datetime.now().strftime("%Y%m%d-%H%M%S")

    # initialize the inverter and establish the telnet connection
    inv = drive.KollmorgenInverter3()

    # initialize the LabJack device
    daq = AqCard.DAQLABJACK2()

    # enable the drive
    inv.en()
    print('Kollmorgen enabled')    
    print("Motor start in 30s ")
    time.sleep(30)
    inv.mtMove(24)
    time.sleep(5)

    p1 = mp.Process(target=AKD , args = (v_soll)
    p2 = mp.Process(target=Stream , args = (key,scanRate,scans,))


    p1.start()
    p2.start()

    p1.join()
    p2.join()

    print("done")            
    
#I have run it , but i cant access the Loop in the Task 1
# Here , from here there are no respond , Labjack take data Only for the 1st.desired Speed
   #answer = input("Do you want to change the speed or stop ? Y/N ")
    #while answer=="Y" :
    #    v = float(input("New speed : "))
    #    inv.vl_cmdu(v) # new speed 
    #    time.sleep(0.1)
    #    print("speed changed ")
    #    time.sleep(0.1)
    #    answer = input("Do you want to change the speed  or stop ? Y/N ")
    #time.sleep(0.1)