classdef Simulation
    
    properties
        timestep=1/30;
        gravity=[0 0 -10]';
        numIterations=20;
        
        bodies = Body;
        rodConstraints = RodConstraint;
    end   
    
    methods
        function updatedSimulation = Update(simulation)

            numBodies = length(simulation.bodies);
            numRodConstraints = length(simulation.rodConstraints);
            
            for i = 1:numBodies
                if(simulation.bodies(i).dynamic)
                    % Add gravity to acceleration. Convert from continuous
                    % to discrete acceleration by multiplying by the
                    % timestep squared.
                    simulation.bodies(i).accelerationDisplacement = simulation.bodies(i).accelerationDisplacement + simulation.timestep*simulation.timestep*simulation.gravity;
                    
                    simulation.bodies(i).displacement =  simulation.bodies(i).displacement + simulation.bodies(i).accelerationDisplacement;
                    simulation.bodies(i).angularDisplacement = simulation.bodies(i).angularDisplacement + simulation.bodies(i).angularAccelerationDisplacement;
                end
            end
            
            % Solve constraints with sequential impulses
            for k = 1:simulation.numIterations
                for i = 1:numRodConstraints
              
                    constraint = simulation.rodConstraints(i);
                    candidateBodyA = simulation.bodies(constraint.indexA); 
                    candidateBodyB = simulation.bodies(constraint.indexB);
                    
                    [newBodyA newBodyB] = constraint.Solve(candidateBodyA,candidateBodyB);
                    simulation.bodies(constraint.indexA) = newBodyA;
                    simulation.bodies(constraint.indexB) = newBodyB;
                end
            end
            
            % Euler step positions, orientations and wipe accelerations
            for i = 1:numBodies
                if(simulation.bodies(i).dynamic)
                    
                    simulation.bodies(i).position = simulation.bodies(i).position + simulation.bodies(i).displacement;
                                        
                    angularDisplacementQuaternion = quaternionFromAxisAngle(simulation.bodies(i).angularDisplacement);
                    simulation.bodies(i).orientation = qMult(angularDisplacementQuaternion,simulation.bodies(i).orientation);
                    
                    simulation.bodies(i).accelerationDisplacement = [0 0 0]';
                    simulation.bodies(i).angularAccelerationDisplacement = [0 0 0]';
                end
            end
            
            updatedSimulation = simulation;
            
        end
    end
end

