# Path: rawe/models/arianne_conf.py
# def makeConf():
#     return \
#         {'g': 9.81,  #  gravitational constant #  [ m /s^2]
#          'rho': 1.23,  #  density of the air #  [ kg/m^3]
# 
#         'alpha0deg': -12.0,
# 
#          # forces
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cL0': 0.145115,
#          'cL_A':  5.474703,
# 
#          'cD_A': 0.027310,
#          'cD_A2': 1.948549,
#          'cD_B2': -0.013039,
#          'cD0':  0.006600,
# 
#          'cY_B': -0.131871,
# 
#          # control surface forces
#          #note sign change from avl to local coords
#          'cL_elev': -0.0074*180/pi,
# 
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cD_elev2': 3.52135e-05, 'cD_A_elev': -0.000101006, 'cD_elev': -6.67268e-06,
#          'cD_ail2':0.000120247, 'cD_B_ail':-1.89122e-05, 'cD_ail':0,
# 
# 
# 
#          # stability derivatives
#          # these are average values across the vlm alpha/beta sweep
#          'cl_p': -0.4928, 'cl_q':    0.0, 'cl_r': 0.0566,
#          'cm_p':     0.0, 'cm_q':-16.737, 'cm_r':    0.0,
#          'cn_p': -0.0223, 'cn_q':    0.0, 'cn_r':-0.0636,
# 
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cl_B': -0.057367,
#          'cl_AB': -0.437572,
# 
#          'cm_A':-2.285041,
#          # cm0 valid for CG/bridle location 0.1 meters behind main wing leading edge
#          'cm0':-0.056816,
# 
#          'cn_B': 0.038296,
#          'cn_AB':-0.052142,
#          
#          # Scaling for CL and CD (old hacks we have done before
#          'cL_scale': 1.0,
#          'cD_scale': 1.0,
# 
#          # control surface moments
#          # these are average values across the vlm alpha/beta sweep
#          #note sign change from avl to local coords
#          'cl_ail':0.0056*180/pi,
#          'cm_elev':0.0262*180/pi,
# 
# 
#          #[kite]
#          'mass':  0.685,  #  mass of the kite   #  [ kg]
#          'tether_mass': 0.0,
# 
#          # use aerodynamic approximations instead of triginometry
#          'alpha_beta_computation':'first_order',
# #         'alpha_beta_computation':'closed_form',
# 
#          # use cos(delta), sin(delta) as states
#          'delta_parameterization':'cos_sin',
# 
#          'sref': 0.1051,
#          'bref': 0.96, #sqrt(sref*AR)
#          'cref': 0.128, #sqrt(sref/AR)
# 
#          'zt': 0.025, # Kurt's suggestion, April 4 2014
#          'xt': 0.00, # 0.018, # quick fix; 0.0 according to Kurt, April 4 2014
# 
#          #INERTIA MATRIX (Kurt's direct measurements)
#          'j1': 0.0163,
#          'j31': -0.0006,
#          'j2': 0.0078,
#          'j3': 0.0229,
# 
#          # [carousel]
#          'rArm': 1.085,
# 
#          # Carousel Friction & inertia
#          'jCarousel': 1e1,
#          'cfric': 1e1,
#          
#          # Physical limitations for the plane actuators
#          'aileron_bound': 0.468, # [rad] <> ~27 deg
#          'elevator_bound': 0.78, # [rad] <> ~45 deg
#         }


import copy
import casadi as C
import matplotlib.pyplot as plt
import numpy
import scipy.io
import pickle
import rawe
import rawekite
import carousel_dae
import load_data
from numpy import pi
from autogen.tocarousel_sysidProto import toProto
from autogen.carousel_sysid_pb2 import Trajectory
from rawe.models.arianne_conf import makeConf
        from rawekite.carouselSteadyState import getSteadyState


if __name__=='__main__':
    nk = 100
    tStart = 20.0 # [sec]
    tEnd   = 21.0 # [sec]
    T = tEnd - tStart
    ts = T / float(nk)

    # load the data
    (data,interval,_) = load_data.load(tStart, tEnd, ts)

    # fix some signs in the data
    data['encoder']['sin_delta'] = -data['encoder']['sin_delta']

    conf = makeConf()
    conf['useVirtualForces'] = 'random_walk'
    conf['useVirtualTorques'] = 'random_walk'
    dae = carousel_dae.makeDae(conf)

    print "setting up ocp..."
    ocp = setupOcp(dae,conf,T,nk,deg=3)

    lineRadiusGuess = 1.7

    # trajectory for initial guess
    delta0 = numpy.arctan2(data['encoder']['sin_delta'][0], data['encoder']['cos_delta'][0])
    ddelta0 = -data['encoder']['speed_rpm'][0]*2*pi/60

    def get_steady_state_guess():

