% Mass point on ground - recitation 09/17/2021
m = 1;
mu = 0.5; % friction coefficient
g = 9.8;
S = 10000;
x0 = [-1 1];
u0 = [0.5 0];
dt = 1e-2;
Tf = 10;
nSt = Tf/dt+1;
x = zeros(nSt,2);
x(1,:)=x0;
u = zeros(nSt,2);
u(1,:)=u0;
for iT=1:nSt
    f = GroundForce2D(x(iT,:),u(iT,:),mu,S,m*g);
    u(iT+1,:)=u(iT,:)+dt/m*f;
    x(iT+1,:)=x(iT,:)+dt*u(iT+1,:);
    plot(x(iT+1,1),x(iT+1,2),'o')
    hold on
    plot([-1 1],[0 0],'-k')
    ylim([-1 1])
    drawnow
    hold off
end

% Kinetic energy
KE = 1/2*m*sum(u.*u,2);
height = Hvals2D(x(:,1),x(:,2));
PE = m*g*height;
ts = (0:nSt)*dt;
energer=PE(height>0)+KE(height>0)-PE(1)-KE(1);
energer=max(abs(energer))

function force = GroundForce2D(x,u,mu,S,mg)
    [h,gradH] = Hvals2D(x(1),x(2));
    force = -mg*[0 1];
    if (h <= 0)
        % Add the ground forces
        n = gradH/norm(gradH); 
        disp = -h/norm(gradH);
        Utan = u - dot(u,n)*n;
        if (norm(Utan) > 1e-10)
            UtanHat = Utan/norm(Utan);
        else
            UtanHat = [0 0];
        end
        force = force + S*(-h/norm(gradH)*(n-mu*UtanHat));
    end
end
    

function [h,gradH] = Hvals2D(x,y) % specifies the ground
    h = y;
    gradH = [0 1];
end

