% Contains all the measurable parameters of the quadrotor. Must be run
% before running the simulation or any other functions in this directory

% Add Library Blocks and 3D animation to Path
    path_1 = genpath('Library_blocks');
    path_2 = genpath('3D_Animation');
    addpath(path_1, path_2);

% Log Analysis Toggle
    logAnalysisToggle = 0;          % 1 for log analysis, 0 for normal operation
   
% Physics Verification Toggle
    physicsVerificationToggle = 1;

% Define Simulink Runtime (if logAnalysisToggle is selected, this will be
% automatically set based on the log files time)
     runtime = 40;
    
% Model Parameters
    m = 1.216;                          % Quadrotor + battery mass
    g = 9.81;                           % Acceleration of gravity
    Jxx = 0.0110;%0.0130;               % Quadrotor and battery motor of inertia around bx (pitch)
    Jyy = 0.0116;%0.0140;               % Quadrotor and battery motor of inertia around by (roll)
    Jzz = 0.0223;%0.0285;               % Quadrotor and battery motor of inertia around bz (yaw)
    Jreq = 4.2012e-05;                  % Rotor and motor moment of inertia around axis of rotation 
    Kt = 1.2007e-05;                    % Rotor thrust constant
    delta_T = [ 0,  0,  9.404e-04 ];    % Thrust constant adjustment factor
    Kh = 3.4574e-04;                    % Rotor in-plane drag constant
    Kd = 1.4852e-07;                    % Rotor drag constant
    rhx = 0.16;                         % X-axis distance from center of mass to a rotor hub
    rhy = 0.16;                         % Y-axis distance from center of mass to a rotor hub
    rhz = 0.03;                         % Z-axis distance from center of mass to a rotor hub
    r_oc = [0; 0; 0];                   % Vector from origin to center of mass
    Rm = 0.235;                         % Motor resistance
    Kq = 96.3422;                       % Motor torque constant
    Kv = 96.3422;                       % Motor back emf constant
    If = 0.3836;                        % Motor internal friction current
    Pmin = 0;                           % Minimum zybo output duty cycle command
    Pmax = 1;                           % Maximum zybo output duty cycle command
    Tc = 0.01;%0.04;                          % Camera system sampling period
    Tq = 0.005;                         % Quad sampling period
    tau_c = 0;                          % Camera system total latency
    Vb = 12.3;                          % Nominal battery voltage (V)
    x_controlled_o = 0;                 % Equilibrium lateral controller output
    y_controlled_o = 0;                 % Equilibrium longitudinal controller output
    yaw_controlled_o = 0;               % Equilibrium yaw controller output
    Kp_mahony = 0.4;                    % Proportional term for mahony filter
    Ki_mahony = 0.001;                  % Integral term for mahony filter
    
    % Define Biquad Filter Parameters
    accel_Fc = 10;
    Fs = 1/Tq;
    accel_K = tan(pi * accel_Fc / Fs);
    Q = 0.707;
    accel_norm = 1.0 / (1.0 + accel_K/Q + accel_K*accel_K);
    
    accel_b0 = accel_K*accel_K*accel_norm;
    accel_b1 = 2.0*accel_b0;
    accel_b2 = accel_b0;
    accel_a0 = 1;
    accel_a1 = 2.0*(accel_K*accel_K -1) * accel_norm;
    accel_a2 = (1.0 - accel_K/Q + accel_K*accel_K) * accel_norm;
    
    accel_SOS_Matrix = [accel_b0, accel_b1, accel_b2, accel_a0, accel_a1, accel_a2];
    
    OF_Fc = 7;
    OF_K = tan(pi * OF_Fc / Fs);
    
    OF_norm = 1.0 / (1.0 + OF_K/Q + OF_K*OF_K);
    
    OF_b0 = OF_K*OF_K*OF_norm;
    OF_b1 = 2.0*OF_b0;
    OF_b2 = OF_b0;
    OF_a0 = 1;
    OF_a1 = 2.0*(OF_K*OF_K -1) * OF_norm;
    OF_a2 = (1.0 - OF_K/Q + OF_K * OF_K) * OF_norm;
    
    OF_SOS_Matrix = [OF_b0, OF_b1, OF_b2, OF_a0, OF_a1, OF_a2];
    
    % Determine Initial Conditions
    
    % Position Initial Conditions
    x_o = 0;
    y_o = 0;
    z_o = 0;
    
    % Velocity Initial Conditions
    x_vel_o = 0;
    y_vel_o = 0;
    z_vel_o = 0;
    
    % Euler Angle Initial Conditions
    roll_o = 0;
    pitch_o = 0;
    yaw_o = 0;
    
    % Euler Rate Initial Conditions
    rollrate_o = 0;
    pitchrate_o = 0;
    yawrate_o = 0;
    
    % Equilibrium rotor speeds
    omega0_o = sqrt((m*g)/(4*Kt));
    omega1_o = sqrt((m*g)/(4*Kt));
    omega2_o = sqrt((m*g)/(4*Kt));
    omega3_o = sqrt((m*g)/(4*Kt));
    
    % Equilibrium height controller output
    height_controlled_o = (((Rm*If   ...
    + (((omega0_o * 2 * Rm * Kv * Kq ...
    * Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
    * Kd))/Vb)*(Pmax - Pmin) + Pmin);
       
if logAnalysisToggle == 1
    %%%%%% Commented out section until logging is .txt file based %%%%%%
    % FNAME
    % if you know the name of the log file that you want to parse, set the it
    % here only if it is in the working directory. Otherwise, you may leave it
    % blank. You will be able to choose the file to parse through an explorer
    % window.
    %
    %fname = 'sampleLogFile.txt';  
    fname = '';
    fpath = 'C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\model\logFiles\';
    
    if(isempty(fname))
        [fname] = uigetfile('.txt','Select log file', fpath);
    end
    
    params.file.name = fname;               % file name only
    params.file.path = fpath;               % file path only
    params.file.pathName = [fpath fname];   % file path with file name
    
    [dataStruct, headers] = parse_log_model(params.file.pathName);
    
    time = dataStruct.Time.data;
    time = time - time(1);
    time_indices = length(time);
    
    runtime = max(time);
    
    % Determine x position error
    x_setpoint = dataStruct.X_Setpoint_Constant.data;
    x_pos_raw = dataStruct.VRPN_X_Constant.data;
    x_error = timeseries(x_setpoint - x_pos_raw, time);
    x_position = timeseries(x_pos_raw, time);

    % Determine y position error
    y_setpoint = dataStruct.Y_Setpoint_Constant.data;
    y_pos_raw = dataStruct.VRPN_Y_Constant.data;
    y_error = timeseries(y_setpoint - y_pos_raw, time);
    y_position = timeseries(y_pos_raw, time);

    % Determine z position error
    z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
    z_pos_raw = dataStruct.VRPN_Alt_Constant.data;
    z_error = timeseries(z_setpoint - z_pos_raw, time);
    z_position = timeseries(z_pos_raw, time);
    
    % Determine x velocity error
    x_vel_setpoint = dataStruct.X_pos_PID_Correction.data;
    x_vel_raw = dataStruct.X_Vel_Correction.data;
    x_vel_error = timeseries(x_vel_setpoint - x_vel_raw, time);
    x_vel = timeseries(x_vel_raw, time);
    
    % Determine y velocity error
    y_vel_setpoint = dataStruct.Y_pos_PID_Correction.data;
    y_vel_raw = dataStruct.Y_Vel_Correction.data;
    y_vel_error = timeseries(y_vel_setpoint - y_vel_raw, time);
    y_vel = timeseries(y_vel_raw, time);

    % Determine pitch error
    pitch_setpoint = dataStruct.X_Vel_PID_Correction.data;
    pitch_angle_raw = dataStruct.Pitch_trim_add_Sum.data;
    pitch_error = timeseries(pitch_setpoint - pitch_angle_raw, time);
    pitch_angle = timeseries(pitch_angle_raw, time);

    % Determine roll error
    roll_setpoint = dataStruct.Y_Vel_PID_Correction.data;
    roll_angle_raw = dataStruct.Roll_Constant.data;
    roll_error = timeseries(roll_setpoint - roll_angle_raw, time);
    roll_angle = timeseries(roll_angle_raw, time);
    
    % Determine yaw error
    yaw_setpoint = dataStruct.Yaw_Setpoint_Constant.data;
    yaw_angle_raw = dataStruct.Yaw_Constant.data;
    yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
    yaw_angle = timeseries(yaw_angle_raw, time);

    % Determine pitch rate error
    pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
    pitchrate_raw = dataStruct.gyro_y.data;
    pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_raw, time);
    pitchrate = timeseries(pitchrate_raw, time);

    % Determine roll rate error
    rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
    rollrate_raw = dataStruct.gyro_x.data;
    rollrate_error = timeseries(rollrate_setpoint - rollrate_raw, time);
    rollrate = timeseries(rollrate_raw, time);

    % Determine yaw rate error
    yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
    yawrate_raw = dataStruct.gyro_z.data;
    yawrate_error = timeseries(yawrate_setpoint - yawrate_raw, time);
    yawrate = timeseries(yawrate_raw, time);

    % Pull motor commands from log
    x_command = dataStruct.Pitch_Rate_PID_Correction.data;
    y_command = dataStruct.Roll_Rate_PID_Correction.data;
    z_command = dataStruct.Altitude_PID_Correction.data;
    yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
    
    % Determine signal mix PWM values
    PWM0 = dataStruct.Signal_Mixer_MOTOR_0.data;
    PWM1 = dataStruct.Signal_Mixer_MOTOR_1.data;
    PWM2 = dataStruct.Signal_Mixer_MOTOR_2.data;
    PWM3 = dataStruct.Signal_Mixer_MOTOR_3.data;
    PWM_arr = ...
        [PWM0, PWM1, PWM2, PWM3];
    motor_commands = timeseries(PWM_arr, time);
    
    % Pull the measurements from the acceleratometer
    raw_accel_data_x = dataStruct.accel_x.data;
    raw_accel_data_y = dataStruct.accel_y.data;
    raw_accel_data_z = dataStruct.accel_z.data;
    raw_accel_data_arr = ...
        [ raw_accel_data_x , raw_accel_data_y , raw_accel_data_z ];
    raw_accel_data = timeseries( raw_accel_data_arr , time );
    
    % Pull the measurements from the gyroscope
    raw_gyro_data_x = dataStruct.gyro_x.data;
    raw_gyro_data_y = dataStruct.gyro_y.data;
    raw_gyro_data_z = dataStruct.gyro_z.data;
    raw_gyro_data_arr = ...
        [ raw_gyro_data_x , raw_gyro_data_y , raw_gyro_data_z ];
    raw_gyro_data = timeseries( raw_gyro_data_arr , time );
        
    % Pull the measurements from the complimentary filter
    pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
    roll_measured_IMU = dataStruct.Roll_Constant.data;
    IMU_angle_arr = ...
        [roll_measured_IMU, pitch_measured_IMU];
    IMU_angle_data = timeseries( IMU_angle_arr, time);
    
    % Pull VRPN pitch and roll
    pitch_measured_VRPN = dataStruct.Pitch_trim_add_Sum.data;
    
    % Define setpoint timeseries
    x_setpoint_model = timeseries(x_setpoint, time);
    y_setpoint_model = timeseries(y_setpoint, time);
    z_setpoint_model = timeseries(z_setpoint, time);
end

if physicsVerificationToggle == 1
    % PHYSICAL SYSTEM PARAMETERS
    % quadrotor + battery inertia tensor
    J = diag([ Jxx Jyy Jzz ]);
    % quadrotor + battery body frame origin to c.o.g. error radius
    r_oc = [ 0 ; 0 ; 0 ]; 
    
    % mass of rigidly attached load
    m_L = 0;
    % dimensions of assuemd rectangular load
    x_L = 0; y_L = 0; z_L = 0; 
    % inertia tensor of load
    J_L = m_L/12*diag([y_L^2+z_L^2,x_L^2+z_L^2,x_L^2+y_L^2]);
    % location of load center of mass in body frame
    r_oL = [0; 0; 0];%[ 0.12 ; 0.12 ; -0.04];%(0.04 + z_L/2) ]; 
    %r_oL = [ .115 ; .115 ; 0.04]; 
    
    % acceleration of gravity 
    g = 9.81; 
    
    % initial dynamics states conditions 
    u0 = 0; 
    v0 = 0; 
    w0 = 0; 
    p0 = 0; 
    q0 = 0; 
    r0 = 0; 
    x0 = 0; 
    y0 = 0; 
    z0 = 0; 
    phi0 = 0; 
    theta0 = 0; 
    psi0 = 0;
    
    % ROTOR PARAMETERS
    
    % nominally equal rotor parameters
    K_T = cell(1,4); K_H = cell(1,4); K_d = cell(1,4); 
    Delta_T = cell(1,4); 
    %Delta_T = [0 0 0];
    J_r = cell(1,4); Gamma_H = cell(1,4); 
    for i = 1:4
        % rotor thrust constants
        K_T{i} = Kt;
        % rotor H-force coefficeint
        K_H{i} = Kh;
        % rotor drag constant
        K_d{i} = Kd;
        % rotor b_z axis velocity thrust adjustment factor 
        Delta_T{i} = 0.5*[ 0 0 2.351e-4 ]; 
        % equivalent rotor + motor moment of inertia 
        J_r{i} = Jreq;
        % h-force xy-plane selection matrix
        Gamma_H{i} = [ 1 0 0 ; 0 1 0 ; 0 0 0 ]; 
    end
    
    % ESC-MOTOR PARAMETERS 
    have_motor_transient = 1;  
    % nominally equal motor and eletronic speed control parameters
    R_m = cell(1,4); K_Q = cell(1,4); K_V = cell(1,4); 
    i_0 = cell(1,4); omega_0 = cell(1,4); K_E = cell(1,4); 
    p_0 = cell(1,4); p_max = cell(1,4); 
    for i = 1:4
        % motor resistance
        R_m{i} = 0.235; 
        % motor torque constant
        K_Q{i} = 96.3422; 
        % motor back-emf constant
        K_V{i} = 96.3422; 
        % motor no-load current 
        i_0{i} = 0.3836; 
        % initial motor/rotor speed
        omega_0{i} = (m*g/4/K_T{1})^0.5;
        % ESC normalized pwm input factor
        K_E{i} = 1;
        % ESC turn-on duty cycle
        p_0{i} = 0; 
        % ESC maximum duty cycle (as calibrated)
        p_max{i} = 1; 
    end
    
    
    
    % ROTOR LOCATION/ORIENTATION PARAMETERS
    
    % locatin of hub 1 in body frame
    r_h{1} = [ rhx ; rhy ; -rhz ]; 
    % rotor 1 thrust unit vector
    Gamma_T{1} = [ 0 ; 0 ; -1 ]; 
    % rotor 1 rotation unit vector
    Gamma_Omega{1} = [ 0 ; 0 ; -1 ]; 
    
    r_h{2} = [ -rhx ; rhy ; -rhz ];
    Gamma_T{2} = [ 0 ; 0 ; -1 ]; 
    Gamma_Omega{2} = [ 0 ; 0 ; 1 ]; 
    
    r_h{3} = [ -rhx ; -rhy ; -rhz ]; 
    Gamma_T{3} = [ 0 ; 0 ; -1 ];
    Gamma_Omega{3} = [ 0 ; 0 ; -1 ];
    
    r_h{4} = [ rhx ; -rhy ; -rhz ]; 
    Gamma_T{4} = [ 0 ; 0 ; -1 ];
    Gamma_Omega{4} = [ 0 ; 0 ; 1 ]; 
    
    
    
    % BATTERY PARAMETERS
    
    % starting/operating battery voltage
    V_0 = 11.4; %12.4; %11.4; 
    % approximate hovering rate of battery discharge
    d_V = 0; 
    
    % DISTURBANCE PARAMETERS
    
    F_D = 1*randn(3,1); 
    Q_D = 0.05*randn(3,1);
end

% Signal mixer using rotor layout
% 1 2
% 3 4
signal_mixer = [ 1, -1, -1, -1 
                 1, -1,  1,  1
                 1,  1, -1,  1
                 1,  1,  1, -1 ];

% Linearize model, load preferred weight set, and build LQR model
[ssA, ssB] = linearization(m, g, Kt, Kh, Kd, delta_T(3), rhx, rhy, rhz, J, Jreq, Vb, Rm, Kv, Kq, If, signal_mixer);
load('weights_modern.mat');
% Set control period to equal period of camera system
T_LQR = Tc;
[lqr_K, lqr_S, lqr_E] = initial_lqr(ssA, ssB, state_weights, command_weights, T_LQR);