From b30154a62d9fe88f425b6837d9112056e89693bf Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Thu, 14 Oct 2021 18:14:13 +0000
Subject: [PATCH] Resolve "More verbose simulation results"

---
 .vscode/extensions.json |  3 ++-
 include/sim.h           | 37 ++++++++++++++++++++++++++-----------
 src/main.cpp            | 16 ++++------------
 3 files changed, 32 insertions(+), 24 deletions(-)

diff --git a/.vscode/extensions.json b/.vscode/extensions.json
index 32f52b8..986e8c3 100644
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -1,6 +1,7 @@
 {
     "recommendations": [
         "ms-vscode.cpptools",
-        "wayou.vscode-todo-highlight"
+        "wayou.vscode-todo-highlight",
+        "usernamehw.errorlens"
     ]
 }
\ No newline at end of file
diff --git a/include/sim.h b/include/sim.h
index 55add14..579528e 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -1,6 +1,8 @@
 #include "Vehicle.h"
 #include "outVector.h"
 
+#include <iostream>
+
 void burnStartTimeCalc(struct Vehicle &);
 void thrustSelection(struct Vehicle &, int t);
 void lqrCalc(struct Vehicle &);
@@ -38,19 +40,33 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
 
   write2CSV(stateVector, State);
 
-  bool returnValue;
+  bool pass = 1;
 
-  if (abs(State.vz) < 5) {
-    if ((abs(State.yaw) < 5) && (abs(State.pitch) < 5)) {
-      returnValue = 1;
-    } else {
-      returnValue = 0;
-    }
+  double landing_angle =
+      pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
+
+  double landing_velocity =
+      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
+
+  if (landing_angle < 5.0) {
+    std::cout << "   Landing Angle < 5°    | PASS | ";
   } else {
-    returnValue = 0;
+    std::cout << "   Landing Angle < 5°    | FAIL | ";
+    pass = pass * 0;
   }
+  std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
+            << std::endl;
 
-  return returnValue;
+  if (landing_velocity < 5.0) {
+    std::cout << "Landing Velocity < 5 m/s | PASS | ";
+  } else {
+    std::cout << "Landing Velocity < 5 m/s | FAIL | ";
+    pass = pass * 0;
+  }
+  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
+            << State.vz << "]" << std::endl;
+
+  return pass;
 }
 
 void burnStartTimeCalc(Vehicle &State) {
@@ -344,8 +360,6 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
              "pitchdot, rolldot, Servo1, Servo2, thrustFiring"
           << std::endl;
 
-  std::cout << "Writing to csv...\n";
-
   // writing to output file
   for (int t = 0; t < State.simTime; t++) {
     outfile << t << ", ";
@@ -377,6 +391,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
   }
 
   outfile.close();
+  std::cout << "simOut.csv created successfully.\n" << std::endl;
 }
 
 double derivative(double current, double previous, double step) {
diff --git a/src/main.cpp b/src/main.cpp
index 50c2e3e..c1efbf6 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -52,16 +52,8 @@ int main() {
 
   bool outcome = sim(State, PrevState);
 
-  std::cout << "Finished"
-            << "\n";
-
-  if (outcome == 1) {
-    std::cout << "Sim Result = Success!";
-    return 0;
-  } else if (outcome == 0) {
-    std::cout << "Sim Result = Failed!";
-    // return 1; Until I figure out how to make CI/CD continue even when run
-    // fails.
-    return 0;
-  }
+  std::cout << std::endl << "Simulation Complete 🚀" << std::endl;
+  //                                            ^^^
+  // 50% chance this makes Mattys linux crash
+  return 0;
 }
\ No newline at end of file