import matplotlib.pyplot as plt
import pandas as pd
import numpy as np

from pyIbex import *
from vibes import vibes 

print("Read Data")
data = pd.read_csv('C:\\Users\\Administrateur\\Desktop\\Nouveau dossier\\NavDaurade.csv')

X, Y= [], []
x,y = Interval(0), Interval(0)
seps = []

print("Compute trajectory")
for i,(dvx, dvy, hdg, alt) in enumerate(zip(data.vx, data.vy, data.heading, data.altitude)):
  dt = 0.05
  hdg = -np.deg2rad(hdg)# - 0.015
  dvx = Interval(dvx).inflate(0.005)
  dvy = Interval(dvy).inflate(0.005) 
  
  
  # downsampling displayed data
  if i%20 == 0:
    R = alt*np.tan(np.deg2rad(78))
    F = Function("x", "y", "(x - %s)^2 + (y - %s)^2 - %f^2"%(y,x,R))
    seps.append(SepFwdBwd(F, CmpOp.LEQ))
    X += [Interval(y)]
    Y += [Interval(x)] 
  
  y = y + dt * ( np.cos(hdg)*dvx - np.sin(hdg)*dvy )
  x = x + dt * ( np.sin(hdg)*dvx + np.cos(hdg)*dvy )

sep = SepUnion(seps)


vibes.beginDrawing()
vibes.newFigure('Trajectory')
for bx, by in zip(X,Y):
  vibes.drawBox(bx[0], bx[1], by[0], by[1])
vibes.setFigureProperties(dict(x=0, y=10, width=500, height=500))
vibes.axisEqual()

print("Run SIVIA")
vibes.newFigure('Path Exploration')
vibes.setFigureProperties(dict(x=0, y=10, width=500, height=500))
X0 = IntervalVector([[-500, 220], [-100, 570]])
vibes.drawBox(X0[0][0], X0[0][1], X0[1][0], X0[1][1], 'b')
vibes.axisEqual()
params = {'color_in':'darkGray[w]', 'color_out':'darkGray[k]', 'color_maybe':'lightGray[lightGray]'}
pySIVIA(X0, sep, 3, **params)

for bx, by in zip(X,Y):
  vibes.drawBox(bx[0], bx[1], by[0], by[1], 'k')
vibes.drawArrow([-400, -50], [-400, 0], 10, 'w[w]')
vibes.drawArrow([-400, -50], [-350, -50], 10, 'w[w]')
vibes.axisEqual()
