import numpy as np
import scipy as sp
import matplotlib.pyplot as plt


def main():
    head_err_0 = np.radians(30)
    path_angle_0 = np.radians(20)
    line_sight_0 = head_err_0 - path_angle_0
    range_0 = 20E3
    z_f = 0
    x_f = 0

    K = np.array([3,4,5])

    tau = np.linspace(0,1,50)

    for k in K:
        head_err = head_err_0 * tau**(k-1)
        line_sight = line_sight_0 + head_err_0*(1 - tau**(k-1))/(k-1)
        path_angle = head_err - line_sight

        z = -line_sight_0 + tau*line_sight
        x = 1 - tau
        
        plt.subplot(321)
        plt.plot(tau, np.degrees(head_err), label=f"K = {k}")
        plt.title("Heading Error")

        plt.subplot(323)
        plt.plot(tau, np.degrees(path_angle), label=f"K = {k}")
        plt.title("Path Angle")

        plt.subplot(325)
        plt.plot(tau, np.degrees(line_sight), label=f"K = {k}")
        plt.title("Line of Sight")

        plt.subplot(322)
        plt.plot(tau, z, label=f"K = {k}")
        plt.title("Altitude Difference")

        plt.subplot(324)
        plt.plot(tau, x, label=f"K = {k}")
        plt.title("Horizontal Distance")

        plt.subplot(326)
        plt.plot(tau, (x**2 + z**2)**0.5, label=f"K = {k}")
        plt.title("Trajectory")

    plt.show()


if __name__ == "__main__":
    main()