# Path: rawe/models/arianne_conf.py
# def makeConf():
#     return \
#         {'g': 9.81,  #  gravitational constant #  [ m /s^2]
#          'rho': 1.23,  #  density of the air #  [ kg/m^3]
# 
#         'alpha0deg': -12.0,
# 
#          # forces
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cL0': 0.145115,
#          'cL_A':  5.474703,
# 
#          'cD_A': 0.027310,
#          'cD_A2': 1.948549,
#          'cD_B2': -0.013039,
#          'cD0':  0.006600,
# 
#          'cY_B': -0.131871,
# 
#          # control surface forces
#          #note sign change from avl to local coords
#          'cL_elev': -0.0074*180/pi,
# 
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cD_elev2': 3.52135e-05, 'cD_A_elev': -0.000101006, 'cD_elev': -6.67268e-06,
#          'cD_ail2':0.000120247, 'cD_B_ail':-1.89122e-05, 'cD_ail':0,
# 
# 
# 
#          # stability derivatives
#          # these are average values across the vlm alpha/beta sweep
#          'cl_p': -0.4928, 'cl_q':    0.0, 'cl_r': 0.0566,
#          'cm_p':     0.0, 'cm_q':-16.737, 'cm_r':    0.0,
#          'cn_p': -0.0223, 'cn_q':    0.0, 'cn_r':-0.0636,
# 
#          # these are calculated by a matlab fitting script based on vlm alpha/beta sweeps
#          'cl_B': -0.057367,
#          'cl_AB': -0.437572,
# 
#          'cm_A':-2.285041,
#          # cm0 valid for CG/bridle location 0.1 meters behind main wing leading edge
#          'cm0':-0.056816,
# 
#          'cn_B': 0.038296,
#          'cn_AB':-0.052142,
#          
#          # Scaling for CL and CD (old hacks we have done before
#          'cL_scale': 1.0,
#          'cD_scale': 1.0,
# 
#          # control surface moments
#          # these are average values across the vlm alpha/beta sweep
#          #note sign change from avl to local coords
#          'cl_ail':0.0056*180/pi,
#          'cm_elev':0.0262*180/pi,
# 
# 
#          #[kite]
#          'mass':  0.685,  #  mass of the kite   #  [ kg]
#          'tether_mass': 0.0,
# 
#          # use aerodynamic approximations instead of triginometry
#          'alpha_beta_computation':'first_order',
# #         'alpha_beta_computation':'closed_form',
# 
#          # use cos(delta), sin(delta) as states
#          'delta_parameterization':'cos_sin',
# 
#          'sref': 0.1051,
#          'bref': 0.96, #sqrt(sref*AR)
#          'cref': 0.128, #sqrt(sref/AR)
# 
#          'zt': 0.025, # Kurt's suggestion, April 4 2014
#          'xt': 0.00, # 0.018, # quick fix; 0.0 according to Kurt, April 4 2014
# 
#          #INERTIA MATRIX (Kurt's direct measurements)
#          'j1': 0.0163,
#          'j31': -0.0006,
#          'j2': 0.0078,
#          'j3': 0.0229,
# 
#          # [carousel]
#          'rArm': 1.085,
# 
#          # Carousel Friction & inertia
#          'jCarousel': 1e1,
#          'cfric': 1e1,
#          
#          # Physical limitations for the plane actuators
#          'aileron_bound': 0.468, # [rad] <> ~27 deg
#          'elevator_bound': 0.78, # [rad] <> ~45 deg
#         }


# Path: kite_sysid/lets_id_some_sys.py
import copy
import casadi as C
import matplotlib.pyplot as plt
import numpy
import scipy.io
import pickle
import rawe
import rawekite
import carousel_dae
import load_data
from numpy import pi
from autogen.tocarousel_sysidProto import toProto
from autogen.carousel_sysid_pb2 import Trajectory
from rawe.models.arianne_conf import makeConf
        from rawekite.carouselSteadyState import getSteadyState



if __name__=='__main__':
    nk = 100
    tStart = 20.0 # [sec]
    tEnd   = 21.0 # [sec]
    T = tEnd - tStart
    ts = T / float(nk)

    # load the data
    (data,interval,_) = load_data.load(tStart, tEnd, ts)

    # fix some signs in the data
    data['encoder']['sin_delta'] = -data['encoder']['sin_delta']

    conf = makeConf()
    conf['useVirtualForces'] = 'random_walk'
    conf['useVirtualTorques'] = 'random_walk'
    dae = carousel_dae.makeDae(conf)

    print "setting up ocp..."
    ocp = setupOcp(dae,conf,T,nk,deg=3)

    lineRadiusGuess = 1.7

    # trajectory for initial guess
    delta0 = numpy.arctan2(data['encoder']['sin_delta'][0], data['encoder']['cos_delta'][0])
    ddelta0 = -data['encoder']['speed_rpm'][0]*2*pi/60

    def get_steady_state_guess():

        conf_ss = makeConf()