print_variable_states.py

import calculate_nearpoint_farpoint
import calculate_visual_angles
import math
import numpy as np
import matplotlib as plt

# Check if agent_position, alpha, delta_alpha, and phi are initialized before first frame 
def print_initial_parameters(Simulation):
    print("Agent's initial position:","X:",Simulation.agent_x_position_array,"Y:",Simulation.agent_y_position_array,"\n",
      "nearPoint's initial position:","X:",Simulation.nearPoint_x_array,"Y:",Simulation.nearPoint_y_array,"\n",
      "farPoint's initial position:","X:",Simulation.farPoint_x_array,"Y:",Simulation.farPoint_y_array,"\n",
      "Agent's alpha: ",Simulation.alpha_array, "\n","Agent's delta_alpha: ",Simulation.delta_alpha_array, "\n",
      "Agent's phi: ",Simulation.phi_array)
    

def visualize_vector_angle(agent_x,agent_y,prev_agent_x,prev_agent_y,npx,npy,fpx,fpy):
    delta_x = agent_x - prev_agent_x    
    delta_y = agent_y - prev_agent_y

    vertical_longline_x, verticle_longline_y = [agent_x, agent_x + delta_x], [agent_y,agent_y + delta_y]
    horizontal_longline_npx, horizontal_longline_npy = [agent_x,npx], [agent_y,npy]
    plt.plot(vertical_longline_x, verticle_longline_y, horizontal_longline_npx, horizontal_longline_npy, c="black", zorder = 4)

    horizontal_longline_fpx, horizontal_longline_fpy = [agent_x,fpx], [agent_y,fpy]
    plt.plot(horizontal_longline_fpx, horizontal_longline_fpy, c="red", zorder = 3)
    plt.plot(agent_x,agent_y, marker="o", markersize=10, markeredgecolor="black", markerfacecolor="green")
    plt.plot(npx,npy, marker="o", markersize=5, markeredgecolor="black", markerfacecolor="steelblue")
    plt.plot(fpx,fpy, marker="o", markersize=5, markeredgecolor="black", markerfacecolor="red")

    npangle = calculate_visual_angles.calculate_visual_angle(prev_agent_x,prev_agent_y,agent_x,agent_y,npx,npy)
    print("VISUAL ANGLE OF NEAR POINT IN DEGREES",math.degrees(npangle))
    fpangle = calculate_visual_angles.calculate_visual_angle(prev_agent_x,prev_agent_y,agent_x,agent_y,fpx,fpy)
    print("VISUAL ANGLE OF NEAR POINT IN DEGREES",math.degrees(fpangle))

    plt.show()

def get_visual_angles(total_time,center_X,center_Y,NP_Angle_array,FP_Angle_array):
    for i in range(0,total_time):
        x = center_X[i]
        y = center_Y[i]
        npx,npy = calculate_nearpoint_farpoint.get_point(x,y,center_X,center_Y,20)
        fpx,fpy = calculate_nearpoint_farpoint.get_point(x,y,center_X,center_Y,50)

        NP_angle = calculate_visual_angles.calculate_visual_angle(x,y,npx,npy)
        FP_angle = calculate_visual_angles.calculate_visual_angle(x,y,fpx,fpy)
        
        NP_angle = math.degrees(NP_angle)
        FP_angle = math.degrees(FP_angle)

        NP_Angle_array.append(NP_angle)
        FP_Angle_array.append(FP_angle)  

        # alpha = two_point_steering_model.control_law(i,NP_angle,FP_angle,NP_Angle_array,FP_Angle_array,1,1,1)
        # control_model_x,control_model_y = two_point_steering_model.calculate_x_y_phi(x,y,i,alpha,phi_array,x_array,y_array,timeStep,1,1)

    return(NP_Angle_array,FP_Angle_array)


# PRINT EQUATIONS TO CHECK VARIABLE VALUES ==============================================================

def print_two_point_steering_equation(i,Agent,NP_angle_array,FP_angle_array,delta_alpha,timeStep):
    current_near_point_angle = NP_angle_array[i-1]
    current_far_point_angle = FP_angle_array[i-1]
    previous_near_point_angle = NP_angle_array[i-2]
    previous_far_point_angle = FP_angle_array[i-2]

    far_point_derivative = current_far_point_angle - previous_far_point_angle
    near_point_derivative = current_near_point_angle - previous_near_point_angle

    print("TIMESTEP:",i, "----------------------------------------------------------------")
    print("NP Angle in Degrees:", math.degrees(current_near_point_angle), " FP Angle in Degrees:", math.degrees(current_far_point_angle))
    print("SALVUCCI & GRAY: delta_alpha = gain*Near point angle*timeStep + gain*Near point angle derivative + gain*Far point angle derivative")
    print(delta_alpha,"=","(",Agent.proportional_near_poing_gain, "*", current_near_point_angle,"*",timeStep,")","+","(", Agent.derivative_near_point_gain,"*",near_point_derivative,")","+", "(",Agent.derivative_far_point_gain,"*",far_point_derivative,")")
    print("\n")

def print_x_y_alpha_phi_equations(i,Agent,delta_alpha,alpha,alpha_array,phi,phi_array,x,y,timeStep):
    print("CALCULATE ALPHA: alpha = previous alpha (",i-2,"th index from alpha array) + delta-alpha*timeStep")
    print(alpha,"=",alpha_array[i-2],"+", delta_alpha,"*",timeStep)

    print("GET CURRENT PHI: phi = previous phi (",i-2,"th index from alpha array)")
    print(phi,"=", phi_array[i-2])

    print("CALCULATE X AND Y")
    print("x = x + sin(phi)*timeStep")
    print(x,"=", x, "+", "(",np.sin(phi),"*",timeStep,")")

    print("y = y + np.cos(phi)*timeStep")
    print(y, "=", y, "+", np.cos(phi),"*",timeStep)

    print("CALCULATE PHI FOR NEXT TIME STEP")
    print("phi = phi + velocity*gain*timeStep*alpha")
    print(phi,"=", phi, "+", "(", Agent.velocity,"*",Agent.phi_gain,"*",timeStep,"*",alpha,")")
    print("-------------------------------------------------------------------------------------")
    print("\n")