#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Aug  5 11:07:43 2021

@author: alex
"""

import numpy as np

## %% Params 
mass=369 #batterie
mass+=1640-114 #corps-carton
mass/=1e3
Area=np.pi*(11.0e-02)**2
r0=11e-02
rho0=1.204
kv_motor=800.0
pwmmin=1075.0
pwmmax=1950.0
U_batt=16.8
PWM_eq=1550
omega_eq=(PWM_eq-pwmmin)/(pwmmax-pwmmin)*U_batt*kv_motor*2*np.pi/60
Nrot=6
g=9.81
b1_formula=r0*omega_eq*np.sqrt(2*rho0*Area*Nrot/mass/g)

print(b1_formula)


# %%   ####### IMPORT DATA 
print("LOADING DATA...")
import pandas as pd

log_path="./logs/vol12/log_real.csv"
raw_data=pd.read_csv(log_path)

print("PROCESSING DATA...")

prep_data=raw_data.drop(columns=[i for i in raw_data.keys() if (("forces" in i ) or ('pos' in i) or ("joy" in i)) ])

if "vol1_ext_alcore" in log_path:
    tmin,tmax=(41,265) 
if "vol2_ext_alcore" in log_path:
    tmin,tmax=(10,140) 
if "vol12" in log_path:
    tmin,tmax=(-1,1e10) 
    
prep_data=prep_data[prep_data['t']>tmin]
prep_data=prep_data[prep_data['t']<tmax]

for i in range(3):
    prep_data['speed_pred[%i]'%(i)]=np.r_[prep_data['speed[%i]'%(i)].values[1:len(prep_data)],0]
    
    
prep_data['dt']=np.r_[prep_data['t'].values[1:]-prep_data['t'].values[:-1],0]
prep_data['t']-=prep_data['t'][0]
prep_data=prep_data.drop(index=[0,len(prep_data)-1])
prep_data=prep_data.reset_index()

data_prepared=prep_data



for i in range(6):
    prep_data['omega_c[%i]'%(i+1)]=(prep_data['PWM_motor[%i]'%(i+1)]-pwmmin)/(pwmmax-pwmmin)*U_batt*kv_motor*2*np.pi/60

# import matplotlib.pyplot as plt

# plt.figure()
# for i in range(3):
#     plt.gcf().add_subplot(3,1,i+1),
#     plt.plot(prep_data["t"],prep_data['acc[%i]'%(i)],label="log acc ")
#     plt.plot(prep_data["t"],prep_data['acc_ned_grad[%i]'%(i)],label="grad calc acc")
#     plt.legend(),plt.grid()

" l'accélération selon k est au max à environ 7 m/s^2, décommenter le plot ci dessus pour voir"

amax=7.0
Fmax_per_rotor=mass*(g+amax)/Nrot
omega_max=U_batt*kv_motor*2*np.pi/60

print("amax",amax,"\nFmax_per_rotor",Fmax_per_rotor,"\nomega_max",omega_max)

" on a alors en supposant v3 nul "

"T_BET=rho0*Area*r0*omega_max*(c1*r0*omega_max-c2*vi)"
" avec c2 = b1 ct1 *- 2/ b1"
"on va prendre le vi du vol stationnaire "

vi_stat=np.sqrt(mass*g/2.0/rho0/Area/Nrot)

print("vi_stat",vi_stat)

" donc T_BET/(rho0*Area*r0*omega_max)=c1*r0*omega-(b1 ct1 *- 2/ b1)*vi"
" T_BET/(rho0*Area*r0*omega_max) =c1 (r0*omega-b1*vi)+2/b1*vi"
" T_BET/(rho0*Area*r0*omega_max)-2/b1*vi =c1 (r0*omega-b1*vi)"
" c1 = T_BET/(rho0*Area*r0*omega_max)/(r0*omega-b1*vi) "

c1 = -(Fmax_per_rotor/(rho0*Area*r0*omega_max)-2/b1_formula*vi_stat)/(r0*omega_max-b1_formula*vi_stat)

print(" C1 estimated :")
print(c1)

print(" C2 estimated :")
print(b1_formula*c1 - 2/ b1_formula)

