% ============================================================================
% Launcher.m
% Author: Brent Dingle, Ph.D.
% Creation Date: 2020
%
% Program to simulate simple rocket launch using a couple data structures
% and Eulerian integration. Challenge is to create a model rocket that
% can take-off and land - gently.
%
% This is just a beginning. It is mostly mathematical in modelling.
% There is little to no consideration for the mechanical components
% nor for the physical construction or shape of components.
% It also only models the vertical direction, and effectively is
% assuming a point mass in most instances.
% There is much work and enhancement to do from here.
% Hopefully this can provide a decent framework to start from.
% It should be sufficient to prove the possibility that a rocket can 
% launch and hover after freefall begins. However, building such could be quite
% a challenge.  =)
%
% VERSION NOTES:
% This version will perform an initial burn for a specified time.
% Once the rocket begins to descend from that burn a landing controller
% will be activiated.
% This controller will attempt to achieve a 0 velocity at a specified height.
% If all goes well the rocket will hover at that height until fuel is gone.
% This can simulate a landing if a height of near 0 is specified.
% The simulation will end when the rocket's height is zero.
%
% TODO: Adjust initial parameters
% ============================================================================

% ------------------------------------------------------------------
% - Initialize our rocket and simulation settings via structures
% ------------------------------------------------------------------
myRocket  = MakeSimpleRocket();
mySim     = InitSimParams();
myLanding = InitLandingController(mySim, myRocket);

% ------------------------------------------------------------------
% - For output we want telemetry values
% ------------------------------------------------------------------
telem.t       = [mySim.t];
telem.h       = [myRocket.height];
telem.v       = [myRocket.vel];
telem.a       = [myRocket.accel];
telem.thrust  = [myRocket.thrust];
telem.fuel    = [myRocket.fuelMass];

% ------------------------------------------------------------------
% Instead of running for a specified time (for t = 0 to endTime)
%     for mySim.t = 0.0 : mySim.dt : mySim.endTime
% we will run until myRocket hits the ground
% but first we must go up, and start to come down
% ------------------------------------------------------------------
while ((myRocket.vel >= 0) || (mySim.t < 2))   % allow 2 seconds for takeoff
  
  % thrust for take-off is likely assumed constant, but need not be 
  thrust = GetThrust(mySim);
  
  % update time
  mySim.t = mySim.t + mySim.dt;
  
  % update the rocket
  myRocket = UpdateRocket(mySim, myRocket, thrust);
    
  % update telemetry array
  telem.t       = [telem.t, mySim.t];
  telem.h       = [telem.h, myRocket.height];
  telem.v       = [telem.v, myRocket.vel];
  telem.a       = [telem.a, myRocket.accel];
  telem.thrust  = [telem.thrust, myRocket.thrust];
  telem.fuel    = [telem.fuel, myRocket.fuelMass];
  
endwhile

% ------------------------------------------------------------------
% We are now 'falling' continue until we hit the ground - softly?
% ------------------------------------------------------------------
while (myRocket.height > 0.0)
  guide.thrust = 0;
  guide.farCorrectionEnabled = false;
  %guide.isSlowing = false;
  
  % For this we make use of a controller to control descent/landing
  guide = GetThrustForDescent(mySim, myRocket, myLanding);
  thrust = guide.thrust;
  myLanding.farCorrectionEnabled = guide.farCorrectionEnabled;
  %myRocket.isSlowing = guide.isSlowing;
  %myRocket.slowStart = guide.slowStart;

  % update time
  mySim.t = mySim.t + mySim.dt;
  
  % update the rocket
  myRocket = UpdateRocket(mySim, myRocket, thrust);
  
  % update telemetry array
  telem.t       = [telem.t, mySim.t];
  telem.h       = [telem.h, myRocket.height];
  telem.v       = [telem.v, myRocket.vel];
  telem.a       = [telem.a, myRocket.accel];
  telem.thrust  = [telem.thrust, myRocket.thrust];
  telem.fuel    = [telem.fuel, myRocket.fuelMass];
  
endwhile

% ------------------------------------------------------------------
% Plot the results
% ------------------------------------------------------------------
PlotTelemetry(telem);

% ------------------------------------------------------------------
% Write out data files of telem
% Use: 
%   csvwrite(filename, x) or csvwrite(filename, x, opt1, ...)
%
% Alternate: 
%   dlmwrite(filename, x, ",", opt1, ...)
% ------------------------------------------------------------------
% Get a time date string to keep files unique for each run
theTime = localtime(time());
strTimestamp = strftime("%Y%m%d_%H%M%S", theTime);  %YYYYMMDD_HHMMSS
printf("Date and time is: %s\n", strTimestamp);

arrLen = length(telem.t);

disp("Writing data to disk...");

% -----------------------------------------------
% Create a metainformation file with 
% first line = [Number of Array elements]
% second line = [delta time]   <-- e.g. constant 0.001
% third line = [start time]    <-- expect 0.0
%
% This may make things easier to read into
% other programs easier later
% -----------------------------------------------
%myFID = fopen("LH_meta.txt", "wt"); 
curName = strcat("LH_meta_", strTimestamp, ".txt");
printf("... outputting file: [%s]\n", curName);
myFID = fopen(curName, "wt"); 
  fprintf(myFID, "%d\n", arrLen);
  fprintf(myFID, "%f\n", mySim.dt);
  fprintf(myFID, "0.0\n");
fclose(myFID);

% -----------------------------------------------
% output Height file
% -----------------------------------------------
%csvwrite("LH_height.txt", telem.h);
curName = strcat("LH_height_", strTimestamp, ".txt");
printf("... outputting file: [%s]\n", curName);
csvwrite(curName, telem.h);

% -----------------------------------------------
% output Thrust file
% -----------------------------------------------
%csvwrite("LH_thrust.txt", telem.thrust);
curName = strcat("LH_thrust_", strTimestamp, ".txt");
printf("... outputting file: [%s]\n", curName);
csvwrite(curName, telem.thrust);

% -----------------------------------------------
% output FuelMass file
% -----------------------------------------------
%csvwrite("LH_fuel.txt", telem.fuel);
curName = strcat("LH_fuel_", strTimestamp, ".txt");
printf("... outputting file: [%s]\n", curName);
csvwrite(curName, telem.fuel);

% -----------------------------------------------
% output combo file
% each timestep has its own line of data:
%    time, height, thrust, fuel   
% -----------------------------------------------
curName = strcat("LH_combo_", strTimestamp, ".txt");
printf("... outputting file: [%s]\n", curName);
myFID = fopen(curName, "wt");
for i = 1:arrLen
  fprintf(myFID, "%f, %f, %f, %f\n", telem.t(i), telem.h(i), telem.thrust(i), telem.fuel(i) );
endfor

fclose(myFID);


disp("... write to disk complete.");

% Output a completion message
disp("Launch and Hover completed.");