clear all
%Please read comments at bottom - Thank you!

%Initial Values ---
mass = 0.43;%kg
circum = 0.69;%m
radius = circum/(6.28);% Rad = cirumference/2*pi
A = (pi)*radius^(2);%Calculating the cross sectional area from the radius
rho = 1.22;%kg/m^3
C_pd = 0.3;%Drag Coefficient

phi = input('Enter the angle between the x axis and velocity vector - from 0 to pi/2 radians' );
theta = input('Enter the angle between the xy-plane and velocity vector (vertical angle) - from 0 to pi/2 radians');
vnorm = 30;%This is the norm of v, the initial speed
vx = vnorm*cos(theta)*cos(phi); %initial x velocity in terms of the input angles and the norm
vy = vnorm*cos(theta)*sin(phi); %intitial y velocity in terms of the input angles and the norm
vz = vnorm*sin(theta); % initial z velocity in terms of the input angles and the norm


v = [vx vy vz];%Instantiating the velocity vector
omega = [0 0 10];%The rotation is 10 rads/s around the z axis
accel = [0  0  0]; %Instantiating the acceleration vector
r = [0 0 0]; %The position vector - the ball is initially at the origin
f = [v(1),v(2),v(3),accel(1),accel(2),accel(3)]; %We use the velocity and acceleration vectors to create a state vector
F_pd = -0.5*rho*C_pd*A*vnorm*v;%This is the equation for drag force
F_l = rho*radius^(3)*cross(omega,v);%Here we have our equation for the lift force
n = 10000; %here is our step max
time = 0; %Setting up our initial time in seconds
tau = .1; %Our time step is 1 second

%Now we will set up our runge kutta integration loop & boundary conditions


for i = 1:n %our iterative interval
    accel = (1/mass)*(F_pd) + (1/mass)*(F_l);%Here I have acceleration in the loop in order to update our vmid(which we will need in accel mid and thus in v and in our determination of r)
    vmid =  v + tau*accel/2;%Our velocity midpoint
    accelmid = (1/mass)*(-0.5*rho*C_pd*A*vnorm*vmid) + (1/mass)*(rho*radius^(3)*cross(omega,vmid)); %Here we are utilizing vmid to establish a midpoint for our acceleration function which we can use in the RK2 procedure to update our velocity
    v = v + tau*accelmid/2;%Here we are updating the velocity using the mid point acceleration
    r = r + tau*vmid/2; %Here we are updating our position vector using the midpoint velocity
    x = r(1); %Attempt at dummy variable for x
    y = r(2);
    z = r(3);
    dummyx = x(i);
    dummyy = y(i);
    dummyz = z(i);
    time = time + tau; %Here we have our time advancing by 1 second each iteration thus giving us n second on our plot
    if ((r(1) < 0) || (r(1) > 105))%If the ball leaves out the left or right side of the field...
        disp('The ball has left the field');%Say that and..
        break %Stop the loop
    elseif (( r(2) < 0) || (r(2) > 68))%If the ball leaves the top or bottom of the field...
        print('The ball has left the field');%Say that and...
        break %Stop the loop
    elseif (r(3) < 0)%If the ball hits the ground after being kicked and before reaching the goal...
        print('The ball did not make it to the goal')%Say that and...
        break %Stop the loop
    elseif ((r(1) == 0) && ((r(2) > 30.34)&&(r(2) < 37.66)) && (r(3) < 2.44))%If the ball reaches the goal area...
        disp('Goooooaaaaaaaalllllllllllll!!!');%Say that and...
        disp(phi) %Return the value of phi which lead to your goal
        disp(theta) %Return the value of theta which lead to the goal
        break
    end
end


figure(1);
hold on
plot3(dummyx,dummyy,dummyz,'*r');