classdef BipedalWalking<handle
    properties
        x0;
        v0;
        z0;
        
        x;
        zc;
        
        delta_t;
        xt;
        zt;
        vt;
        g;
        mass;
        acc;
        Tc;
        t;
        
        left_foot_x;
        left_foot_z;
        right_foot_x;
        right_foot_z;
        support_leg;
        
        orbital_energy;
        target_orbital_energy;
        capture_point;
        
        step_length;
        xf;
        vf;
    end
    
    methods
        function obj = BipedalWalking(x0, v0, z0, delta_t)
            obj.x0 = x0;
            obj.v0 = v0;
            obj.z0 = z0;
            
            obj.x = x0;
            obj.zc = z0;
            
            obj.delta_t = delta_t;
            obj.xt = 0;
            obj.zt = 0;
            obj.vt = 0;
            obj.g = 9.8;
            obj.mass = 10.0;
            obj.acc = 0.0;
            obj.Tc = sqrt(obj.zc / obj.g);
            obj.t = 0;
            
            obj.left_foot_x = 0;
            obj.left_foot_z = 0;
            obj.right_foot_x = 0;
            obj.right_foot_z = 0;
            obj.support_leg = 0;
            
            obj.orbital_energy = 0;
            obj.target_orbital_energy = 0;
            obj.capture_point = 0;
            
            obj.step_length = 0;
            obj.xf = 0;
            obj.vf = 0;
        end
        
        function update(obj)
            obj.t = obj.t + obj.delta_t;
            [obj.xt, obj.vt] = obj.updateXtVt(obj.x0, obj.v0, obj.t);
            obj.zt = obj.zc;
            obj.orbital_energy = obj.updateOrbitalEnergy(obj.xt, obj.vt);
        end
        
        function [xt, vt] = updateXtVt(obj, x0, v0, t)
            tau = t / obj.Tc;
            xt = x0 * cosh(tau) + obj.Tc * v0 * sinh(tau);
            vt = (x0 / obj.Tc) * sinh(tau) + v0 * cosh(tau);
        end
        
        function orbital_energy = updateOrbitalEnergy(obj, x, v)
            orbital_energy = (v^2 - (obj.g / obj.zc) * x^2) / 2;
        end
        
        function capture_point = updateCapturePoint(obj, xt, vt, target_OE)
            if vt^2 < 2 * target_OE
                capture_point = obj.capture_point;
            else
                capture_point = xt + sign(vt) * sqrt((vt^2 - 2 * target_OE) * obj.zc / obj.g);
            end
        end
        
        function switchSupportLeg(obj)
            if obj.support_leg == 0
                obj.x0 = obj.left_foot_x + obj.xt - obj.right_foot_x;
                obj.support_leg = 1;
            elseif obj.support_leg == 1
                obj.x0 = obj.right_foot_x + obj.xt - obj.left_foot_x;
                obj.support_leg = 0;
            end
            
            obj.t = 0;
            obj.v0 = obj.vt;
        end
    end
end