diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index e9647443a9bbf0ea429fa31c8d0be004c7a436b6..f8ca4994c50192342e40715f62d50805b629ba14 100755
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,3 +1,3 @@
-idf_component_register(SRCS "ftm_main.c"
+idf_component_register(SRCS "math_3d.c" "ftm.c" "commands.c" "ftm_main.c"
                        INCLUDE_DIRS ".")
 target_compile_options(${COMPONENT_LIB} PRIVATE "-Wno-format")
diff --git a/main/commands.c b/main/commands.c
new file mode 100644
index 0000000000000000000000000000000000000000..659ce4b8cdd992040c30eb9f0983c9c77bd6ad9e
--- /dev/null
+++ b/main/commands.c
@@ -0,0 +1,369 @@
+#include "commands.h"
+
+#include <stdio.h>
+
+
+wifi_args_t sta_args;
+wifi_args_t sta_args;
+wifi_args_t ap_args;
+wifi_scan_arg_t scan_args;
+wifi_ftm_args_t ftm_args;
+wifi_calibrate_args_t calibrate_args;
+
+int wifi_cmd_sta(int argc, char **argv) {
+  int nerrors = arg_parse(argc, argv, (void **)&sta_args);
+
+  if (nerrors != 0) {
+    arg_print_errors(stderr, sta_args.end, argv[0]);
+    return 1;
+  }
+
+  ESP_LOGI(TAG_STA, "sta connecting to '%s'", sta_args.ssid->sval[0]);
+  wifi_cmd_sta_join(sta_args.ssid->sval[0], sta_args.password->sval[0]);
+  return 0;
+}
+
+bool wifi_cmd_sta_join(const char *ssid, const char *pass) {
+  int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+
+  wifi_config_t wifi_config = {0};
+
+  strlcpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
+  if (pass) {
+    strlcpy((char *)wifi_config.sta.password, pass,
+            sizeof(wifi_config.sta.password));
+  }
+
+  if (bits & CONNECTED_BIT) {
+    s_reconnect = false;
+    xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
+    ESP_ERROR_CHECK(esp_wifi_disconnect());
+    xEventGroupWaitBits(s_wifi_event_group, DISCONNECTED_BIT, 0, 1,
+                        portTICK_PERIOD_MS);
+  }
+
+  s_reconnect = true;
+  s_retry_num = 0;
+  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+  ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
+  ESP_ERROR_CHECK(esp_wifi_connect());
+
+  xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1,
+                      5000 / portTICK_PERIOD_MS);
+
+  return true;
+}
+
+int wifi_cmd_scan(int argc, char **argv) {
+  int nerrors = arg_parse(argc, argv, (void **)&scan_args);
+
+  if (nerrors != 0) {
+    arg_print_errors(stderr, scan_args.end, argv[0]);
+    return 1;
+  }
+
+  ESP_LOGI(TAG_STA, "sta start to scan");
+  if (scan_args.ssid->count == 1) {
+    wifi_perform_scan(scan_args.ssid->sval[0], false);
+  } else {
+    wifi_perform_scan(NULL, false);
+  }
+  return 0;
+}
+
+bool wifi_cmd_ap_set(const char *ssid, const char *pass) {
+  s_reconnect = false;
+  strlcpy((char *)g_ap_config.ap.ssid, ssid, MAX_SSID_LEN);
+  if (pass) {
+    if (strlen(pass) != 0 && strlen(pass) < 8) {
+      s_reconnect = true;
+      ESP_LOGE(TAG_AP, "password cannot be less than 8 characters long");
+      return false;
+    }
+    strlcpy((char *)g_ap_config.ap.password, pass, MAX_PASSPHRASE_LEN);
+  }
+
+  if (strlen(pass) == 0) {
+    g_ap_config.ap.authmode = WIFI_AUTH_OPEN;
+  }
+
+  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
+  ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+  return true;
+}
+
+int wifi_cmd_ap(int argc, char **argv) {
+  int nerrors = arg_parse(argc, argv, (void **)&ap_args);
+
+  if (nerrors != 0) {
+    arg_print_errors(stderr, ap_args.end, argv[0]);
+    return 1;
+  }
+
+  if (true == wifi_cmd_ap_set(ap_args.ssid->sval[0], ap_args.password->sval[0]))
+    ESP_LOGI(
+        TAG_AP,
+        "Starting SoftAP with FTM Responder support, SSID - %s, Password - %s",
+        ap_args.ssid->sval[0], ap_args.password->sval[0]);
+  else
+    ESP_LOGE(TAG_AP, "Failed to start SoftAP!");
+
+  return 0;
+}
+
+int wifi_cmd_query(int argc, char **argv) {
+  wifi_config_t cfg;
+  wifi_mode_t mode;
+
+  esp_wifi_get_mode(&mode);
+  if (WIFI_MODE_AP == mode) {
+    esp_wifi_get_config(WIFI_IF_AP, &cfg);
+    ESP_LOGI(TAG_AP, "AP mode, %s %s", cfg.ap.ssid, cfg.ap.password);
+  } else if (WIFI_MODE_STA == mode) {
+    int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+    if (bits & CONNECTED_BIT) {
+      esp_wifi_get_config(WIFI_IF_STA, &cfg);
+      ESP_LOGI(TAG_STA, "sta mode, connected %s", cfg.ap.ssid);
+    } else {
+      ESP_LOGI(TAG_STA, "sta mode, disconnected");
+    }
+  } else {
+    ESP_LOGI(TAG_STA, "NULL mode");
+    return 0;
+  }
+
+  return 0;
+}
+
+int wifi_cmd_list_offsets(int argc, char **argv) {
+  ESP_LOGI(TAG_STA, "-------- OFFSETS --------");
+  for (size_t i = 0; i < 4; i++) {
+    ESP_LOGI(TAG_STA, "AP_%d : %.2f", i, calibration_offsets[i]);
+  }
+
+  return 0;
+}
+
+int wifi_cmd_ftm(int argc, char **argv) {
+  int nerrors = arg_parse(argc, argv, (void **)&ftm_args);
+  wifi_ap_record_t *ap_record;
+  EventBits_t bits;
+
+  wifi_ftm_initiator_cfg_t ftmi_cfg = {
+      .frm_count = 32,
+      .burst_period = 2,
+  };
+
+  if (nerrors != 0) {
+    arg_print_errors(stderr, ftm_args.end, argv[0]);
+    return 0;
+  }
+
+  if (ftm_args.initiator->count != 0 && ftm_args.responder->count != 0) {
+    ESP_LOGE(TAG_STA, "Invalid FTM cmd argument");
+    return 0;
+  }
+
+  if (ftm_args.responder->count != 0)
+    goto ftm_responder;
+
+  bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+  if (bits & CONNECTED_BIT && !ftm_args.ssid->count) {
+    memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
+    ftmi_cfg.channel = s_ap_channel;
+  } else if (ftm_args.ssid->count == 1) {
+    ap_record = find_ftm_responder_ap(ftm_args.ssid->sval[0]);
+    if (ap_record) {
+      memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
+      ftmi_cfg.channel = ap_record->primary;
+    } else {
+      return 0;
+    }
+  } else {
+    ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
+    return 0;
+  }
+
+  if (ftm_args.frm_count->count != 0) {
+    uint8_t count = ftm_args.frm_count->ival[0];
+    if (count != 0 && count != 8 && count != 16 && count != 24 && count != 32 &&
+        count != 64) {
+      ESP_LOGE(TAG_STA,
+               "Invalid Frame Count! Valid options are 0/8/16/24/32/64");
+      return 0;
+    }
+    ftmi_cfg.frm_count = count;
+  }
+
+  if (ftm_args.burst_period->count != 0) {
+    if (ftm_args.burst_period->ival[0] >= 2 &&
+        ftm_args.burst_period->ival[0] < 256) {
+      ftmi_cfg.burst_period = ftm_args.burst_period->ival[0];
+    } else {
+      ESP_LOGE(TAG_STA, "Invalid Burst Period! Valid range is 2-255");
+      return 0;
+    }
+  }
+
+  ESP_LOGI(TAG_STA,
+           "Requesting FTM session with Frm Count - %d, Burst Period - %dmSec "
+           "(0: No Preference)",
+           ftmi_cfg.frm_count, ftmi_cfg.burst_period * 100);
+
+  if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
+    ESP_LOGE(TAG_STA, "Failed to start FTM session");
+    return 0;
+  }
+
+  bits =
+      xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
+                          pdTRUE, pdFALSE, portMAX_DELAY);
+  /* Processing data from FTM session */
+  if (bits & FTM_REPORT_BIT) {
+    ftm_process_report();
+    free(s_ftm_report);
+    s_ftm_report = NULL;
+    s_ftm_report_num_entries = 0;
+    ESP_LOGI(TAG_STA,
+             "Estimated RTT - %d nSec, Estimated Distance - %d.%02d meters",
+             s_rtt_est, s_dist_est / 100, s_dist_est % 100);
+  } else {
+    /* Failure case */
+  }
+
+  return 0;
+
+ftm_responder:
+  if (ftm_args.offset->count != 0) {
+    int16_t offset_cm = ftm_args.offset->ival[0];
+
+    esp_wifi_ftm_resp_set_offset(offset_cm);
+  }
+
+  if (ftm_args.enable->count != 0) {
+    if (!s_ap_started) {
+      ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
+      return 0;
+    }
+    g_ap_config.ap.ftm_responder = true;
+    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+    ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder enabled");
+
+    return 0;
+  }
+
+  if (ftm_args.disable->count != 0) {
+    if (!s_ap_started) {
+      ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
+      return 0;
+    }
+    g_ap_config.ap.ftm_responder = false;
+    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+    ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder disabled");
+  }
+
+  return 0;
+}
+
+/* Calibration Routine */
+int wifi_cmd_calibrate(int argc, char **argv) {
+  int nerrors = arg_parse(argc, argv, (void **)&calibrate_args);
+  wifi_ap_record_t *ap_record;
+  EventBits_t bits;
+
+  wifi_ftm_initiator_cfg_t ftmi_cfg = {
+      .frm_count = 8,
+      .burst_period = 2,
+  };
+
+  if (nerrors != 0) {
+    arg_print_errors(stderr, calibrate_args.end, argv[0]);
+    return 0;
+  }
+
+  bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+  if (bits & CONNECTED_BIT && !calibrate_args.ssid->count) {
+    memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
+    ftmi_cfg.channel = s_ap_channel;
+  } else if (calibrate_args.ssid->count == 1) {
+    ap_record = find_ftm_responder_ap(calibrate_args.ssid->sval[0]);
+    if (ap_record) {
+      memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
+      ftmi_cfg.channel = ap_record->primary;
+    } else {
+      return 0;
+    }
+  } else {
+    ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
+    return 0;
+  }
+
+  /**
+   * Check if actual distance is set
+   */
+
+  if (calibrate_args.actual_distance->count != 0) {
+    if (calibrate_args.actual_distance->dval[0] < 0) {
+      ESP_LOGE(TAG_STA, "Invalid distance, please enter a positive value");
+      return 0;
+    }
+  }
+
+  /**
+   * Check if IP id is correct
+   */
+
+  if (calibrate_args.ap_id->count != 0) {
+    uint8_t ap_id = calibrate_args.ap_id->ival[0];
+    if (ap_id != 0 && ap_id != 1 && ap_id != 2 && ap_id != 3) {
+      ESP_LOGE(TAG_STA, "Invalid AP id. Valid options are 0/1/2/3");
+      return 0;
+    }
+  } else {
+    ESP_LOGE(TAG_STA,
+             "Please provide an AP id to calibrate. Valid options are 0/1/2/3");
+    return 0;
+  }
+
+  ESP_LOGI(TAG_STA,
+           "Calibration Routine with %s\nRequesting FTM session with Frm Count "
+           "- %d, Burst Period - %dmSec (0: No Preference)",
+           calibrate_args.ssid->sval[0], ftmi_cfg.frm_count,
+           ftmi_cfg.burst_period * 100);
+
+  if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
+    ESP_LOGE(TAG_STA, "Failed to start FTM session");
+    return 0;
+  }
+
+  bits =
+      xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
+                          pdTRUE, pdFALSE, portMAX_DELAY);
+  /* Processing data from FTM session */
+  if (bits & FTM_REPORT_BIT) {
+    ftm_process_report();
+    free(s_ftm_report);
+    s_ftm_report = NULL;
+    s_ftm_report_num_entries = 0;
+
+    ESP_LOGI(TAG_STA, "-------- CALIBRATION --------");
+
+    calibration_offsets[calibrate_args.ap_id->ival[0]] =
+        (calibrate_args.actual_distance->dval[0] * 100 - s_dist_est) / 100;
+
+    ESP_LOGI(TAG_STA, "Calibrating AP_%d", calibrate_args.ap_id->ival[0]);
+    ESP_LOGI(TAG_STA, "Estimated RTT : %d nSec\n", s_rtt_est);
+
+    ESP_LOGI(TAG_STA, "Estimated distance (no calibration) : %d.%02d",
+             s_dist_est / 100, s_dist_est % 100);
+    ESP_LOGI(TAG_STA, "Actual distance : %.2f",
+             calibrate_args.actual_distance->dval[0]);
+    ESP_LOGI(TAG_STA, "Calibration offset : %.2f",
+             calibration_offsets[calibrate_args.ap_id->ival[0]]);
+
+  } else {
+    /* Failure case */
+  }
+
+  return 0;
+}
\ No newline at end of file
diff --git a/main/commands.h b/main/commands.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c535e67f10f93db4ce7f4cf9274967e8dd420b3
--- /dev/null
+++ b/main/commands.h
@@ -0,0 +1,84 @@
+#ifndef COMMANDS_INCLUDE
+#define COMMANDS_INCLUDE
+
+#include "argtable3/argtable3.h"
+#include "cmd_system.h"
+#include "esp_console.h"
+#include "esp_err.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "esp_mac.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "nvs_flash.h"
+
+#include "ftm.h"
+
+
+/*
+ * Arguments for Wifi related commands
+ */
+typedef struct {
+  struct arg_str *ssid;
+  struct arg_str *password;
+  struct arg_end *end;
+} wifi_args_t;
+
+
+/*
+ * Arguments for the Wifi scan command
+ */
+typedef struct {
+  struct arg_str *ssid;
+  struct arg_end *end;
+} wifi_scan_arg_t;
+
+
+typedef struct {
+  /* FTM Initiator */
+  struct arg_lit *initiator;
+  struct arg_int *frm_count;
+  struct arg_int *burst_period;
+  struct arg_str *ssid;
+  /* FTM Responder */
+  struct arg_lit *responder;
+  struct arg_lit *enable;
+  struct arg_lit *disable;
+  struct arg_int *offset;
+  struct arg_end *end;
+} wifi_ftm_args_t;
+
+typedef struct {
+  struct arg_dbl *actual_distance;
+  struct arg_str *ssid;
+  struct arg_int *ap_id;
+  struct arg_end *end;
+} wifi_calibrate_args_t;
+
+extern wifi_args_t sta_args;
+extern wifi_args_t sta_args;
+extern wifi_args_t ap_args;
+extern wifi_scan_arg_t scan_args;
+extern wifi_ftm_args_t ftm_args;
+extern wifi_calibrate_args_t calibrate_args;
+
+int wifi_cmd_sta(int argc, char **argv);
+
+bool wifi_cmd_sta_join(const char *ssid, const char *pass);
+
+int wifi_cmd_scan(int argc, char **argv);
+
+bool wifi_cmd_ap_set(const char *ssid, const char *pass);
+
+int wifi_cmd_ap(int argc, char **argv);
+
+int wifi_cmd_query(int argc, char **argv);
+
+int wifi_cmd_list_offsets(int argc, char **argv);
+
+int wifi_cmd_ftm(int argc, char **argv);
+
+int wifi_cmd_calibrate(int argc, char **argv);
+
+#endif //include guard
\ No newline at end of file
diff --git a/main/ftm.c b/main/ftm.c
new file mode 100644
index 0000000000000000000000000000000000000000..afe79da60d412c8e85001d7a6a4369705a57b8d6
--- /dev/null
+++ b/main/ftm.c
@@ -0,0 +1,169 @@
+#include "ftm.h"
+
+
+const char *TAG_STA = "ftm_station";
+const char *TAG_AP = "ftm_ap";
+
+wifi_config_t g_ap_config = {.ap.max_connection = 4,
+                             .ap.authmode = WIFI_AUTH_WPA2_PSK,
+                             .ap.ftm_responder = true};
+
+double calibration_offsets[4];
+
+bool s_reconnect = true;
+int s_retry_num = 0;
+
+EventGroupHandle_t s_wifi_event_group;
+const int CONNECTED_BIT = BIT0;
+const int DISCONNECTED_BIT = BIT1;
+
+EventGroupHandle_t s_ftm_event_group;
+const int FTM_REPORT_BIT = BIT0;
+int FTM_FAILURE_BIT = BIT1;
+wifi_ftm_report_entry_t *s_ftm_report;
+uint8_t s_ftm_report_num_entries;
+uint32_t s_rtt_est, s_dist_est;
+bool s_ap_started;
+uint8_t s_ap_channel;
+uint8_t s_ap_bssid[ETH_ALEN];
+
+const int g_report_lvl =
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_DIAG
+    BIT0 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RTT
+    BIT1 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_T1T2T3T4
+    BIT2 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RSSI
+    BIT3 |
+#endif
+    0;
+
+uint16_t g_scan_ap_num;
+wifi_ap_record_t *g_ap_list_buffer;
+
+void ftm_process_report(void) {
+  int i;
+  char *log = NULL;
+
+  if (!g_report_lvl)
+    return;
+
+  log = malloc(200);
+  if (!log) {
+    ESP_LOGE(TAG_STA, "Failed to alloc buffer for FTM report");
+    return;
+  }
+
+  bzero(log, 200);
+  sprintf(log, "%s%s%s%s", g_report_lvl & BIT0 ? " Diag |" : "",
+          g_report_lvl & BIT1 ? "   RTT   |" : "",
+          g_report_lvl & BIT2 ? "       T1       |       T2       |       T3   "
+                                "    |       T4       |"
+                              : "",
+          g_report_lvl & BIT3 ? "  RSSI  |" : "");
+  ESP_LOGI(TAG_STA, "FTM Report:");
+  ESP_LOGI(TAG_STA, "|%s", log);
+  for (i = 0; i < s_ftm_report_num_entries; i++) {
+    char *log_ptr = log;
+
+    bzero(log, 200);
+    if (g_report_lvl & BIT0) {
+      log_ptr += sprintf(log_ptr, "%6d|", s_ftm_report[i].dlog_token);
+    }
+    if (g_report_lvl & BIT1) {
+      log_ptr += sprintf(log_ptr, "%7u  |", s_ftm_report[i].rtt);
+    }
+    if (g_report_lvl & BIT2) {
+      log_ptr += sprintf(log_ptr, "%14llu  |%14llu  |%14llu  |%14llu  |",
+                         s_ftm_report[i].t1, s_ftm_report[i].t2,
+                         s_ftm_report[i].t3, s_ftm_report[i].t4);
+    }
+    if (g_report_lvl & BIT3) {
+      log_ptr += sprintf(log_ptr, "%6d  |", s_ftm_report[i].rssi);
+    }
+    ESP_LOGI(TAG_STA, "|%s", log);
+  }
+  free(log);
+}
+
+bool wifi_perform_scan(const char *ssid, bool internal) {
+  wifi_scan_config_t scan_config = {0};
+  scan_config.ssid = (uint8_t *)ssid;
+  uint8_t i;
+
+  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+  if (ESP_OK != esp_wifi_scan_start(&scan_config, true)) {
+    ESP_LOGI(TAG_STA, "Failed to perform scan");
+    return false;
+  }
+
+  esp_wifi_scan_get_ap_num(&g_scan_ap_num);
+  if (g_scan_ap_num == 0) {
+    ESP_LOGI(TAG_STA, "No matching AP found");
+    return false;
+  }
+
+  if (g_ap_list_buffer) {
+    free(g_ap_list_buffer);
+  }
+  g_ap_list_buffer = malloc(g_scan_ap_num * sizeof(wifi_ap_record_t));
+  if (g_ap_list_buffer == NULL) {
+    ESP_LOGE(TAG_STA, "Failed to malloc buffer to print scan results");
+    return false;
+  }
+
+  if (esp_wifi_scan_get_ap_records(
+          &g_scan_ap_num, (wifi_ap_record_t *)g_ap_list_buffer) == ESP_OK) {
+    if (!internal) {
+      for (i = 0; i < g_scan_ap_num; i++) {
+        ESP_LOGI(TAG_STA,
+                 "[%s][rssi=%d]"
+                 "%s",
+                 g_ap_list_buffer[i].ssid, g_ap_list_buffer[i].rssi,
+                 g_ap_list_buffer[i].ftm_responder ? "[FTM Responder]" : "");
+      }
+    }
+  }
+
+  ESP_LOGI(TAG_STA, "sta scan done");
+
+  return true;
+}
+
+wifi_ap_record_t *find_ftm_responder_ap(const char *ssid) {
+  bool retry_scan = false;
+  uint8_t i;
+
+  if (!ssid)
+    return NULL;
+
+retry:
+  if (!g_ap_list_buffer || (g_scan_ap_num == 0)) {
+    ESP_LOGI(TAG_STA, "Scanning for %s", ssid);
+    if (false == wifi_perform_scan(ssid, true)) {
+      return NULL;
+    }
+  }
+
+  for (i = 0; i < g_scan_ap_num; i++) {
+    if (strcmp((const char *)g_ap_list_buffer[i].ssid, ssid) == 0)
+      return &g_ap_list_buffer[i];
+  }
+
+  if (!retry_scan) {
+    retry_scan = true;
+    if (g_ap_list_buffer) {
+      free(g_ap_list_buffer);
+      g_ap_list_buffer = NULL;
+    }
+    goto retry;
+  }
+
+  ESP_LOGI(TAG_STA, "No matching AP found");
+
+  return NULL;
+}
\ No newline at end of file
diff --git a/main/ftm.h b/main/ftm.h
new file mode 100644
index 0000000000000000000000000000000000000000..58de7e0e3ed77ed9f208d1adcd031fa96b05781e
--- /dev/null
+++ b/main/ftm.h
@@ -0,0 +1,56 @@
+#ifndef FTM_INCLUDE
+#define FTM_INCLUDE
+
+#include "argtable3/argtable3.h"
+#include "cmd_system.h"
+#include "esp_console.h"
+#include "esp_err.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "esp_mac.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "nvs_flash.h"
+#include <string.h>
+
+
+extern const char *TAG_STA;
+extern const char *TAG_AP;
+
+// Calibration offsets array
+extern double calibration_offsets[4];
+
+extern wifi_config_t g_ap_config;
+
+#define ETH_ALEN 6
+#define MAX_CONNECT_RETRY_ATTEMPTS 5
+
+extern bool s_reconnect;
+extern int s_retry_num;
+
+extern EventGroupHandle_t s_wifi_event_group;
+extern const int CONNECTED_BIT;
+extern const int DISCONNECTED_BIT;
+extern EventGroupHandle_t s_ftm_event_group;
+extern const int FTM_REPORT_BIT;
+extern int FTM_FAILURE_BIT;
+extern wifi_ftm_report_entry_t *s_ftm_report;
+extern uint8_t s_ftm_report_num_entries;
+extern uint32_t s_rtt_est, s_dist_est;
+extern bool s_ap_started;
+extern uint8_t s_ap_channel;
+extern uint8_t s_ap_bssid[ETH_ALEN];
+
+extern const int g_report_lvl;
+
+extern uint16_t g_scan_ap_num;
+extern wifi_ap_record_t *g_ap_list_buffer;
+
+bool wifi_perform_scan(const char *ssid, bool internal);
+
+wifi_ap_record_t *find_ftm_responder_ap(const char *ssid);
+
+void ftm_process_report(void);
+
+#endif // include guard
\ No newline at end of file
diff --git a/main/ftm_main.c b/main/ftm_main.c
index cb913b522999e09afaa7669904fa25e587becde1..cbdec16b4080eebb472abc5ba633cb362fd926cd 100755
--- a/main/ftm_main.c
+++ b/main/ftm_main.c
@@ -23,159 +23,9 @@
 #include <stdio.h>
 #include <string.h>
 
