diff --git a/code/Plotty.h b/code/Plotty.h
index 6f1c2ad..06a5d5a 100644
--- a/code/Plotty.h
+++ b/code/Plotty.h
@@ -356,6 +356,11 @@ public:
 		splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
 	}
 
+    void addLabel(const std::string& txt, const Point3 pos, int tag) {
+        //gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n";
+        splot.getCustom() << "set label " << tag << " '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
+    }
+
     void setActivity(const int act) {
 
         std::string activity = "Unkown";
diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp
index 49eb815..43dc0f9 100644
--- a/code/mainTrilat.cpp
+++ b/code/mainTrilat.cpp
@@ -78,6 +78,15 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     }
     plot.plot();
 
+
+    // Output dir
+    auto outputDir = std::filesystem::path(Settings::outputDir);
+    outputDir.append(Settings::CurrentPath.name + "_" + std::to_string(walkIdx));
+
+    if (!std::filesystem::exists(outputDir)) {
+        std::filesystem::create_directories(outputDir);
+    }
+
     std::vector<WiFiMeasurement> obs;
 
     Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -141,12 +150,14 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
             if (apPosDistMap.size() > 3)
             {
                 // Do update for real
+                std::vector<MACAddress> macs;
                 std::vector<Point2> apPositions;
                 std::vector<float> ftmDists;
                 std::vector<float> rssiDists;
 
                 for (const auto& kvp : apPosDistMap)
                 {
+                    macs.push_back(kvp.first);
                     apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
                     ftmDists.push_back(kvp.second.first);
                     rssiDists.push_back(kvp.second.second);
@@ -182,14 +193,41 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
                 if (Settings::PlotCircles)
                 {
                     plot.clearDistanceCircles();
+                    plot.splot.getCustom().str("");
+
+                    for (size_t i = 0; i < 20; i++)
+                    {
+                        plot.splot.getCustom() << "unset label " << i << "\n";
+                    }
 
                     for (size_t i = 0; i < ftmDists.size(); i++)
                     {
                         plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
                         plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
+                    
+                        // Distance labels
+                        std::stringstream ss;
+                        ss << setup.nuc(macs[i]).ID << ": " << ftmDists[i] << "m";
+    
+                        plot.addLabel(ss.str(), Point3(70, i*5, 0), i);
                     }
                 }
 
+               
+
+                // Png Output
+                //if (Settings::PlotToPng)
+                //{
+                //    plot.gp.setTerminal("png", K::GnuplotSize(1280, 720));
+                //    auto pngPath = outputDir / "png" / "trilat" / "frame.png";
+
+                //    // clear folder
+                //    //std::filesystem::remove_all(pngPath);
+                //    forceDirectories(pngPath.parent_path());
+                //    //std::filesystem::create_directory(pngPath);
+
+                //    plot.gp.setOutput(appendFileSuffixToPath(pngPath, ts.ms()).string());
+                //}
 
                 plot.plot();
                 std::this_thread::sleep_for(std::chrono::milliseconds(100));