import numpy as np
import matplotlib.pyplot as plt

# Arxikes synthikes:

# Initial Position (tyxaia!)
x = 5.0
y = -3.0
theta = np.deg2rad(60)  # σε rad

# Goal Position
x_goal = 0.0
y_goal = 0.0

# Gains
k_rho = 2
k_alpha = 3.5
k_beta = -0.5


# Simulation stuff
dt = 0.05 # arketa mikro profanws gia na proswmoiazei me continuous system, alla
# oxi poly mikro wste na kanei 100 wres na kanei sim :)

max_steps = 1000

#-----------------------------

x_hist = [x]
y_hist = [y]

for _ in range(max_steps):
    dx = x_goal - x
    dy = y_goal - y
    rho = np.sqrt(dx**2 + dy**2)
    if rho < 0.05:   # ftasame!
        break

    alpha = -theta + np.arctan2(dy, dx)
    alpha = np.arctan2(np.sin(alpha), np.cos(alpha))
    beta = -theta - alpha
    beta = np.arctan2(np.sin(beta), np.cos(beta))

    v = k_rho * rho
    omega = k_alpha * alpha + k_beta * beta

    # exiswseis unicycle:
    x += v * np.cos(theta) * dt
    y += v * np.sin(theta) * dt
    theta += omega * dt

    x_hist.append(x)
    y_hist.append(y)

# Plots:
plt.figure()
plt.plot(x_hist, y_hist, 'b-', label='Trajectory')
plt.plot(x_goal, y_goal, 'r*', markersize=12, label='Goal (0,0)')
plt.axis('equal')
plt.grid(True)
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.title('Unicycle Robot towards Goal Position (0,0)')
plt.show()