% Define parameters
radius = 1; % circle radius
mass = 1; % circle mass
speed = 10;
angle = deg2rad(45);
[xv,yv]=pol2cart(angle,speed);
initialPosition = [-7, -7]; % initial position (x, y)
initialVelocity = [xv, yv]; % initial velocity (vx, vy)
timeStep = 0.1; % time step
simulationTime = 10; % total simulation time
floorY = -8; % y-coordinate of the floor plane
wallX = 8; % x-coordinate of the walls
airResistanceCoeff = 0.1; % air resistance coefficient
elasticityCoeff = 0.8; % elasticity coefficient
torque = 0; % no torque applied
momentOfInertia = 0.5 * mass * radius^2; % moment of inertia

% Initialize variables
position = initialPosition;
velocity = initialVelocity;
angularVelocity = 0;

% Initialize energy variables
kineticEnergy = zeros(1, simulationTime / timeStep + 1);
potentialEnergy = zeros(1, simulationTime / timeStep + 1);

time = 0:timeStep:simulationTime;

figure('Units','normalized','Position',[0.2 0.2 0.7 0.7]); tl = tiledlayout(3,1);

a = 1;

% Simulation loop
for t = time
    acceleration = [0, -9.8]; % example: constant acceleration due to gravity  
    % Apply air resistance
    airResistance = -airResistanceCoeff * velocity;
    acceleration = acceleration + airResistance;
    
    % Update velocity and position
    velocity = velocity + acceleration * timeStep;
    position = position + velocity * timeStep;
    
    % Calculate angular acceleration
    angularAcceleration = torque / momentOfInertia;
    
    % Update angular velocity and position
    angularVelocity = angularVelocity + angularAcceleration * timeStep;
    angle = angle + angularVelocity * timeStep;
    
    % Collision detection with floor
    if position(2) - radius < floorY
        position(2) = floorY + radius;
        velocity(2) = -elasticityCoeff * velocity(2); % reverse and dampen the y-velocity
        angularVelocity = -elasticityCoeff * angularVelocity; % reverse and dampen the angular velocity
    end
    
    % Collision detection with walls
    if position(1) - radius < -wallX
        position(1) = -wallX + radius;
        velocity(1) = -elasticityCoeff * velocity(1); % reverse and dampen the x-velocity
        angularVelocity = -elasticityCoeff * angularVelocity; % reverse and dampen the angular velocity
    elseif position(1) + radius > wallX
        position(1) = wallX - radius;
        velocity(1) = -elasticityCoeff * velocity(1); % reverse and dampen the x-velocity
        angularVelocity = -elasticityCoeff * angularVelocity; % reverse and dampen the angular velocity
	end

	% Calculate the arc length traveled
    arcLength = velocity(1) * timeStep;
    
    % Update angle based on arc length
    angle = angle - arcLength / radius;
    
    % Plot circle
	nexttile(1)
    theta = linspace(0, 2*pi, 100);
    x = position(1) + radius * cos(theta + angle);
    y = position(2) + radius * sin(theta + angle);
    plot(x, y);
    hold on;
    plot([-wallX, wallX], [floorY, floorY], 'k--'); % plot the floor plane
    line([-wallX, -wallX], [-10, 10], 'Color', 'red'); % plot the left wall
    line([wallX, wallX], [-10, 10], 'Color', 'red'); % plot the right wall
	xlabel('X');
	ylabel('Y');
	title('Rolling Circle Simulation');
    
    % Visualize angle
    angleLineX = [position(1), position(1) + radius * cos(angle)];
    angleLineY = [position(2), position(2) + radius * sin(angle)];
    plot(angleLineX, angleLineY, 'r--');
    axis equal;
    xlim([-10, 10]);
    ylim([-10, 10]);
    hold off;

	% Calculate kinetic energy
    kineticEnergy(a) = 0.5 * mass * norm(velocity)^2 + 0.5 * momentOfInertia * angularVelocity^2;
    % Calculate potential energy
    potentialEnergy(a) = mass * 9.8 * (position(2) - radius - floorY);
	% Calculate total energy
	totalEnergy = kineticEnergy + potentialEnergy;

	% Plot total energy
	nexttile(2);
	plot(time, totalEnergy);
	xlabel('Time');
	ylabel('Energy');
	title('Total Energy of the Circle');
	
	% Plot kinetic and potential energy
	nexttile(3);
	plot(time, kineticEnergy, 'b', 'DisplayName', 'Kinetic Energy');
	hold on;
	plot(time, potentialEnergy, 'r', 'DisplayName', 'Potential Energy');
	xlabel('Time');
	ylabel('Energy');
	title('Kinetic (b) and Potential (r) Energy of the Circle');
	
	% Adjust layout
	title(tl, 'Energy Analysis of the Circle');
	drawnow;
	a = a + 1;
end