How to reach a target apoapsis with a gravity turn?

Question

Ok, here I go.

I have as a project to simulate the launch of a payload by a rocket into polar orbit. Since original question was too big, let's get it into separate parts. First of all, trajectory and movement.

Considering the only experience we ever got with orbital mechanics and rocket launches was KSP, I am going to base my program on it.

So, as far as I understand, to launch something into a circular orbit, you need to perform a gravity turn. KSP seems to be doing it by increasing the pitch angle over time and when your periapsis is at the desired height, your pitch angle should change to 90 degrees(or 0-perpendicular to vertical anyways) and thrust in order to get the required speed. From this I tried to come up with a rough algorithm in matlab which plots the trajectory up to 600km, where it reaches with an x-speed(horizontal) of 7.8km/s(which seems fit for polar orbit) and -1.2m/s verical speed based on trial and error method. But I believe I can make this more efficient if I implement the KSP method, aka calculate the periapsis at each step until it reaches the desired height, then shut down the engine, change the pitch angle to horizontal and thrust up in order to get the desired speed. But I have no ideea how to do this.

clear; clc;
format longG
figure;
stage= [11600  4424     1400   275;
        1650   695      300     37;
        564.25 258.92   93.09   22.27;  
        47     37        36   28];
    stage(3,:)=stage(3,:).*(10^3)



%  Rocket parameters based on scout I row 1 gross mass; row 2 dry mass; row 3 engine thrust; row 4 burn time
%  Isp=[280 22];
x=0;
z=0;
vx=0;
vz=0;
theta=90;t=0;deltat=0.25;
 g=9.80665 ;
dth=-1.2;
 K=(6.67428+0.00067)*10^(-11);
% Vertical Take off
while vz<=100 && z>=0
    v=sqrt(vx^2+vz^2);
    if(t<stage(4,1))
        T=stage(3,1)
        m=sum(stage(1,:))-(stage(1,1)-stage(2,1))/stage(4,1)*t
    end
    ax=1/m*T*cosd(theta);
    G=m*g
    az=1/m*(T*sind(theta)-G)
    vx=vx+ax*deltat
    vz=vz+az*deltat
    x=x+vx*deltat;
    z=z+vz*deltat;
    theta=atan2d(vz,vx);
    t=t+deltat;
    plot(x,z,'r*'); hold on;
%    pause(0.1)
end
% Establishing trajectory
;flight=0;

vz
for i=1:3
while t<stage(4,i);
    v=sqrt(vx^2+vz^2);
        T=stage(3,i)
        m=sum(stage(1,i:end))-(stage(1,i)-stage(2,i))/stage(4,i)*t
% r=distance(x,z)
g=gravitacc(x,z)
G=m*g
    ax=1/m*T*cosd(theta);
    G=m*g
    az=1/m*(T*sind(theta)-G);
    vx=vx+ax*deltat
    vz=vz+az*deltat
    x=x+vx*deltat;
    z=z+vz*deltat;
%     theta=atan2d(vz,vx)+dth*deltat;
    if theta>=15
        theta=theta+dth*deltat;   
    end
        t=t+deltat
    plot(x,z,'r*'); hold on;
%    pause(0.1)
end
plot(x,z,'bd');
text(x,z,num2str(i));
flight=flight+t
t=0;
end
t=0;
dth=-0.5
i=4
theta
while t<=600 & vz>0 & vx<=7800 & t<stage(4,i)
     v=sqrt(vx^2+vz^2);
        T=stage(3,i)/2.5;
        m=sum(stage(1,i:end))-(stage(1,i)-stage(2,i))/stage(4,i)*t;
% r=distance(x,z)
g=gravitacc(x,z);

    ax=1/m*T*cosd(theta);
    G=m*g;
    az=1/m*(T*sind(theta)-G);
    vx=vx+ax*deltat;
    vz=vz+az*deltat;
    x=x+vx*deltat;
    z=z+vz*deltat;
   theta=atan2d(vz,vx);
 if theta>=0  theta=theta+dth*deltat;
 end
     t=t+deltat;

    plot(x,z,'r*'); hold on;
% text(x,z,num2str(vx));   
% pause(0.1);
end
% theta
% m
while t<=600 & vz>0 
     v=sqrt(vx^2+vz^2);
        T=0;
%         m=sum(stage(1,i:end))-(stage(1,i)-stage(2,i))/stage(4,i)*t;
% r=distance(x,z)
g=gravitacc(x,z);

    ax=1/m*T*cosd(theta);
    G=m*g;
    az=1/m*(T*sind(theta)-G);
    vx=vx+ax*deltat;
    vz=vz+az*deltat;
    x=x+vx*deltat;
    z=z+vz*deltat;
    theta=atan2d(vz,vx);
    t=t+deltat;

    plot(x,z,'r*'); hold on;
% text(x,z,num2str(vx));   
% pause(0.1);   
end
% m
% theta
% fligh=flight+t
z
vx
vz