-typedef struct {
-  struct arg_str *ssid;
-  struct arg_str *password;
-  struct arg_end *end;
-} wifi_args_t;
-
-typedef struct {
-  struct arg_str *ssid;
-  struct arg_end *end;
-} wifi_scan_arg_t;
-
-typedef struct {
-  /* FTM Initiator */
-  struct arg_lit *initiator;
-  struct arg_int *frm_count;
-  struct arg_int *burst_period;
-  struct arg_str *ssid;
-  /* FTM Responder */
-  struct arg_lit *responder;
-  struct arg_lit *enable;
-  struct arg_lit *disable;
-  struct arg_int *offset;
-  struct arg_end *end;
-} wifi_ftm_args_t;
-
-typedef struct {
-  struct arg_dbl *actual_distance;
-  struct arg_str *ssid;
-  struct arg_int *ap_id;
-  struct arg_end *end;
-} wifi_calibrate_args_t;
-
-// Point structure
-typedef struct {
-  double x;
-  double y;
-  double z;
-} point_t;
-
-static wifi_args_t sta_args;
-static wifi_args_t ap_args;
-static wifi_scan_arg_t scan_args;
-static wifi_ftm_args_t ftm_args;
-static wifi_calibrate_args_t calibrate_args;
-
-// Calibration offsets array
-double calibration_offsets[4];
-
-wifi_config_t g_ap_config = {.ap.max_connection = 4,
-                             .ap.authmode = WIFI_AUTH_WPA2_PSK,
-                             .ap.ftm_responder = true};
-
-#define ETH_ALEN 6
-#define MAX_CONNECT_RETRY_ATTEMPTS 5
-
-static bool s_reconnect = true;
-static int s_retry_num = 0;
-static const char *TAG_STA = "ftm_station";
-static const char *TAG_AP = "ftm_ap";
-
-static EventGroupHandle_t s_wifi_event_group;
-static const int CONNECTED_BIT = BIT0;
-static const int DISCONNECTED_BIT = BIT1;
-
-static EventGroupHandle_t s_ftm_event_group;
-static const int FTM_REPORT_BIT = BIT0;
-static const int FTM_FAILURE_BIT = BIT1;
-static wifi_ftm_report_entry_t *s_ftm_report;
-static uint8_t s_ftm_report_num_entries;
-static uint32_t s_rtt_est, s_dist_est;
-static bool s_ap_started;
-static uint8_t s_ap_channel;
-static uint8_t s_ap_bssid[ETH_ALEN];
-
-const int g_report_lvl =
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_DIAG
-    BIT0 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RTT
-    BIT1 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_T1T2T3T4
-    BIT2 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RSSI
-    BIT3 |
-#endif
-    0;
-
-uint16_t g_scan_ap_num;
-wifi_ap_record_t *g_ap_list_buffer;
-
-// Distance function declaration
-
-double distance(point_t a, point_t b) {
-  double dx = a.x - b.x;
-  double dy = a.y - b.y;
-  double dz = a.z - b.z;
-  return sqrt(dx * dx + dy * dy + dz * dz);
-}
-
-// Takes the known positions of the points as arguments and the distances from
-// the unknown point to the known points as arguments
-
-point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4, double d1,
-                    double d2, double d3, double d4) {
-  double x1 = p1.x, y1 = p1.y, z1 = p1.z;
-  double x2 = p2.x, y2 = p2.y, z2 = p2.z;
-  double x3 = p3.x, y3 = p3.y, z3 = p3.z;
-  double x4 = p4.x, y4 = p4.y, z4 = p4.z;
-
-  double A1 = 2 * (x2 - x1);
-  double B1 = 2 * (y2 - y1);
-  double C1 = 2 * (z2 - z1);
-  double D1 = d1 * d1 - d2 * d2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2 -
-              z1 * z1 + z2 * z2;
-
-  double A2 = 2 * (x3 - x1);
-  double B2 = 2 * (y3 - y1);
-  double C2 = 2 * (z3 - z1);
-  double D2 = d1 * d1 - d3 * d3 - x1 * x1 + x3 * x3 - y1 * y1 + y3 * y3 -
-              z1 * z1 + z3 * z3;
-
-  double A3 = 2 * (x4 - x1);
-  double B3 = 2 * (y4 - y1);
-  double C3 = 2 * (z4 - z1);
-  double D3 = d1 * d1 - d4 * d4 - x1 * x1 + x4 * x4 - y1 * y1 + y4 * y4 -
-              z1 * z1 + z4 * z4;
-
-  double D4 = d1 * d1 - d1 * d1 - x1 * x1 + x1 * x1 - y1 * y1 + y1 * y1 -
-              z1 * z1 + z1 * z1;
-
-  double a = A2 / A1;
-  double b = B2 / B1;
-  double c = C2 / C1;
-  double d = D2 / A1;
-  double e = D3 / A1 - a * D1 / A1;
-  double f = D3 / C1 - c * D1 / C1;
-  double g = B3 / B1 - b;
-  double h = D3 / B1 - d * B3 / B1;
-  double i = D2 / B1 - a * D1 / B1;
-  double j = D4 / C1 - c * D1 / C1;
-  double k = D4 / B1 - d * B3 / B1;
-  double l = i - a * g;
-
-  double m = j - c * g;
-  double n = (k - b * m / l) / (e - a * m / l);
-  double o = (i - j * n) / l;
-  double p = d - a * n - c * o;
-
-  point_t result = {n, o, p};
-  return result;
-}
+#include "math_3d.h"
+#include "commands.h"
+#include "ftm.h"
 
 static void event_handler(void *arg, esp_event_base_t event_base,
                           int32_t event_id, void *event_data) {
@@ -221,51 +71,6 @@ static void event_handler(void *arg, esp_event_base_t event_base,
   }
 }
 
