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

###############################################################################
#               additional variables for the initialization                   #
###############################################################################

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 = 5000                                                  # number of scans

###############################################################################
#                         Initialization                                      #
###############################################################################

print("Please write your desired  Motor 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() #KollmorgenInver3 is the Library for the Inverter

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



# initialize the LabJack device
daq = AqCard.DAQLABJACK2()
####################################################################################################
inv.opMode(1)                       #Inverter configuration 
inv.drv_cmdsource(0)                 #Inverter configuration 
inv.unit_vrotary(2) # Deg/s          #Inverter configuration 
time.sleep(0.1)                      #Inverter configuration 
#Analogausgang anpassen               #Inverter configuration 
inv.Aout_mode(1)                         #Inverter configuration 
time.sleep(0.1)                          #Inverter configuration 
inv.Aout_vscale(100) # Annehmen : Max v 1000 deg/s = 10V  #Inverter configuration 
time.sleep(0.1)                          #Inverter configuration 
inv.mtSave()                                 #Inverter configuration 
time.sleep(0.1)                              #Inverter configuration 
#Start the Motor , The motor running there with ge speed of v_desired
inv.vl_cmdu(v_desired)
# STREAM WITH LABJACK / ain is the analog Input 
if key == 'read': 
    timer, positionFB, ain = daq.acquireData('read', scanRate, scans)
else:
    timer, positionFB, ain = daq.acquireData('stream',scanRate,scans)
answer = input("you wand to change the speed or shout the motor stop ? Y/N ")
while answer=="Y" :
    v = float(input("please give you new speed: "))
    inv.vl_cmdu(v) #config the motor with the new speed 
    time.sleep(0.1)
    print("Speed changed")
    time.sleep(0.1)
    if key == 'read':  # I want continue stream with the new speed but that dont work ?
        timer, positionFB, ain = daq.acquireData('read', scanRate, scans)
    else:
        timer, positionFB, ain = daq.acquireData('stream',scanRate,scans)
    antwort = input("you wand to change the speed or shout the motor stop ? Y/N")
time.sleep(0.1)
inv.stop() # after the answer "N" i stop the motor 
print("Motor stopped")
time.sleep(5)
print('DONE')
inv.dis() # Disconect with the Inverter 
###############################################################################
#                         save the data                                       #
###############################################################################

# get the Drehzahl, Drehmoment
    
speed_streamed  = [first[0] for first in ain]

Torque_streamed = [first[1] for first in ain]
    
# dump the data with json
with open('./'+folder+'/'+fileName+'.dat', 'w') as datfile:
    json.dump((timer, positionFB, speed_streamed , Torque_streamed ), datfile)


b = open('./'+folder+'/'+fileName+'.dat', 'r')
data =json.load(b)
b.close() 
v = data[1]
print(v)

# close the connection
daq.close()

###############################################################################
#                                  End                                        #
###############################################################################