close all; clear; clc;

x0 = 0.0; v0 = 0.2; z0 = 1.0;
delta_t = 0.01;
bipedal_walking = BipedalWalking(x0, v0, z0, delta_t);
bipedal_walking.target_orbital_energy = 0.1;

data_len = 1001;
switch_index = 50;

orbital_energy_array = [bipedal_walking.orbital_energy];
xt_array = [bipedal_walking.xt];
vt_array = [bipedal_walking.vt];
com_x = [x0];
com_z = [z0];
left_foot_x_array = [bipedal_walking.left_foot_x];
left_foot_z_array = [bipedal_walking.left_foot_z];
right_foot_x_array = [bipedal_walking.right_foot_x];
right_foot_z_array = [bipedal_walking.right_foot_z];

j = 1;
swing_foot_reference_x = [0];
for i = 1:data_len-1
    bipedal_walking.update();
    
    if i <= switch_index
        bipedal_walking.capture_point = bipedal_walking.updateCapturePoint(bipedal_walking.xt, bipedal_walking.vt, bipedal_walking.target_orbital_energy);
        if bipedal_walking.support_leg == 0
            bipedal_walking.right_foot_x = bipedal_walking.left_foot_x + bipedal_walking.capture_point;
            com_x = [com_x, bipedal_walking.xt + bipedal_walking.left_foot_x];
            com_z = [com_z, bipedal_walking.zt];
        else
            bipedal_walking.left_foot_x = bipedal_walking.right_foot_x + bipedal_walking.capture_point;
            com_x = [com_x, bipedal_walking.xt + bipedal_walking.right_foot_x];
            com_z = [com_z, bipedal_walking.zt];
        end
    else
        if bipedal_walking.support_leg == 0
            bipedal_walking.right_foot_x = swing_foot_reference_x(j);
            com_x = [com_x, bipedal_walking.xt + bipedal_walking.left_foot_x];
            com_z = [com_z, bipedal_walking.zt];
        else
            bipedal_walking.left_foot_x = swing_foot_reference_x(j);
            com_x = [com_x, bipedal_walking.xt + bipedal_walking.right_foot_x];
            com_z = [com_z, bipedal_walking.zt];
        end
        j = j + 1;
    end
    
    if (i >= switch_index) && (mod(i, switch_index) == 0)
        bipedal_walking.switchSupportLeg();
        
        [xt, vt] = bipedal_walking.updateXtVt(bipedal_walking.x0, bipedal_walking.v0, bipedal_walking.delta_t * (switch_index));
        capture_point = bipedal_walking.updateCapturePoint(xt, vt, bipedal_walking.target_orbital_energy);
        
        if bipedal_walking.support_leg == 0
            swing_foot_reference_x = linspace(bipedal_walking.right_foot_x, bipedal_walking.left_foot_x + capture_point, switch_index);
        else
            swing_foot_reference_x = linspace(bipedal_walking.left_foot_x, bipedal_walking.right_foot_x + capture_point, switch_index);
        end
        j = 1;
    end
    
    left_foot_x_array = [left_foot_x_array, bipedal_walking.left_foot_x];
    right_foot_x_array = [right_foot_x_array, bipedal_walking.right_foot_x];
end
        
t = (0:data_len-1) * delta_t;
com_x_series = timeseries(com_x, t);
com_z_series = timeseries(com_z, t);
left_foot_x_series = timeseries(left_foot_x_array, t);
right_foot_x_series = timeseries(right_foot_x_array, t);

simOut = sim('sim_main');