#! python3
import sys
import math

####THIS SCRIPT HAS 4 OUTPUT VARIABLES: NUMBER OF STEPS, TIME DELAY (IN MS), DIFFERENTIAL X AND Y MOVEMENTS (IN UM)
####IT writes these steps into the output file (.txt) ehich can then be loaded and run via the sutter multi-link command-line interface
####The script also generates a log file which stores the input and output information 

print("multilink(theta_deg,totdist_um,time_sec,'output_filename.txt'")

def multilink(theta_deg,totdist_um,time_sec,filename):
    triggerdelay=False
    wrongdist = False
    diffx=0
    diffy=0
    outfile = open(filename, "w")
    logfile = open("%s.log"%filename[0:-4], "w")
    
    time_ms = time_sec * 1000
    theta_rad = math.radians(theta_deg)

    print(theta_deg,theta_rad)

    totx_um = -1*totdist_um*math.cos(theta_rad)
    totx_um = round(totx_um,3)
    toty_um = totdist_um*math.sin(theta_rad)
    toty_um = round(toty_um,3)

    print(totx_um, toty_um)

    # determine the number of steps and step size
    # time to step with ROE (stepper-motor + read/write) is about 0.8s (800 ms)
    steptime_ms = 800 ##assume there is human error with this measurement (+/-0.5sec in the total time)
    numsteps = time_ms/steptime_ms 

    if 0 < abs(totx_um) < abs(toty_um) or toty_um==0: 
        diffx = totx_um/numsteps
        diffy = toty_um/numsteps

    elif abs(totx_um) >= abs(toty_um) > 0 or totx_um==0:
        diffy = toty_um/numsteps
        diffx = totx_um/numsteps
    if 0 < abs(diffx) < 0.0625: # 0.0625 microns is the smallest step-size
        # this is a boolean that means your step size needed to be reassigned to the min
        triggerdelay = True
        # this is a problem since the min step the piezo can make is 0.0625 microns
        diffx = 0.0625*(diffx/abs(diffx))
        print("You have an X step lower than the minimum for the piezo")
      
    if 0 < abs(diffy) < 0.0625:
        triggerdelay = True
        # this is a problem since the min step the piezo can make is 0.0625 microns
        diffy = 0.0625*(diffy/abs(diffy))
        print("You have a Y step lower than the minimum for the piezo")

    if triggerdelay:
        new_numstep = totdist_um / 0.0625
        
        time_nodelay_ms = new_numstep * steptime_ms
        delay_needed_ms = time_ms - time_nodelay_ms
        delay_perstep_ms = delay_needed_ms/new_numstep
        if delay_perstep_ms >= 260:
            delay_perstep_ms = delay_perstep_ms - 260
        else:
            print("Delay per step is less than minimum allowed (set by time to run a pause command)!!!")
            triggerdelay = False
            wrongdist =True
            real_dist_um = numsteps*(math.sqrt(diffx**2+diffy**2))
            print("actual distance is going to be %s um"%real_dist_um)
        new_numstep=round(new_numstep)

        
    numsteps = round(numsteps)
    
    if not triggerdelay:
        for i in range(0,numsteps):
            # for the ROE x and y seem to be switched so we swap them in the txt output
            outfile.write("CUR_DIFF_MOVE=%s %s 0\n"%(diffx,diffy))
    else:
        for i in range(0,new_numstep):
            # for the ROE x and y seem to be switched so we swap them in the txt output
            outfile.write("CUR_DIFF_MOVE=%s %s 0\n"%(diffx,diffy))
            outfile.write("Pause=%s\n"%delay_perstep_ms)
        
    outfile.close()

    vel = totdist_um/time_sec

    logfile.write("Input Angle=%s degrees \n"%theta_deg)
    logfile.write("Input Distance=%s microns \n"%totdist_um)
    logfile.write("Input Total time=%s sec \n"%time_sec)
    
    logfile.write("Input Velocity=%s microns/sec\n\n"%vel)
    logfile.write("Step Size in (x,y) microns=(%s,%s)\n"%(diffx,diffy))
    
    if triggerdelay:
        logfile.write("Delay per Step=%s ms \n"%delay_perstep_ms)
        logfile.write("Number of Steps=%s n"%new_numstep)
    else:
        logfile.write("Number of Steps=%s \n"%numsteps)
        
    if wrongdist:
        logfile.write("Actual Distance=%s \n"%real_dist_um)
        logfile.write("Actual Velocity=%s \n"%(real_dist_um/time_sec))
    else:
        logfile.write("Actual Distance=%s \n"%totdist_um)