# -*- coding: utf-8 -*-
import os
os.chdir("D:\Documents(D)\Bellevue\\2-PCSI1\\3_TPs\Cycle 8\TP8.1 - Berce BB\MobyCrea_experimental")

import numpy as np
import matplotlib.pyplot as plt

# Ouverture du fichier de coordonnées du point B
fichier_B=open('Point_B.txt','r')
contenu_B=fichier_B.read()
fichier_B.close()
lignes_B=contenu_B.split('\n')

# Ouverture du fichier de coordonnées du point C
fichier_C=open('Point_C.txt','r')
contenu_C=fichier_C.read()
fichier_C.close()
lignes_C=contenu_C.split('\n')


# La fonction data_gen extrait les données t, x, y du fichier de coordonnées
def data_gen(lignes):
    t=[]
    x=[]
    y=[]
    for ligne in lignes[2:-1]:
        data=ligne.replace(',','.').split('\t')
        t.append(float(data[0]))
        x.append(float(data[1]))
        y.append(float(data[2]))
    return np.array(t),np.array(x),np.array(y)

tB,xB,yB=data_gen(lignes_B)
tC,xC,yC=data_gen(lignes_C)
t=tB

# Calcul des coordonnées du point A à partir des positions extrêmes du points B
xA=((np.max(xB)+np.min(xB))/2)*np.ones(len(tB))
yA=((np.max(yB)+np.min(yB))/2)*np.ones(len(tB))

# Tracé des trajectoires des points A, B et C
plt.figure('Trajectoires')
plt.axis('equal')
plt.scatter(xB,yB,color='green')
plt.scatter(xC,yC,color='blue')
plt.scatter(xA,yA,color='red')

# Calcul de theta_5 à partir des relevés expérimentaux
theta_5=np.pi+np.arctan((yB-yA)/(xB-xA))
theta_52=[theta_5[0]]
dt=0
im=0

for i,te in enumerate(theta_5[1:]):
    if np.abs(theta_5[i-1]-te)>1 and np.abs(i-im)!=1:
        im=i
        dt-=np.pi
    theta_52.append(te+dt)
theta_5=np.array(theta_52)

# Tracé de theta_5
plt.figure('Theta_5')
plt.plot(tB,theta_5)

# Calcul des distances lAB et lBC à partir des relevés expérimentaux
lAB=np.average(np.sqrt((xA-xB)**2+(yA-yB)**2))
lBC=np.average(np.sqrt((xC-xB)**2+(yC-yB)**2))

# Calcul du facteur d'échelle
lAB_reel=0.019
ech=lAB_reel/lAB

# Appliquer le facteur d'échelle sur les distances
lAB=lAB_reel
lBC=ech*lBC

# Calcul de theta_6 à partir des relevés expérimentaux
theta_6=np.arctan((yB-yC)/(xB-xC))
theta_62=[theta_6[0]]
dt=0
im=0
for i,te in enumerate(theta_6[1:]):
    if np.abs(theta_6[i-1]-te)>1 and np.abs(i-im)!=1:
        im=i
        dt+=np.pi*np.sign(theta_6[i-1]-te)
    theta_62.append(te+dt)
theta_6=np.array(theta_62)

# Calcul de theta_6 à partir de la fermeture géométrique
theta_6_model=np.arccos(-lAB*np.cos(theta_5)/lBC)

# Tracé des deux formes de theta_6
plt.figure('Theta_6')
plt.plot(tB,theta_6,tB,theta_6_model)

# Calcul de y à partir des relevés expérimentaux
y=ech*(yC-yA)

# Calcul de y à partir de la fermeture géométrique
y_model=lAB*np.sin(theta_5)+lBC*np.sin(theta_6)

# Tracé des deux formes de y
plt.figure('y')
plt.plot(tB,y,tB,y_model)

dt=t[1:]-t[:-1]

# Calcul de omega_5
omega_5=(theta_5[1:]-theta_5[:-1])/dt

# Calcul de omega_6
omega_6=(theta_6[1:]-theta_6[:-1])/dt

omega_6_model=-lAB*np.sin(theta_5[:-1])/(lBC*np.sin(theta_6[:-1]))*omega_5

plt.figure('Omega_6')
plt.plot(t[:-1],omega_6,t[:-1],omega_6_model)

# Calcul de v
v=(y[1:]-y[:-1])/dt

v_model=lAB*np.cos(theta_5[:-1])*omega_5+lBC*np.cos(theta_6[:-1])*omega_6

plt.figure('v')
plt.plot(t[:-1],v,t[:-1],v_model)