-static void ftm_process_report(void) {
-  int i;
-  char *log = NULL;
-
-  if (!g_report_lvl)
-    return;
-
-  log = malloc(200);
-  if (!log) {
-    ESP_LOGE(TAG_STA, "Failed to alloc buffer for FTM report");
-    return;
-  }
-
-  bzero(log, 200);
-  sprintf(log, "%s%s%s%s", g_report_lvl & BIT0 ? " Diag |" : "",
-          g_report_lvl & BIT1 ? "   RTT   |" : "",
-          g_report_lvl & BIT2 ? "       T1       |       T2       |       T3   "
-                                "    |       T4       |"
-                              : "",
-          g_report_lvl & BIT3 ? "  RSSI  |" : "");
-  ESP_LOGI(TAG_STA, "FTM Report:");
-  ESP_LOGI(TAG_STA, "|%s", log);
-  for (i = 0; i < s_ftm_report_num_entries; i++) {
-    char *log_ptr = log;
-
-    bzero(log, 200);
-    if (g_report_lvl & BIT0) {
-      log_ptr += sprintf(log_ptr, "%6d|", s_ftm_report[i].dlog_token);
-    }
-    if (g_report_lvl & BIT1) {
-      log_ptr += sprintf(log_ptr, "%7u  |", s_ftm_report[i].rtt);
-    }
-    if (g_report_lvl & BIT2) {
-      log_ptr += sprintf(log_ptr, "%14llu  |%14llu  |%14llu  |%14llu  |",
-                         s_ftm_report[i].t1, s_ftm_report[i].t2,
-                         s_ftm_report[i].t3, s_ftm_report[i].t4);
-    }
-    if (g_report_lvl & BIT3) {
-      log_ptr += sprintf(log_ptr, "%6d  |", s_ftm_report[i].rssi);
-    }
-    ESP_LOGI(TAG_STA, "|%s", log);
-  }
-  free(log);
-}
-
 void initialise_wifi(void) {
   esp_log_level_set("wifi", ESP_LOG_WARN);
   static bool initialized = false;
@@ -291,442 +96,9 @@ void initialise_wifi(void) {
   initialized = true;
 }
 
-static bool wifi_cmd_sta_join(const char *ssid, const char *pass) {
-  int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
-
-  wifi_config_t wifi_config = {0};
-
-  strlcpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
-  if (pass) {
-    strlcpy((char *)wifi_config.sta.password, pass,
-            sizeof(wifi_config.sta.password));
-  }
-
-  if (bits & CONNECTED_BIT) {
-    s_reconnect = false;
-    xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
-    ESP_ERROR_CHECK(esp_wifi_disconnect());
-    xEventGroupWaitBits(s_wifi_event_group, DISCONNECTED_BIT, 0, 1,
-                        portTICK_PERIOD_MS);
-  }
-
-  s_reconnect = true;
-  s_retry_num = 0;
-  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
-  ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
-  ESP_ERROR_CHECK(esp_wifi_connect());
-
-  xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1,
-                      5000 / portTICK_PERIOD_MS);
-
-  return true;
-}
-
-static int wifi_cmd_sta(int argc, char **argv) {
-  int nerrors = arg_parse(argc, argv, (void **)&sta_args);
-
-  if (nerrors != 0) {
-    arg_print_errors(stderr, sta_args.end, argv[0]);
-    return 1;
-  }
-
-  ESP_LOGI(TAG_STA, "sta connecting to '%s'", sta_args.ssid->sval[0]);
-  wifi_cmd_sta_join(sta_args.ssid->sval[0], sta_args.password->sval[0]);
-  return 0;
-}
-
-static bool wifi_perform_scan(const char *ssid, bool internal) {
-  wifi_scan_config_t scan_config = {0};
-  scan_config.ssid = (uint8_t *)ssid;
-  uint8_t i;
-
-  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
-  if (ESP_OK != esp_wifi_scan_start(&scan_config, true)) {
-    ESP_LOGI(TAG_STA, "Failed to perform scan");
-    return false;
-  }
-
-  esp_wifi_scan_get_ap_num(&g_scan_ap_num);
-  if (g_scan_ap_num == 0) {
-    ESP_LOGI(TAG_STA, "No matching AP found");
-    return false;
-  }
-
-  if (g_ap_list_buffer) {
-    free(g_ap_list_buffer);
-  }
-  g_ap_list_buffer = malloc(g_scan_ap_num * sizeof(wifi_ap_record_t));
-  if (g_ap_list_buffer == NULL) {
-    ESP_LOGE(TAG_STA, "Failed to malloc buffer to print scan results");
-    return false;
-  }
-
-  if (esp_wifi_scan_get_ap_records(
-          &g_scan_ap_num, (wifi_ap_record_t *)g_ap_list_buffer) == ESP_OK) {
-    if (!internal) {
-      for (i = 0; i < g_scan_ap_num; i++) {
-        ESP_LOGI(TAG_STA,
-                 "[%s][rssi=%d]"
-                 "%s",
-                 g_ap_list_buffer[i].ssid, g_ap_list_buffer[i].rssi,
-                 g_ap_list_buffer[i].ftm_responder ? "[FTM Responder]" : "");
-      }
-    }
-  }
-
-  ESP_LOGI(TAG_STA, "sta scan done");
-
-  return true;
-}
-
-static int wifi_cmd_scan(int argc, char **argv) {
-  int nerrors = arg_parse(argc, argv, (void **)&scan_args);
-
-  if (nerrors != 0) {
-    arg_print_errors(stderr, scan_args.end, argv[0]);
-    return 1;
-  }
-
-  ESP_LOGI(TAG_STA, "sta start to scan");
-  if (scan_args.ssid->count == 1) {
-    wifi_perform_scan(scan_args.ssid->sval[0], false);
-  } else {
-    wifi_perform_scan(NULL, false);
-  }
-  return 0;
-}
-
-static bool wifi_cmd_ap_set(const char *ssid, const char *pass) {
-  s_reconnect = false;
-  strlcpy((char *)g_ap_config.ap.ssid, ssid, MAX_SSID_LEN);
-  if (pass) {
-    if (strlen(pass) != 0 && strlen(pass) < 8) {
-      s_reconnect = true;
-      ESP_LOGE(TAG_AP, "password cannot be less than 8 characters long");
-      return false;
-    }
-    strlcpy((char *)g_ap_config.ap.password, pass, MAX_PASSPHRASE_LEN);
-  }
-
-  if (strlen(pass) == 0) {
-    g_ap_config.ap.authmode = WIFI_AUTH_OPEN;
-  }
-
-  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
-  ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
-  return true;
-}
-
-static int wifi_cmd_ap(int argc, char **argv) {
-  int nerrors = arg_parse(argc, argv, (void **)&ap_args);
-
-  if (nerrors != 0) {
-    arg_print_errors(stderr, ap_args.end, argv[0]);
-    return 1;
-  }
-
-  if (true == wifi_cmd_ap_set(ap_args.ssid->sval[0], ap_args.password->sval[0]))
-    ESP_LOGI(
-        TAG_AP,
-        "Starting SoftAP with FTM Responder support, SSID - %s, Password - %s",
-        ap_args.ssid->sval[0], ap_args.password->sval[0]);
-  else
-    ESP_LOGE(TAG_AP, "Failed to start SoftAP!");
-
-  return 0;
-}
-
-static int wifi_cmd_query(int argc, char **argv) {
-  wifi_config_t cfg;
-  wifi_mode_t mode;
-
-  esp_wifi_get_mode(&mode);
-  if (WIFI_MODE_AP == mode) {
-    esp_wifi_get_config(WIFI_IF_AP, &cfg);
-    ESP_LOGI(TAG_AP, "AP mode, %s %s", cfg.ap.ssid, cfg.ap.password);
-  } else if (WIFI_MODE_STA == mode) {
-    int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
-    if (bits & CONNECTED_BIT) {
-      esp_wifi_get_config(WIFI_IF_STA, &cfg);
-      ESP_LOGI(TAG_STA, "sta mode, connected %s", cfg.ap.ssid);
-    } else {
-      ESP_LOGI(TAG_STA, "sta mode, disconnected");
-    }
-  } else {
-    ESP_LOGI(TAG_STA, "NULL mode");
-    return 0;
-  }
-
-  return 0;
-}
-
-wifi_ap_record_t *find_ftm_responder_ap(const char *ssid) {
-  bool retry_scan = false;
-  uint8_t i;
-
-  if (!ssid)
-    return NULL;
-
-retry:
-  if (!g_ap_list_buffer || (g_scan_ap_num == 0)) {
-    ESP_LOGI(TAG_STA, "Scanning for %s", ssid);
-    if (false == wifi_perform_scan(ssid, true)) {
-      return NULL;
-    }
-  }
-
-  for (i = 0; i < g_scan_ap_num; i++) {
-    if (strcmp((const char *)g_ap_list_buffer[i].ssid, ssid) == 0)
-      return &g_ap_list_buffer[i];
-  }
-
-  if (!retry_scan) {
-    retry_scan = true;
-    if (g_ap_list_buffer) {
-      free(g_ap_list_buffer);
-      g_ap_list_buffer = NULL;
-    }
-    goto retry;
-  }
-
-  ESP_LOGI(TAG_STA, "No matching AP found");
-
-  return NULL;
-}
-
-static int wifi_cmd_ftm(int argc, char **argv) {
-  int nerrors = arg_parse(argc, argv, (void **)&ftm_args);
-  wifi_ap_record_t *ap_record;
-  EventBits_t bits;
-
-  wifi_ftm_initiator_cfg_t ftmi_cfg = {
-      .frm_count = 32,
-      .burst_period = 2,
-  };
-
-  if (nerrors != 0) {
-    arg_print_errors(stderr, ftm_args.end, argv[0]);
-    return 0;
-  }
-
-  if (ftm_args.initiator->count != 0 && ftm_args.responder->count != 0) {
-    ESP_LOGE(TAG_STA, "Invalid FTM cmd argument");
-    return 0;
-  }
-
-  if (ftm_args.responder->count != 0)
-    goto ftm_responder;
-
-  bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
-  if (bits & CONNECTED_BIT && !ftm_args.ssid->count) {
-    memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
-    ftmi_cfg.channel = s_ap_channel;
-  } else if (ftm_args.ssid->count == 1) {
-    ap_record = find_ftm_responder_ap(ftm_args.ssid->sval[0]);
-    if (ap_record) {
-      memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
-      ftmi_cfg.channel = ap_record->primary;
-    } else {
-      return 0;
-    }
-  } else {
-    ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
-    return 0;
-  }
-
-  if (ftm_args.frm_count->count != 0) {
-    uint8_t count = ftm_args.frm_count->ival[0];
-    if (count != 0 && count != 8 && count != 16 && count != 24 && count != 32 &&
-        count != 64) {
-      ESP_LOGE(TAG_STA,
-               "Invalid Frame Count! Valid options are 0/8/16/24/32/64");
-      return 0;
-    }
-    ftmi_cfg.frm_count = count;
-  }
-
-  if (ftm_args.burst_period->count != 0) {
-    if (ftm_args.burst_period->ival[0] >= 2 &&
-        ftm_args.burst_period->ival[0] < 256) {
-      ftmi_cfg.burst_period = ftm_args.burst_period->ival[0];
-    } else {
-      ESP_LOGE(TAG_STA, "Invalid Burst Period! Valid range is 2-255");
-      return 0;
-    }
-  }
-
-  ESP_LOGI(TAG_STA,
-           "Requesting FTM session with Frm Count - %d, Burst Period - %dmSec "
-           "(0: No Preference)",
-           ftmi_cfg.frm_count, ftmi_cfg.burst_period * 100);
-
-  if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
-    ESP_LOGE(TAG_STA, "Failed to start FTM session");
-    return 0;
-  }
-
-  bits =
-      xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
-                          pdTRUE, pdFALSE, portMAX_DELAY);
-  /* Processing data from FTM session */
-  if (bits & FTM_REPORT_BIT) {
-    ftm_process_report();
-    free(s_ftm_report);
-    s_ftm_report = NULL;
-    s_ftm_report_num_entries = 0;
-    ESP_LOGI(TAG_STA,
-             "Estimated RTT - %d nSec, Estimated Distance - %d.%02d meters",
-             s_rtt_est, s_dist_est / 100, s_dist_est % 100);
-  } else {
-    /* Failure case */
-  }
-
-  return 0;
-
-ftm_responder:
-  if (ftm_args.offset->count != 0) {
-    int16_t offset_cm = ftm_args.offset->ival[0];
-
-    esp_wifi_ftm_resp_set_offset(offset_cm);
-  }
-
-  if (ftm_args.enable->count != 0) {
-    if (!s_ap_started) {
-      ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
-      return 0;
-    }
-    g_ap_config.ap.ftm_responder = true;
-    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
-    ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder enabled");
-
-    return 0;
-  }
-
-  if (ftm_args.disable->count != 0) {
-    if (!s_ap_started) {
-      ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
-      return 0;
-    }
-    g_ap_config.ap.ftm_responder = false;
-    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
-    ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder disabled");
-  }
-
-  return 0;
-}
-
-/* Calibration Routine */
-static int wifi_cmd_calibrate(int argc, char **argv) {
-  int nerrors = arg_parse(argc, argv, (void **)&calibrate_args);
-  wifi_ap_record_t *ap_record;
-  EventBits_t bits;
-
-  wifi_ftm_initiator_cfg_t ftmi_cfg = {
-      .frm_count = 8,
-      .burst_period = 2,
-  };
-
-  if (nerrors != 0) {
-    arg_print_errors(stderr, calibrate_args.end, argv[0]);
-    return 0;
-  }
-
-  bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
-  if (bits & CONNECTED_BIT && !calibrate_args.ssid->count) {
-    memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
-    ftmi_cfg.channel = s_ap_channel;
-  } else if (calibrate_args.ssid->count == 1) {
-    ap_record = find_ftm_responder_ap(calibrate_args.ssid->sval[0]);
-    if (ap_record) {
-      memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
-      ftmi_cfg.channel = ap_record->primary;
-    } else {
-      return 0;
-    }
-  } else {
-    ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
-    return 0;
-  }
-
-  /**
-   * Check if actual distance is set
-   */
-
-  if (calibrate_args.actual_distance->count != 0) {
-    if (calibrate_args.actual_distance->dval[0] < 0) {
-      ESP_LOGE(TAG_STA, "Invalid distance, please enter a positive value");
-      return 0;
-    }
-  }
-
-  /**
-   * Check if IP id is correct
-   */
-
-  if (calibrate_args.ap_id->count != 0) {
-    uint8_t ap_id = calibrate_args.ap_id->ival[0];
-    if (ap_id != 0 && ap_id != 1 && ap_id != 2 && ap_id != 3) {
-      ESP_LOGE(TAG_STA, "Invalid AP id. Valid options are 0/1/2/3");
-      return 0;
-    }
-  } else {
-    ESP_LOGE(TAG_STA,
-             "Please provide an AP id to calibrate. Valid options are 0/1/2/3");
-    return 0;
-  }
-
-  ESP_LOGI(TAG_STA,
-           "Calibration Routine with %s\nRequesting FTM session with Frm Count "
-           "- %d, Burst Period - %dmSec (0: No Preference)",
-           calibrate_args.ssid->sval[0], ftmi_cfg.frm_count,
-           ftmi_cfg.burst_period * 100);
-
-  if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
-    ESP_LOGE(TAG_STA, "Failed to start FTM session");
-    return 0;
-  }
-
-  bits =
-      xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
-                          pdTRUE, pdFALSE, portMAX_DELAY);
-  /* Processing data from FTM session */
-  if (bits & FTM_REPORT_BIT) {
-    ftm_process_report();
-    free(s_ftm_report);
-    s_ftm_report = NULL;
-    s_ftm_report_num_entries = 0;
-
-    ESP_LOGI(TAG_STA, "-------- CALIBRATION --------");
-
-    calibration_offsets[calibrate_args.ap_id->ival[0]] =
-        (calibrate_args.actual_distance->dval[0] * 100 - s_dist_est) / 100;
-
-    ESP_LOGI(TAG_STA, "Calibrating AP_%d", calibrate_args.ap_id->ival[0]);
-    ESP_LOGI(TAG_STA, "Estimated RTT : %d nSec\n", s_rtt_est);
-
-    ESP_LOGI(TAG_STA, "Estimated distance (no calibration) : %d.%02d",
-             s_dist_est / 100, s_dist_est % 100);
-    ESP_LOGI(TAG_STA, "Actual distance : %.2f",
-             calibrate_args.actual_distance->dval[0]);
-    ESP_LOGI(TAG_STA, "Calibration offset : %.2f",
-             calibration_offsets[calibrate_args.ap_id->ival[0]]);
-
-  } else {
-    /* Failure case */
-  }
-
-  return 0;
-}
-
-static int wifi_cmd_list_offsets(int argc, char **argv) {
-  ESP_LOGI(TAG_STA, "-------- OFFSETS --------");
-  for (size_t i = 0; i < 4; i++) {
-    ESP_LOGI(TAG_STA, "AP_%d : %.2f", i, calibration_offsets[i]);
-  }
-
-  return 0;
-}
-
+/*
+ * Register the FTM related commands
+ */
 void register_wifi(void) {
   sta_args.ssid = arg_str1(NULL, NULL, "<ssid>", "SSID of AP");
   sta_args.password = arg_str0(NULL, NULL, "<pass>", "password of AP");
@@ -863,4 +235,4 @@ void app_main(void) {
 
   // start console REPL
   ESP_ERROR_CHECK(esp_console_start_repl(repl));
-}
+}
\ No newline at end of file
diff --git a/main/math_3d.c b/main/math_3d.c
new file mode 100644
index 0000000000000000000000000000000000000000..327739b340cf9433dcb4f93e542f763eaede4efc
--- /dev/null
+++ b/main/math_3d.c
@@ -0,0 +1,59 @@
+#include "math_3d.h"
+#include <math.h>
+
+double distance(point_t a, point_t b) {
+  double dx = a.x - b.x;
+  double dy = a.y - b.y;
+  double dz = a.z - b.z;
+  return sqrt(dx * dx + dy * dy + dz * dz);
+}
+
+point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4,
+                    double d1, double d2, double d3, double d4) {
+  double x1 = p1.x, y1 = p1.y, z1 = p1.z;
+  double x2 = p2.x, y2 = p2.y, z2 = p2.z;
+  double x3 = p3.x, y3 = p3.y, z3 = p3.z;
+  double x4 = p4.x, y4 = p4.y, z4 = p4.z;
+
+  double A1 = 2 * (x2 - x1);
+  double B1 = 2 * (y2 - y1);
+  double C1 = 2 * (z2 - z1);
+  double D1 = d1 * d1 - d2 * d2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2 -
+              z1 * z1 + z2 * z2;
+
+  double A2 = 2 * (x3 - x1);
+  double B2 = 2 * (y3 - y1);
+  double C2 = 2 * (z3 - z1);
+  double D2 = d1 * d1 - d3 * d3 - x1 * x1 + x3 * x3 - y1 * y1 + y3 * y3 -
+              z1 * z1 + z3 * z3;
+
+  double A3 = 2 * (x4 - x1);
+  double B3 = 2 * (y4 - y1);
+  double C3 = 2 * (z4 - z1);
+  double D3 = d1 * d1 - d4 * d4 - x1 * x1 + x4 * x4 - y1 * y1 + y4 * y4 -
+              z1 * z1 + z4 * z4;
+
+  double D4 = d1 * d1 - d1 * d1 - x1 * x1 + x1 * x1 - y1 * y1 + y1 * y1 -
+              z1 * z1 + z1 * z1;
+
+  double a = A2 / A1;
+  double b = B2 / B1;
+  double c = C2 / C1;
+  double d = D2 / A1;
+  double e = D3 / A1 - a * D1 / A1;
+  double f = D3 / C1 - c * D1 / C1;
+  double g = B3 / B1 - b;
+  double h = D3 / B1 - d * B3 / B1;
+  double i = D2 / B1 - a * D1 / B1;
+  double j = D4 / C1 - c * D1 / C1;
+  double k = D4 / B1 - d * B3 / B1;
+  double l = i - a * g;
+
+  double m = j - c * g;
+  double n = (k - b * m / l) / (e - a * m / l);
+  double o = (i - j * n) / l;
+  double p = d - a * n - c * o;
+
+  point_t result = {n, o, p};
+  return result;
+}
\ No newline at end of file
diff --git a/main/math_3d.h b/main/math_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a0d4808be1df8ac22d563787726d65c8c1455f6
--- /dev/null
+++ b/main/math_3d.h
@@ -0,0 +1,21 @@
+#pragma once
+
+/*
+ * A 3D point in cartesian coordinates.
+ */
+typedef struct {
+  double x;
+  double y;
+  double z;
+} point_t;
+
+/*
+ * Computes the distance between two 3D points.
+ */
+double distance(point_t a, point_t b);
+
+/*
+ * 
+ */
+point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4,
+                    double d1, double d2, double d3, double d4);
\ No newline at end of file