import sympy as sy
import numpy as np
import matplotlib.pyplot as plt

# Exercício 1: Vetores de um robô #

#Um robô pode pode rodar por um  ângulo ang (sentido horario), e pode avançar em linha reta dist metros
# intrucoes = [(ang,dist), (ang,dist), ...]
#O robô começa na origem, orientado ao longo do eixo x

#a
#Calcule a posição do robô após cada passo.
#Faça um gráfico da trajetória do robô.

# Δx = dist × cos(θ)
# Δy = dist × sin(θ)

inst = [(45,3), (90,2), (45,3), (45,2), (90,3)]
pos = [(0,0)]
ang0 = 0


for ang, dist in inst:
    ang0 = ang0 - ang  
    dx = dist * np.cos(np.radians(ang0))
    dy = dist * np.sin(np.radians(ang0))
    x, y = pos[-1]
    pos.append((x + dx, y + dy))
   

for i, (x, y) in enumerate(pos):
    print(f"Passo {i}: ({x:.2f}, {y:.2f})")


# Cria o gráfico
for i in range(len(pos) - 1):
    x0, y0 = pos[i]
    x1, y1 = pos[i+1]
    dx = x1 - x0
    dy = y1 - y0
    plt.arrow(x0, y0, dx, dy, color='r', width=0.01, length_includes_head=True)

#xs, ys = zip(*pos)     #os pontos 
#plt.plot(xs, ys, 'bo-')
plt.xlabel("X")
plt.ylabel("Y")
plt.title("Trajetória do Robô")
plt.grid(True)
plt.axis('equal')
plt.show()

#b
#Quais são as coordenadas finais do robô?

x, y = pos[-1]
x = round(x, 2)
y = round(y, 2)
print(f"Coordenadas finais: ({x}, {y})")

#c
#Qual é a instrução necessária para fazer o robô retornar ao ponto inicial?

#dist = sqrt(x**2 + y**2) (modulo do vetor)

xf, yf = pos[-1]
dist = np.sqrt(xf**2 + yf**2)


ang_needed = - (ang0 % 360) ####### corrigir

print(f"Para retornar à origem: ({ang_needed:.2f} graus, {dist:.2f} metros)")