%clear java;

%javaaddpath(fullfile(pwd, 'krpc-java-0.4.8.jar'));
%javaaddpath(fullfile(pwd, 'javatuples-1.2.jar'));
%javaaddpath(fullfile(pwd, 'protobuf-java-3.8.0.jar'));

%import krpc.client.*
import krpc.client.Connection
import krpc.client.RPCException
import krpc.client.services.KRPC
%import krpc.client.services.SpaceCenter
import krpc.client.services.SpaceCenter
import java.io.IOException

try
    connection = Connection.newInstance('Foobar');
    krpc = KRPC.newInstance(connection);
    disp(krpc.getStatus().getVersion());

    krpc_sc = SpaceCenter.newInstance(connection);
    vessel = krpc_sc.getActiveVessel();
    disp(vessel.getName());

    orbit = vessel.getOrbit();
    body = orbit.getBody();
    refFrame = body.getReferenceFrame();
    surfRefFrame = vessel.getSurfaceReferenceFrame();
    FlightFrame = vessel.flight(surfRefFrame);

    VesselPos = vessel.position(refFrame);
    VesselVel = vessel.velocity(refFrame);
    VesselDir = vessel.direction(refFrame);
    CtrlIntf = vessel.getControl();

    pos_i = VesselPos.getValue0(); pos_j = VesselPos.getValue1(); pos_k = VesselPos.getValue2();
    vel_i = VesselVel.getValue0(); vel_j = VesselVel.getValue1(); vel_k = VesselVel.getValue2();
    dir_i = VesselDir.getValue0(); dir_j = VesselDir.getValue1(); dir_k = VesselDir.getValue2();

    tic
    pause('on');

    IntError = 0;
    PrevT = toc;
    PrevError = 0;

    while 0 == 0,
        FAlt = FlightFrame.getMeanAltitude();
        NewT = toc;

        DeltaT = NewT - PrevT;

        TargetVal = 300 + heaviside(NewT - 60) .* (NewT - 60) .* -1;

        Error = TargetVal - FAlt;
        IntError = IntError + Error .* DeltaT;
        DiffError = (Error - PrevError) ./ DeltaT;
        Throttle = 0.1 * ( Error + 0.0001 .* IntError + 3.5 .* DiffError);
        PrevError = Error; PrevT = NewT;

        Throttle = max(min(Throttle, 1.0), 0.0);
        CtrlIntf.setThrottle(Throttle);

        pause(0.010);
    end;

catch ME
    disp(ME);
end;