#importing the needed librarys
import numpy as np
import time
import serial
import math

#setting the variables
steps = 200*16 #number of steps the steppermotor makes in 1 revolution
A = 3 #the amplitude of the projection in degrees
t_end = 2*np.pi #the period of the functions
t_cycle = 0.05 #the real time for one period in s
N = 100 #the number of times it will plot the functions
port = '/dev/ttyACM0' #the port that is connected to the arduino

w = t_end/t_cycle #calculating the angular frequentie
res = 360/steps #calculating the angular resolution of the steppers in degrees

#standard codes:
x_n = b'0'
x_p = b'1'
y_n = b'2'
y_p = b'3'

#defining the x and y functions
def x_func(t):
    x = 0.5*A*math.cos(w*t) #the x function corrected for real time and the set amplitude
    level_x = np.floor(x / res) #creating a steped version of the function based on the resolution
    return level_x

def y_func(t):
    y = 0.5*A*math.sin(w*t) #the y function corrected for real time and the set amplitude
    level_y = np.floor(y / res) #creating a steped version of the function based on the resolution
    return level_y

arduino = serial.Serial(port=port, baudrate=115200, timeout=.1) #astablishing a serial connection with the arduino
time.sleep(2) #giving the connection time to stat up

#defining the functions that send the signal to the arduino
#0 - move x in the negative direction; 1 - move x in the positive direction; 2 - move y in the negative direction; 3 - move y in the positive direction
#dir will be a 0 for negative direction and 1 for positive direction
def send(code):
    arduino.write(code) #writing the signal to the arduino for a x movement with a certain direction

#setting a start for the recurcive x and y veriables
x_old = x_func(0)
y_old = y_func(0)

start = time.time() #getting the start time
t_pas = time.time() - start #calculating the elepsed time to set the start for the time recurcive variable
i=0
k=0

while t_pas <= N*t_cycle: #passing trough the loop as long as t is below the set cylce time times the number of cycles
    #calculating the current x and y values based on the passed time since start
    x_cur = x_func(t_pas)
    y_cur = y_func(t_pas)
    #calculating the difference between the current x and y, and the x and y before
    dx = x_cur - x_old
    dy = y_cur - y_old

    #checking if x and/or y have changed to the next step, and if that thay have changed in the positive or negative directions
    #after the chek it will send the correcponding code
    if dx >= 1:
        for j in range(int(dx)):
            arduino.write(x_p)
    elif dx <= -1:
        for j in range(int(-dx)):
            arduino.write(x_n)
    if dy >= 1:
        for j in range(int(dy)):
            arduino.write(y_p)
    elif dy <= -1:
        for j in range(int(-dy)):
            arduino.write(y_n)
    
    #updating the x and y current values to the old values
    x_old = x_cur
    y_old = y_cur

    t_pas = time.time() - start #updating the passed time since start

print(f"done")