diff --git a/.gitignore b/.gitignore
index 630aea9..1dfa7e4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -58,3 +58,4 @@ FreeRTOS/*
 
 # VS Code
 .vscode
+src/qemu.log
\ No newline at end of file
diff --git a/run.sh b/run.sh
index 4be5bfd..8e040e7 100755
--- a/run.sh
+++ b/run.sh
@@ -15,7 +15,7 @@ run() {
 run "./setup_network.sh"
 run "cd src"
 run "make clean"
-run "make -j"
+run "make -j > /dev/null"
 
 QEMU_MAC_ADDRESS="52:54:00:12:34:AD"
 CMD="qemu-system-arm -machine mps2-an385 -cpu cortex-m3 \
diff --git a/src/NTPTask.c b/src/NTPTask.c
index aa0144f..5e18369 100644
--- a/src/NTPTask.c
+++ b/src/NTPTask.c
@@ -2,10 +2,12 @@
 
 #include "NTP_main_utility.h"
 #include "NTP_peer.h"
+#include "NTP_poll.h"
 #include "NTP_vfo.h"
 
-static void vNTPTaskSendUsingStandardInterface(void *pvParameters);
+SemaphoreHandle_t timeMutex;
 
+// Global NTP client variables
 uint32_t NTP1_server_IP;
 Socket_t xSocket;
 struct freertos_sockaddr xDestinationAddress;
@@ -13,173 +15,194 @@ Assoc_table *assoc_table;
 struct ntp_s s;
 struct ntp_c c;
 
+// Task function declaration
+static void vNTPTask(void *pvParameters);
+static void vNTPTaskInterrupt(void *pvParameters);
+
+// Function to start NTP client task
 void vStartNTPClientTasks_SingleTasks(uint16_t usTaskStackSize, UBaseType_t uxTaskPriority)
 {
     BaseType_t x;
 
-    /* Create the echo client tasks. */
-
-    xTaskCreate(vNTPTaskSendUsingStandardInterface, /* The function that implements the task. */
-                "Echo0",                            /* Just a text name for the task to aid debugging. */
-                usTaskStackSize,                    /* The stack size is defined in FreeRTOSIPConfig.h. */
-                (void *)x,                          /* The task parameter, not used in this case. */
-                uxTaskPriority, /* The priority assigned to the task is defined in FreeRTOSConfig.h. */
-                NULL);          /* The task handle is not used. */
+    // Create the NTP client task
+    xTaskCreate(vNTPTask,        /* Task function */
+                "NTPSyncTask",   /* Task name */
+                usTaskStackSize, /* Stack size */
+                (void *)x,       /* Task parameter (unused) */
+                uxTaskPriority,  /* Task priority */
+                NULL);           /* No task handle required */
+    // Create the NTP client task for clock adjustment
+    // xTaskCreate(vNTPTaskInterrupt,    /* Task function */
+    //             "NTPClockAdjustTask", /* Task name */
+    //             usTaskStackSize / 25, /* Stack size */
+    //             (void *)x,            /* Task parameter (unused) */
+    //             uxTaskPriority,       /* Task priority */
+    //             NULL);                /* No task handle required */
 }
 
-static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
+void initTimeMutex() { timeMutex = xSemaphoreCreateMutex(); }
+
+static void init_network_and_DNS(const char *pcHostNames[], uint32_t NTP_server_IPs[])
 {
-    // setup sleep
-    const TickType_t x100ms = 100UL / portTICK_PERIOD_MS;
-    const TickType_t x700ms = 700UL / portTICK_PERIOD_MS;
     const TickType_t x1000ms = 1000UL / portTICK_PERIOD_MS;
-    const TickType_t x10000ms = 10000UL / portTICK_PERIOD_MS;
-
-    /* Create the socket. */
-    xSocket = FreeRTOS_socket(FREERTOS_AF_INET4,   /* Used for IPv4 UDP socket. */
-                                                   /* FREERTOS_AF_INET6 can be used for IPv6 UDP socket. */
-                              FREERTOS_SOCK_DGRAM, /*FREERTOS_SOCK_DGRAM for UDP.*/
+    xSocket = FreeRTOS_socket(FREERTOS_AF_INET4,   /* IPv4 UDP socket */
+                              FREERTOS_SOCK_DGRAM, /* Datagram (UDP) */
                               FREERTOS_IPPROTO_UDP);
 
-    /* Check the socket was created. */
+    // Check socket creation
     configASSERT(xSocket != FREERTOS_INVALID_SOCKET);
 
-    // Array of hostname strings
-    const char *pcHostNames[NMAX] = {"sth1.ntp.se",  // first one should be closest to user
-                                     "sth2.ntp.se", "svl1.ntp.se", "mmo1.ntp.se",
-                                     "lul1.ntp.se"};  // , "time-a-g.nist.gov", "time-a-wwv.nist.gov"
-
-    uint32_t NTP_server_IPs[NMAX];
-    uint8_t DNSok;
-
+    // Resolve hostnames to IPs
     for (int i = 0; i < NMAX; i++)
     {
-        DNSok = 0;
-        int retry = 0;
-        while (DNSok == 0)
+        for (int retry = 0; retry < 10; retry++)
         {
-            if (retry >= 10)
-            {
-                // FreeRTOS_printf(("\n\nDNS lookup failed, killing process.\n\n"));
-                return;
-            }
-
-            /* get the IP of the NTP server with FreeRTOS_gethostbyname */
+            // Resolve hostname
             NTP_server_IPs[i] = FreeRTOS_gethostbyname(pcHostNames[i]);
 
             if (NTP_server_IPs[i] == 0)
             {
-                // FreeRTOS_printf(("\n\nDNS lookup failed, trying again in 1 second. \n\n"));
-                retry++;
-                vTaskDelay(x1000ms);
+                FreeRTOS_printf(("\n\nDNS lookup failed, retrying in 1 second. \n\n"));
+                vTaskDelay(x1000ms);  // Retry delay
             }
             else
             {
-                // print successful and the hostname
-                DNSok = 1;
+                FreeRTOS_printf(("\n\nDNS lookup successful for %s\n\n", pcHostNames[i]));
+                break;
+            }
+
+            // Exit if max retries reached
+            if (retry == 9)
+            {
+                FreeRTOS_printf(("\n\nDNS lookup failed after multiple attempts. Exiting task.\n\n"));
+                return;
             }
         }
     }
-    // FreeRTOS_printf(("\n\nDNS lookup successful\n\n"));
-
-    /* Setup destination address */
-    xDestinationAddress.sin_family = FREERTOS_AF_INET4;  // or FREERTOS_AF_INET6 if the destination's IP is IPv6.
-    xDestinationAddress.sin_address.ulIP_IPv4 = NTP1_server_IP;  // destination IP
-    xDestinationAddress.sin_port = FreeRTOS_htons(123);          // dest port
-    xDestinationAddress.sin_len = (uint8_t)sizeof(struct freertos_sockaddr);
-
-    struct ntp_r *r;
-    r = malloc(sizeof(ntp_r));
-    // memset(r, 0, sizeof(ntp_r));
-    // r->dstaddr = DSTADDR;
-    // r->version = VERSION;
-    // r->leap = NOSYNC;
-    // r->mode = MODE;
-    // r->stratum = MAXSTRAT;
-    // r->poll = MINPOLL;
-    // r->precision = -18;
-
-    struct ntp_x *x;
-    x = malloc(sizeof(ntp_x));
-    memset(x, 0, sizeof(ntp_x));
-    x->dstaddr = DSTADDR;
+
+    // Set up destination address
+    xDestinationAddress.sin_family = FREERTOS_AF_INET4;
+    xDestinationAddress.sin_port = FreeRTOS_htons(123);  // NTP default port
+    xDestinationAddress.sin_len = sizeof(struct freertos_sockaddr);
+}
+
+// Task function for NTP client
+static void vNTPTask(void *pvParameters)
+{
+    // Sleep intervals in milliseconds
+    const TickType_t x100ms = 100UL / portTICK_PERIOD_MS;
+    const TickType_t x1000ms = 1000UL / portTICK_PERIOD_MS;
+    const TickType_t x10000ms = 10000UL / portTICK_PERIOD_MS;
+
+    const char *pcHostNames[NMAX] = {"sth1.ntp.se", "sth2.ntp.se", "svl1.ntp.se", "mmo1.ntp.se", "lul1.ntp.se"};
+
+    // Array to store resolved IP addresses of NTP servers
+    uint32_t NTP_server_IPs[NMAX] = {0};
+    init_network_and_DNS(pcHostNames, NTP_server_IPs);
+    initTimeMutex();
+
+    // Allocate and initialize NTP packet structures
+    struct ntp_r *r = malloc(sizeof(ntp_r));
+    struct ntp_x *x = calloc(1, sizeof(ntp_x));  // Zero-initialized
+
     x->version = VERSION;
-    x->leap = NOSYNC;
     x->mode = MODE;
     x->stratum = MAXSTRAT;
     x->poll = MINPOLL;
     x->precision = -18;
 
+    // Initialize NTP system and control structures
     memset(&s, 0, sizeof(ntp_s));
     s.leap = NOSYNC;
     s.stratum = MAXSTRAT;
     s.poll = MINPOLL;
     s.precision = -18;
-    s.p = NULL;
 
     memset(&c, 0, sizeof(ntp_c));
     c.lastTimeStampTick = 0;
 
+    // Handle frequency settings
     if (FREQ_EXISTS)
     {
-        c.freq = (double)SYSCLK_FRQ;
+        c.freq = (double)configTICK_RATE_HZ;
         rstclock(FSET, 0, 0);
     }
     else
     {
         rstclock(NSET, 0, 0);
     }
+
     c.jitter = LOG2D(s.precision);
     assoc_table_init(NTP_server_IPs);
-    /*
-        get time from one NTP server, use as start reference
-    */
+
+    // Initialize time with the first NTP server
     NTP1_server_IP = NTP_server_IPs[0];
-    x->srcaddr = NTP1_server_IP;
     xDestinationAddress.sin_address.ulIP_IPv4 = NTP1_server_IP;
-    // FreeRTOS_printf(("\n\n Getting first initial reference time from IP (which is %s): %lu.%lu.%lu.%lu \n\n",
-    //  pcHostNames[0],
-    //  NTP1_server_IP & 0xFF,            // Extract the fourth byte
-    //  (NTP1_server_IP >> 8) & 0xFF,     // Extract the third byte
-    //  (NTP1_server_IP >> 16) & 0xFF,    // Extract the second byte
-    //  (NTP1_server_IP >> 24) & 0xFF));  // Extract the first byte
-
-    prep_xmit(x);
-    xmit_packet(x);
-    recv_packet(r);
-    x->xmt = r->xmt;
+    x->srcaddr = NTP1_server_IP;
 
+    prep_xmit(x);    // Prepare for transmission
+    xmit_packet(x);  // Send initial request
+    recv_packet(r);  // Receive the response
+
+    x->xmt = r->xmt;  // Set initial transmission timestamp
     settime(x->xmt);
     gettime(1);
 
-    uint32_t ulCount = 0UL;
+    for (int i = 0; i < NMAX; i++)
+    {
+        // Update destination address with current NTP server
+        NTP1_server_IP = NTP_server_IPs[i];
+
+        FreeRTOS_printf(("\n\nGetting time from IP (%s): %lu.%lu.%lu.%lu\n\n", pcHostNames[i], (NTP1_server_IP & 0xFF),
+                         ((NTP1_server_IP >> 8) & 0xFF), ((NTP1_server_IP >> 16) & 0xFF),
+                         ((NTP1_server_IP >> 24) & 0xFF)));
+
+        prep_xmit(x);    // Prepare NTP packet
+        xmit_packet(x);  // Send request
+        recv_packet(r);  // Receive response
+
+        // Process the received response
+        receive(r);
+        vTaskDelay(x1000ms);  // Delay before next init
+    }
+    // return;
+
+    // Main NTP client loop
     for (;;)
     {
-        for (int i = 0; i < NMAX; i++)
-        {
-            // set the destination IP to the current NTP server
-            NTP1_server_IP = NTP_server_IPs[i];
-            x->srcaddr = NTP1_server_IP;
-            xDestinationAddress.sin_address.ulIP_IPv4 = NTP1_server_IP;
-
-            FreeRTOS_printf(("\n\n Getting from IP (which is %s): %lu.%lu.%lu.%lu \n\n", pcHostNames[i],
-                             NTP1_server_IP & 0xFF,            // Extract the fourth byte
-                             (NTP1_server_IP >> 8) & 0xFF,     // Extract the third byte
-                             (NTP1_server_IP >> 16) & 0xFF,    // Extract the second byte
-                             (NTP1_server_IP >> 24) & 0xFF));  // Extract the first byte
-
-            prep_xmit(x);
-            xmit_packet(x);
-            recv_packet(r);
-            x->xmt = r->xmt;
-            printTimestamp(r->rec, "r->rec");
-
-            receive(r);  // handle the response
-        }
+        // gettime(1);  // Get current time
+        printTimestamp(c.localTime, "current time\n\n");
+
+        clock_adjust(r);  // Adjust the clock
 
-        FreeRTOS_printf(("\n\nSleeping for 10 seconds before next get\n\n"));
+        vTaskDelay(x100ms);  // Delay before next adjustment
 
-        // sleep for 10 seconds
-        vTaskDelay(x10000ms);
+        // recv_packet(r);      // Receive response
+        // receive(r);
     }
+
+    // Cleanup allocated memory (typically not reached in FreeRTOS tasks)
+    free(r);
+    free(x);
 }
+
+// static void vNTPTaskInterrupt(void *pvParameters)
+// {
+//     // Sleep intervals in milliseconds
+//     const TickType_t x100ms = 100UL / portTICK_PERIOD_MS;
+//     const TickType_t x1000ms = 1000UL / portTICK_PERIOD_MS;
+//     const TickType_t x10000ms = 10000UL / portTICK_PERIOD_MS;
+
+//     while (!init)
+//     {
+//         vTaskDelay(x1000ms);
+//     }
+//     for (;;)
+//     {
+//         FreeRTOS_printf(("\n\nAdjust clock\n\n"));
+//         clock_adjust();  // Adjust the clock
+//         FreeRTOS_printf(("\n\nAdjusted clock\n\n"));
+
+//         vTaskDelay(x100ms);  // Delay before next adjustment
+//     }
+// }
\ No newline at end of file
diff --git a/src/NTPTask.h b/src/NTPTask.h
index 47e2b3b..4f3b766 100644
--- a/src/NTPTask.h
+++ b/src/NTPTask.h
@@ -5,11 +5,12 @@
 #include "freq.h"
 
 /*
- * Create the UDP echo client tasks.  This is the version where an echo request
- * is made from the same task that listens for the echo reply.
+ * Create the NTP client task and the clock adjust interrupt task.
  */
 void vStartNTPClientTasks_SingleTasks(uint16_t usTaskStackSize, UBaseType_t uxTaskPriority);
 
+extern SemaphoreHandle_t timeMutex;
+
 extern uint32_t NTP1_server_IP;
 extern Socket_t xSocket;
 extern struct freertos_sockaddr xDestinationAddress;
@@ -17,7 +18,4 @@ extern Assoc_table *assoc_table;
 extern struct ntp_s s;
 extern struct ntp_c c;
 
-extern TickType_t lastTimeStampTick;
-extern tstamp lastTimeStampTstamp;
-
 #endif /* UDP_ECHO_CLIENT_DEMO_H */
diff --git a/src/NTP_TDMG.c b/src/NTP_TDMG.c
index 2b3e9ee..1e40f7f 100644
--- a/src/NTP_TDMG.c
+++ b/src/NTP_TDMG.c
@@ -3,75 +3,25 @@
 #include "NTPTask.h"
 #include "NTP_main_utility.h"
 
-// Socket_t xSocket;
-// struct freertos_sockaddr xDestinationAddress;
-
-// struct ntp_s s;
-// struct ntp_c c;
-
-// void ntp_init(ntp_r *r , ntp_x *x , const char *pcHostNames[], uint32_t *NTP_server_IPs) {
-void ntp_init()
-{
-    // const TickType_t x1000ms = 1000UL / portTICK_PERIOD_MS;
-    // const TickType_t x10000ms = 10000UL / portTICK_PERIOD_MS;
-
-    // uint8_t DNSok;
-
-    // for (int i = 0; i < NMAX; i++)
-    // {
-    //     DNSok = 0;
-    //     while (DNSok == 0)
-    //     {
-    //         /* get the IP of the NTP server with FreeRTOS_gethostbyname */
-    //         NTP_server_IPs[i] = FreeRTOS_gethostbyname(pcHostNames[i]);
-
-    //         if (NTP_server_IPs[i] == 0)
-    //         {
-    //             FreeRTOS_printf(("\n\nDNS lookup failed, trying again in 1 second. \n\n"));
-    //             vTaskDelay(x1000ms);
-    //         }
-    //         else
-    //         {
-    //             // print successful and the hostname
-    //             FreeRTOS_printf(("\n\nDNS lookup successful for %s\n\n", pcHostNames[i]));
-    //             DNSok = 1;
-    //         }
-    //     }
-    // }
-
-    // // struct ntp_p *p; /* peer structure pointer */
-
-    // memset(r, 0, sizeof(ntp_r));
-    // r->dstaddr = DSTADDR;
-    // r->version = VERSION;
-    // r->leap = NOSYNC;
-    // r->mode = MODE;
-    // r->stratum = MAXSTRAT;
-    // r->poll = MINPOLL;
-    // r->precision = PRECISION;
-
-    // memset(x, 0, sizeof(ntp_x));
-    // x->dstaddr = DSTADDR;
-    // x->version = VERSION;
-    // x->leap = NOSYNC;
-    // x->mode = MODE;
-    // x->stratum = MAXSTRAT;
-    // x->poll = MINPOLL;
-    // x->precision = PRECISION;
-
-    // memset(&s, sizeof(ntp_s), 0);
-    // s.leap = NOSYNC;
-    // s.stratum = MAXSTRAT;
-    // s.poll = MINPOLL;
-    // s.precision = PRECISION;
-    // s.p = NULL;
-
-    // memset(&c, sizeof(ntp_c), 0);
-}
-
+/**
+ * Initializes the association table with NTP peers.
+ *
+ * This function allocates memory for the association table and then mobilizes
+ * each NTP peer. The IP addresses of the NTP peers are provided in the
+ * NTP_server_IPs array.
+ *
+ * @param NTP_server_IPs An array of IP addresses for the NTP peers.
+ */
 void assoc_table_init(uint32_t *NTP_server_IPs)
 {
     assoc_table->peers = (ntp_p **)malloc(NMAX * sizeof(ntp_p));
+    if (assoc_table->peers == NULL)
+    {
+        // Handle memory allocation failure, e.g. by logging an error message and returning
+        FreeRTOS_printf(("Memory allocation failed for association table.\n"));
+        return;
+    }
+
     assoc_table->size = NMAX;
 
     for (int i = 0; i < NMAX; i++)
@@ -80,22 +30,54 @@ void assoc_table_init(uint32_t *NTP_server_IPs)
     }
 }
 
+/**
+ * Deinitializes the association table.
+ *
+ * This function demobilizes each NTP peer and then frees the memory for the
+ * association table.
+ */
+void assoc_table_deinit()
+{
+    for (int i = 0; i < assoc_table->size; i++)
+    {
+        free(assoc_table->peers[i]);
+    }
+
+    free(assoc_table->peers);
+    assoc_table->peers = NULL;
+    assoc_table->size = 0;
+}
+
+/**
+ * Adds or updates an NTP peer in the association table.
+ *
+ * @param srcaddr The source address of the NTP peer.
+ * @param hmode The host mode of the NTP peer.
+ * @param xmt The transmit timestamp of the NTP peer.
+ * @return 1 if an existing entry was updated, 0 otherwise.
+ */
 int assoc_table_add(uint32_t srcaddr, char hmode, tstamp xmt)
 {
     // Check for duplicate peers
     for (int i = 0; i < assoc_table->size; i++)
     {
         ntp_p *assoc = assoc_table->peers[i];
+
         if (assoc->srcaddr == srcaddr && assoc->hmode == hmode)
         {
             assoc->xmt = xmt;
-            assoc->t = xmt;
+            // assoc->t = c.t;
             return 1;  // Entry already exists
         }
     }
     return 0;
 }
 
+/**
+ * Updates an NTP peer in the association table.
+ *
+ * @param p The NTP peer with updated data.
+ */
 void assoc_table_update(ntp_p *p)
 {
     for (int i = 0; i < assoc_table->size; i++)
@@ -103,12 +85,19 @@ void assoc_table_update(ntp_p *p)
         ntp_p *assoc = assoc_table->peers[i];
         if (assoc->srcaddr == p->srcaddr && assoc->hmode == p->hmode)
         {
-            assoc = p;
+            // Copy the data from p to assoc
+            assoc_table->peers[i] = p;
             return;
         }
     }
 }
 
+/**
+ * Calculates the square root of a number using binary search.
+ *
+ * @param number The number to calculate the square root of.
+ * @return The square root of the number.
+ */
 double sqrt(double number)
 {
     double low = 0, high = number;
@@ -135,6 +124,13 @@ double sqrt(double number)
     return (low + high) / 2;
 }
 
+/**
+ * Subtracts two unsigned 64-bit integers and returns the result.
+ *
+ * @param x The first unsigned 64-bit integer.
+ * @param y The second unsigned 64-bit integer.
+ * @return The difference between x and y as a signed 64-bit integer.
+ */
 int64_t subtract_uint64_t(uint64_t x, uint64_t y)
 {
     int neg = 0;
@@ -172,6 +168,13 @@ int64_t subtract_uint64_t(uint64_t x, uint64_t y)
     return result;
 }
 
+/**
+ * Adds two int64_t numbers and returns the result.
+ *
+ * @param x The first int64_t number.
+ * @param y The second int64_t number.
+ * @return The sum of x and y as an int64_t number.
+ */
 int64_t add_int64_t(int64_t x, int64_t y)
 {
     int32_t xFrac = (int32_t)(x & 0xFFFFFFFF);
@@ -193,13 +196,24 @@ int64_t add_int64_t(int64_t x, int64_t y)
     return result;
 }
 
+/**
+ * Sets the current time to a new value.
+ *
+ *
+ * @param newTime The new time value to set.
+ */
 void settime(tstamp newTime)
 {
-    c.t = newTime;
+    c.t++;
     c.localTime = newTime;
     c.lastTimeStampTick = xTaskGetTickCount();
 }
 
+/**
+ * @brief Get the time from the current tick count.
+ *
+ * @param override Flag indicating whether to override small differences in tick count.
+ */
 void gettime(int override)
 {
     TickType_t currentTick = xTaskGetTickCount();
@@ -207,10 +221,10 @@ void gettime(int override)
     // calculate the difference between the current tick and the last tick
     TickType_t tickDifference = currentTick - c.lastTimeStampTick;
 
-    // FreeRTOS_printf(("Tick diff\n"));
-    // FreeRTOS_printf_wrapper_double("", tickDifference);
+    FreeRTOS_printf(("Tick diff\n"));
+    FreeRTOS_printf_wrapper_double("", tickDifference);
     if (tickDifference < 10 && !override) return;  // Ignore small differences
-    // FreeRTOS_printf(("changing local time...\n\n"));
+    FreeRTOS_printf(("changing local time...\n\n"));
 
     // calculate tickDifference / 1000 as integer division
     TickType_t numSecondsInTicks = tickDifference / 1000;
@@ -243,21 +257,20 @@ void gettime(int override)
     c.localTime = newTimeStamp;
 }
 
+/**
+ * Prints a timestamp and a comment to the console.
+ *
+ * @param timestamp The timestamp to print.
+ * @param comment The comment to print with the timestamp.
+ */
 void printTimestamp(tstamp timestamp, const char *comment)
 {
     // Extract the whole seconds and fractional part from the timestamp
-    // uint32_t seconds = (uint32_t)(timestamp >> 32);
-    // uint32_t fractions = (uint32_t)(timestamp & 0xFFFFFFFF);
-    // double fractionAsSecond = fractions / (double)0xFFFFFFFF;
-    time_t seconds = (time_t)((timestamp >> 32) - 2208988800ull);
+    time_t seconds = (time_t)((timestamp >> 32) - 2208988800ull);  // Convert NTP epoch to Unix epoch
     uint32_t fractions = (timestamp & 0xFFFFFFFF);
 
-    // double fractionAsSecond = fractions / (double)0xFFFFFFFF;
-    double fractionAsSecond = (double)fractions / FRAC;  // / 4294967296.0 = 2^32
-
-    // time_t timeInSeconds = (time_t)((r->rec >> 32) - 2208988800ull);
-    // uint32_t frac = (uint32_t)(r->rec & 0xFFFFFFFF);
-    // FreeRTOS_printf(("\n\n Time according to %s: %s.%u\n", pcHostNames[i], ctime(&timeInSeconds), frac));
+    // Convert fractions to a fraction of a second
+    double fractionAsSecond = (double)fractions / FRAC;  // FRAC is presumably defined as 0xFFFFFFFF or 4294967296.0
 
     // Print the timestamp with the comment
     FreeRTOS_printf(("%s Timestamp: %s. seconds\n", comment, ctime(&seconds)));
@@ -266,6 +279,12 @@ void printTimestamp(tstamp timestamp, const char *comment)
 
 #define MAX_UINT64_DIGITS 20  // Maximum digits in uint64_t
 
+/**
+ * Converts a 64-bit unsigned integer to a string.
+ *
+ * @param value The number to convert.
+ * @param str The output string. This must be large enough to hold all digits of the number plus a null terminator.
+ */
 void uint64_to_str(uint64_t value, char *str)
 {
     char temp[MAX_UINT64_DIGITS];
@@ -295,17 +314,29 @@ void uint64_to_str(uint64_t value, char *str)
     str[j] = '\0';  // Null-terminate string
 }
 
+/**
+ * A wrapper around the FreeRTOS_printf function that allows printing a 64-bit unsigned integer.
+ *
+ * @param format The format string for the message. This should contain one %s specifier where the number will be
+ * inserted.
+ * @param value The number to print.
+ */
 void FreeRTOS_printf_wrapper(const char *format, uint64_t value)
 {
-    char buffer[MAX_UINT64_DIGITS + 50];  // Additional space for message text
     char numberStr[MAX_UINT64_DIGITS];
-    uint64_to_str(value, numberStr);
-    sprintf(buffer, format, numberStr);
-    // FreeRTOS_printf(("%s\n", buffer));
+    uint64_to_str(value, numberStr);       // Convert the number to a string
+    FreeRTOS_printf((format, numberStr));  // Print the formatted message
 }
 
 #define MAX_DOUBLE_DIGITS 30  // A buffer size that should handle most cases
 
+/**
+ * Converts a double value to a string with a specified precision.
+ *
+ * @param value The double value to convert.
+ * @param str The output string. This must be large enough to hold all digits of the number plus a null terminator.
+ * @param precision The number of digits after the decimal point.
+ */
 void double_to_str(double value, char *str, int precision)
 {
     if (precision > 9) precision = 9;  // Limit precision to 9 digits for simplicity
@@ -367,18 +398,30 @@ void double_to_str(double value, char *str, int precision)
     str[index] = '\0';  // Null-terminate the string
 }
 
+/**
+ * A wrapper around the FreeRTOS_printf function that allows printing a double value.
+ *
+ * @param format The format string for the message. This should contain one %s specifier where the number will be
+ * inserted.
+ * @param value The double value to print.
+ */
 void FreeRTOS_printf_wrapper_double(const char *format, double value)
 {
     char buffer[MAX_DOUBLE_DIGITS + 50];  // Additional space for message text
-    double_to_str(value, buffer, 6);      // Example with 6 decimal places
+    double_to_str(value, buffer, 10);     // Example with 6 decimal places
     FreeRTOS_printf(("%s\n", buffer));
 }
 
+/**
+ * Prints a 64-bit unsigned integer as two 32-bit parts.
+ *
+ * @param number The 64-bit unsigned integer to print.
+ */
 void print_uint64_as_32_parts(uint64_t number)
 {
     uint32_t lower_part = (uint32_t)number;          // Extracts the lower 32 bits
     uint32_t upper_part = (uint32_t)(number >> 32);  // Shifts right by 32 bits and extracts the upper 32 bits
 
-    // printf("Upper 32 bits: %u\n", upper_part);
-    // printf("Lower 32 bits: %u\n", lower_part);
+    printf("Upper 32 bits: %u\n", upper_part);
+    printf("Lower 32 bits: %u\n", lower_part);
 }
\ No newline at end of file
diff --git a/src/NTP_TDMG.h b/src/NTP_TDMG.h
index 829994c..becf12f 100644
--- a/src/NTP_TDMG.h
+++ b/src/NTP_TDMG.h
@@ -7,10 +7,6 @@ G Global Constants
 */
 #ifndef NTP_TDMG_H
 #define NTP_TDMG_H
-// A.1.1.  Definitions, Constants, Parameters
-
-// TODO
-// #include "FreeRTOS_IP_Common.h"
 
 #include <math.h>     /* avoids complaints about sqrt() */
 #include <stdlib.h>   /* for malloc() and friends */
@@ -19,6 +15,7 @@ G Global Constants
 
 #include "FreeRTOS_IP.h"
 #include "FreeRTOS_Sockets.h"
+#include "semphr.h"
 
 /*
  * Data types
@@ -49,8 +46,6 @@ typedef uint64_t
 typedef uint32_t tdist; /* NTP short format */
 
 typedef uint32_t ipaddr; /* IPv4 or IPv6 address */
-// TODO use freertos ipaddr
-//  extern IP_Address;
 
 typedef uint32_t digest; /* md5 digest */
 typedef int8_t s_char;   /* precision and poll interval (log2) */
@@ -66,12 +61,12 @@ typedef int8_t s_char;   /* precision and poll interval (log2) */
 /*
  * Timestamp conversion macroni
  */
-#define FRIC 65536.                 /* 2^16 as a double */
-#define D2FP(r) ((tdist)((r)*FRIC)) /* NTP short */
+#define FRIC 65536.                   /* 2^16 as a double */
+#define D2FP(r) ((tdist)((r) * FRIC)) /* NTP short */
 #define FP2D(r) ((double)(r) / FRIC)
 
-#define FRAC 4294967296.              /* 2^32 as a double */
-#define D2LFP(a) ((tstamp)((a)*FRAC)) /* NTP timestamp */
+#define FRAC 4294967296.                /* 2^32 as a double */
+#define D2LFP(a) ((tstamp)((a) * FRAC)) /* NTP timestamp */
 #define LFP2D(a) ((double)(a) / FRAC)
 #define U2LFP(a) (((unsigned long long)((a).tv_sec + JAN_1970) << 32) + (unsigned long long)((a).tv_usec / 1e6 * FRAC))
 
@@ -185,12 +180,6 @@ typedef int8_t s_char;   /* precision and poll interval (log2) */
 #define min(a, b) ((a) < (b) ? (a) : (b))
 #define max(a, b) ((a) < (b) ? (b) : (a))
 
-/* Global variable */
-// extern struct ntp_s s;
-// extern struct ntp_c c;
-
-// A.1.2.  Packet Data Structures
-
 /*
  * The receive and transmit packets may contain an optional message
  * authentication code (MAC) consisting of a key identifier (keyid) and
@@ -257,8 +246,6 @@ typedef struct ntp_x
     digest dgst;      /* message digest */
 } ntp_x;
 
-// A.1.3.  Association Data Structures
-
 /*
  * Filter stage structure.  Note the t member in this and other
  * structures refers to process time, not real time.  Process time
@@ -327,29 +314,33 @@ typedef struct ntp_p
     int nextdate;         /* next poll time */
 } ntp_p;
 
-// A.1.4.  System Data Structures
-
 /*
- * Chime list.  This is used by the intersection algorithm.
+ * Chime list structure based on Marzullo's algorithm, used in the intersection algorithm of NTP.
+ *
+ * This structure represents an entry in the chime list.
  */
 typedef struct ntp_m
-{                    /* m is for Marzullo */
-    struct ntp_p *p; /* peer structure pointer */
-    int type;        /* high +1, mid 0, low -1 */
-    double edge;     /* correctness interval edge */
+{
+    struct ntp_p *p;
+    int type;
+    double edge;
 } ntp_m;
 
 /*
- * Survivor list.  This is used by the clustering algorithm.
+ * Survivor list structure used in the clustering algorithm of NTP.
+ *
+ * This structure represents an entry in the survivor list.
  */
 typedef struct ntp_v
 {
-    struct ntp_p *p; /* peer structure pointer */
-    double metric;   /* sort metric */
+    struct ntp_p *p;
+    double metric;
 } ntp_v;
 
 /*
- * System structure
+ * NTP peer structure based on RFC 5905.
+ *
+ * This structure represents an NTP peer in the system.
  */
 typedef struct ntp_s
 {
@@ -371,10 +362,10 @@ typedef struct ntp_s
     int n;                    /* number of survivors */
 } ntp_s;
 
-// A.1.5.  Local Clock Data Structures
-
 /*
- * Local clock structure
+ * Local clock structure based on RFC 5905.
+ *
+ * This structure represents the local clock in an NTP system.
  */
 typedef struct ntp_c
 {
@@ -390,28 +381,6 @@ typedef struct ntp_c
     TickType_t lastTimeStampTick; /* freertos tick*/
 } ntp_c;
 
-typedef struct ntp_packet
-{
-    uint8_t li_vn_mode;       // Eight bits. li, vn, and mode.
-                              // li.   Two bits.   Leap indicator.
-                              // vn.   Three bits. Version number of the protocol.
-                              // mode. Three bits. Client will pick mode 3 for client.
-    uint8_t stratum;          // Eight bits. Stratum level of the local clock.
-    uint8_t poll;             // Eight bits. Maximum interval between successive messages.
-    uint8_t precision;        // Eight bits. Precision of the local clock.
-    uint32_t rootDelay;       // 32 bits. Total round trip delay time.
-    uint32_t rootDispersion;  // 32 bits. Max error aloud from primary clock source.
-    uint32_t refId;           // 32 bits. Reference clock identifier.
-    uint32_t refTm_s;         // 32 bits. Reference time-stamp seconds.
-    uint32_t refTm_f;         // 32 bits. Reference time-stamp fraction of a second.
-    uint32_t origTm_s;        // 32 bits. Originate time-stamp seconds.
-    uint32_t origTm_f;        // 32 bits. Originate time-stamp fraction of a second.
-    uint32_t rxTm_s;          // 32 bits. Received time-stamp seconds.
-    uint32_t rxTm_f;          // 32 bits. Received time-stamp fraction of a second.
-    uint32_t txTm_s;          // 32 bits. Transmit time-stamp seconds.
-    uint32_t txTm_f;          // 32 bits. Transmit time-stamp fraction of a second.
-} ntp_packet;
-
 // Association Data Structures used in the assoc table to find associations between peers and addresses
 typedef struct Assoc_table
 {
@@ -423,9 +392,6 @@ void assoc_table_init(uint32_t *);
 int assoc_table_add(uint32_t, char, tstamp);
 void assoc_table_update(ntp_p *);
 
-// void ntp_init(ntp_r *, ntp_x *, const char *[], uint32_t *);
-void ntp_init();
-
 double sqrt(double number);
 
 int64_t subtract_uint64_t(uint64_t x, uint64_t y);
diff --git a/src/NTP_main_utility.c b/src/NTP_main_utility.c
index 12001cc..c1510dd 100644
--- a/src/NTP_main_utility.c
+++ b/src/NTP_main_utility.c
@@ -3,8 +3,18 @@
 #include "NTPTask.h"
 #include "NTP_peer.h"
 
-static struct ntp_p *p; /* peer structure pointer */
-
+/**
+ * This function creates a new NTP peer structure and initializes it.
+ *
+ * @param srcaddr - The IP source address.
+ * @param dstaddr - The IP destination address.
+ * @param version - The NTP version.
+ * @param mode - The host mode.
+ * @param keyid - The key identifier.
+ * @param flags - The peer flags.
+ *
+ * @return A pointer to the new NTP peer structure, or NULL if the memory allocation failed.
+ */
 struct ntp_p *mobilize(uint32_t srcaddr, /* IP source address, change from ipaddr to uint32_t */
                        uint32_t dstaddr, /* IP destination address, change from ipaddr to uint32_t */
                        int version,      /* version */
@@ -13,13 +23,13 @@ struct ntp_p *mobilize(uint32_t srcaddr, /* IP source address, change from ipadd
                        int flags         /* peer flags */
 )
 {
-    struct ntp_p *p; /* peer process pointer */
+    struct ntp_p *p = calloc(1, sizeof(struct ntp_p)); /* allocate and zero-initialize association memory */
+
+    if (p == NULL)
+    {
+        return NULL; /* return NULL if memory allocation failed */
+    }
 
-    /*
-     * Allocate and initialize association memory
-     */
-    p = malloc(sizeof(struct ntp_p));
-    memset(p, 0, sizeof(ntp_p));
     p->srcaddr = srcaddr;
     p->dstaddr = dstaddr;
     p->version = version;
@@ -28,11 +38,16 @@ struct ntp_p *mobilize(uint32_t srcaddr, /* IP source address, change from ipadd
     p->hpoll = MINPOLL;
     clear(p, X_INIT);
     p->flags = flags;
-    return (p);
+
+    return p;
 }
 
-/*
- * find_assoc() - find a matching association
+/**
+ * This function searches for a matching association in the association table.
+ *
+ * @param r - A pointer to the received NTP packet.
+ *
+ * @return A pointer to the matching association, or NULL if no match is found.
  */
 struct
     ntp_p /* peer structure pointer or NULL */
@@ -68,7 +83,16 @@ digest md5(int keyid /* key identifier */
     return (/* MD5 digest */ 0);
 }
 
-void prv_swap_fields(struct ntp_packet *pkt)
+/**
+ * This function swaps the byte order of the fields in an NTP packet.
+ *
+ * @param pkt - A pointer to the NTP packet.
+ *
+ * The function uses the FreeRTOS_htonl function to perform the byte order swap. This function converts a 32-bit number
+ * from host byte order to network byte order. Network byte order is big-endian, so this function is necessary when
+ * sending data over the network on a little-endian system.
+ */
+static void prv_swap_fields(struct ntp_packet *pkt)
 {
     /* NTP messages are big-endian */
     pkt->rootDelay = FreeRTOS_htonl(pkt->rootDelay);
@@ -87,21 +111,22 @@ void prv_swap_fields(struct ntp_packet *pkt)
     pkt->txTm_f = FreeRTOS_htonl(pkt->txTm_f);
 }
 
-void create_ntp_r(struct ntp_r *r, ntp_packet *pkt)
+/**
+ * This function creates and initializes an NTP receive structure from an NTP packet.
+ *
+ * @param r - A pointer to the NTP receive structure to be initialized.
+ * @param pkt - A pointer to the NTP packet.
+ */
+static void prv_create_ntp_r(struct ntp_r *r, ntp_packet *pkt)
 {
     r->srcaddr = NTP1_server_IP;  // Set to zero if not known
     r->dstaddr = DSTADDR;         // Set to zero if not known
 
     // Extract version, leap, and mode from li_vn_mode
-    // pkt->li_vn_mode = FreeRTOS_ntohl(pkt->li_vn_mode) << 24;
-    r->leap = (pkt->li_vn_mode >> 6) & 0x3;     // Extract bits 0-1
-    r->version = (pkt->li_vn_mode >> 3) & 0x7;  // Extract bits 2-4
-    r->mode = (pkt->li_vn_mode) & 0x7;          // Extract bits 5-7
-
-    // uint8_t li_vn_mode;      // Eight bits. li, vn, and mode.
-    //                          // li.   Two bits.   Leap indicator.
-    //                          // vn.   Three bits. Version number of the protocol.
-    //                          // mode. Three bits. Client will pick mode 3 for client
+    r->leap = (pkt->li_vn_mode >> 6) & 0x3;     // Extract bits 0-1. li, Leap indicator.
+    r->version = (pkt->li_vn_mode >> 3) & 0x7;  // Extract bits 2-4. vn, Version number of the protocol.
+    r->mode = (pkt->li_vn_mode) & 0x7;          // Extract bits 5-7. mode, Client will pick mode 3 for client
+
     r->stratum = ((char)pkt->stratum);
     r->poll = (char)pkt->poll;
     r->precision = (s_char)pkt->precision;
@@ -125,6 +150,11 @@ void create_ntp_r(struct ntp_r *r, ntp_packet *pkt)
     r->dst = dst;  // Set the timestamp to the ms passed since vTaskStartScheduler started
 }
 
+/**
+ * This function receives an NTP packet, converts it to an NTP receive structure, and prints the original timestamp.
+ *
+ * @param r - A pointer to the NTP receive structure to be filled.
+ */
 void recv_packet(ntp_r *r)
 {
     ntp_packet pkt;
@@ -135,7 +165,7 @@ void recv_packet(ntp_r *r)
 
     memset(r, 0, sizeof(ntp_r));  // Clear the receive packet struct
 
-    // FreeRTOS_printf(("Receiving packet...\n"));
+    FreeRTOS_printf(("Receiving packet...\n"));
 
     iReturned = FreeRTOS_recvfrom(xSocket, &pkt, sizeof(ntp_packet), 0, &xSourceAddress, &xAddressLength);
 
@@ -143,26 +173,31 @@ void recv_packet(ntp_r *r)
     {
         prv_swap_fields(&pkt);
 
-        // FreeRTOS_printf(("Packet received\n"));
+        FreeRTOS_printf(("Packet received\n"));
 
         // do conversion
         // ntp_packet -> ntp_r
-        create_ntp_r(r, &pkt);
-        printTimestamp(r->org, "The original time is: ");
+        prv_create_ntp_r(r, &pkt);
     }
     else
     {
-        // FreeRTOS_printf(("Error receiving packet\n"));
+        FreeRTOS_printf(("Error receiving packet\n"));
     }
 }
+
+/**
+ * This function prepares an NTP transmit structure for sending.
+ *
+ * @param x - A pointer to the NTP transmit structure to be prepared.
+ */
 void prep_xmit(ntp_x *x)
 {
     x->leap = s.leap;
     x->stratum = s.stratum;
 
     // Root delay and dispersion can come from a server or client
-    x->rootdelay = s.rootdelay;
-    x->rootdisp = s.rootdisp;
+    x->rootdelay = D2FP(s.rootdelay);
+    x->rootdisp = D2FP(s.rootdisp);
     x->refid = s.refid;
 
     // Assuming tstamp is a type that can be split into seconds and fraction of a second
@@ -173,7 +208,13 @@ void prep_xmit(ntp_x *x)
     x->xmt = c.localTime;
 }
 
-void prv_translate_ntp_x_to_ntp_packet(const ntp_x *x, ntp_packet *pkt)
+/**
+ * Translates the fields of an ntp_x structure to an ntp_packet structure.
+ *
+ * @param x The ntp_x structure containing the fields to be translated.
+ * @param pkt The ntp_packet structure to store the translated fields.
+ */
+static void prv_translate_ntp_x_to_ntp_packet(const ntp_x *x, ntp_packet *pkt)
 {
     // Combine leap, version, and mode into li_vn_mode
     pkt->li_vn_mode = (x->leap & 0x03) << 6 | (x->version & 0x07) << 3 | (x->mode & 0x07);
@@ -199,8 +240,12 @@ void prv_translate_ntp_x_to_ntp_packet(const ntp_x *x, ntp_packet *pkt)
     pkt->txTm_f = (uint32_t)(x->xmt & 0xFFFFFFFF);
 }
 
-/*
- * xmit_packet - transmit packet to network
+/**
+ * @brief Transmits an NTP packet.
+ *
+ * This function transmits an NTP packet using the provided transmit packet pointer.
+ *
+ * @param x Pointer to the transmit packet.
  */
 void xmit_packet(ntp_x *x /* transmit packet pointer */
 )
@@ -208,19 +253,22 @@ void xmit_packet(ntp_x *x /* transmit packet pointer */
     /* setup ntp_packet *pkt */
     ntp_packet pkt;  // Allocate memory for the packet
 
-    // set xmit time to current time!
+    xDestinationAddress.sin_address.ulIP_IPv4 = NTP1_server_IP;
+    x->srcaddr = NTP1_server_IP;
+
+    // set xmit time to current time
     gettime(0);
     x->xmt = c.localTime;
 
     prv_translate_ntp_x_to_ntp_packet(x, &pkt);
     prv_swap_fields(&pkt);
 
-    // FreeRTOS_printf(("Sending packet...\n"));
+    FreeRTOS_printf(("Sending packet...\n"));
 
     // Add the association to the table
     if (!assoc_table_add(x->srcaddr, x->mode, x->xmt))
     {
-        // FreeRTOS_printf(("Error adding association to table\n"));
+        FreeRTOS_printf(("Error adding association to table\n"));
     }
 
     int32_t iReturned;
@@ -230,16 +278,14 @@ void xmit_packet(ntp_x *x /* transmit packet pointer */
 
     if (iReturned == sizeof(ntp_packet))
     {
-        // FreeRTOS_printf(("Sent packet\n"));
+        FreeRTOS_printf(("Sent packet\n"));
     }
     else
     {
-        // FreeRTOS_printf(("Failed to send packet\n"));
+        FreeRTOS_printf(("Failed to send packet\n"));
     }
 }
 
-// A.4.  Kernel System Clock Interface
-
 /*
  * System clock utility functions
  *
@@ -253,42 +299,90 @@ void xmit_packet(ntp_x *x /* transmit packet pointer */
  */
 #define JAN_1970 2208988800UL /* 1970 - 1900 in seconds */
 
-/*
- * step_time() - step system time to given offset value
+/**
+ * @brief Updates the local time based on a specified offset in seconds.
+ *
+ * @param offset_seconds Offset in seconds to be applied to the local time.
  */
-void step_time(double offset /* clock offset */
-)
+static void prv_update_localTime(double offset_seconds, int isAdjustment /* 0 for step, 1 for adjust */)
 {
-    struct timeval unix_time;
-    tstamp time;
+    if (xSemaphoreTake(timeMutex, portMAX_DELAY))
+    {
+        // printf("Updating local time\n");
+        // TickType_t currentTick = xTaskGetTickCount();
+        // FreeRTOS_printf_wrapper_double("Offset in ticks: ", currentTick);
+        // TickType_t offsetTicks = (TickType_t)(offset_seconds * configTICK_RATE_HZ);
+        // FreeRTOS_printf_wrapper_double("Offset in ticks: ", D2LFP(offset_seconds));
+        // FreeRTOS_printf_wrapper_double("Offset in ticks: ", offsetTicks);
+
+        // // Calculate the number of seconds in the tick difference
+        // TickType_t numSecondsInTicks = offsetTicks / configTICK_RATE_HZ;
+
+        // // Calculate the remaining fraction of ticks converted to NTP fractions
+        // TickType_t remainingTicks = offsetTicks % configTICK_RATE_HZ;
+        // uint32_t newFractions = (uint32_t)(((double)remainingTicks / configTICK_RATE_HZ) * FRAC);
+        // FreeRTOS_printf_wrapper_double("Offset in ticks: ", newFractions);
+
+        // Extract current fractions and seconds from the last known local time
+        uint32_t currentFractions = (uint32_t)(c.localTime & 0xFFFFFFFF);
+        uint32_t newFractions = (uint32_t)(D2LFP(offset_seconds));
+        uint32_t currentSeconds = (uint32_t)(c.localTime >> 32);
+
+        uint32_t tempFractions;
+        if (isAdjustment)
+        {
+            tempFractions = currentFractions + newFractions;
+            if (tempFractions < currentFractions)
+            {                      // Handle overflow
+                currentSeconds++;  // Increment the seconds part due to overflow
+            }
+        }
+        else
+        {
+            tempFractions = currentFractions - newFractions;
+            if (tempFractions > currentFractions)
+            {                      // Handle underflow
+                currentSeconds--;  // Decrement the seconds part due to underflow
+            }
+        }
+        // FreeRTOS_printf_wrapper_double("", currentFractions);
+        // FreeRTOS_printf_wrapper_double("", newFractions);
 
-    /*
-     * Convert from double to native format (signed) and add to the
-     * current time.  Note the addition is done in native format to
-     * avoid overflow or loss of precision.
-     */
-    // gettimeofday(&unix_time, NULL);
-    time = D2LFP(offset) + U2LFP(unix_time);
-    unix_time.tv_sec = time >> 32;
-    unix_time.tv_usec = (long)(((time - unix_time.tv_sec) << 32) / FRAC * 1e6);
-    // settimeofday(&unix_time, NULL);
+        // Construct the new timestamp
+        uint64_t newTimeStamp = ((uint64_t)currentSeconds << 32) | (tempFractions & 0xFFFFFFFF);
+
+        // Update the control structure with the new tick and local time
+        c.localTime = newTimeStamp;
+
+        // Optionally print the updated time for debugging
+        // printTimestamp(c.localTime, "Updated time:");
+
+        xSemaphoreGive(timeMutex);
+    }
 }
 
-/*
- * adjust_time() - slew system clock to given offset value
+/**
+ * @brief Steps the time by applying the given clock offset.
+ *
+ * This function updates the local time by applying the provided clock offset.
+ *
+ * @param offset The clock offset to apply.
  */
-void adjust_time(double offset /* clock offset */
-)
+void step_time(double offset)
 {
-    struct timeval unix_time;
-    tstamp time;
+    double ntp_time = (offset);
+    prv_update_localTime(ntp_time, 0);
+}
 
-    /*
-     * Convert from double to native format (signed) and add to the
-     * current time.
-     */
-    time = D2LFP(offset);
-    unix_time.tv_sec = time >> 32;
-    unix_time.tv_usec = (long)(((time - unix_time.tv_sec) << 32) / FRAC * 1e6);
-    // adjtime(&unix_time, NULL);
+/**
+ * Adjusts the system time by the given offset.
+ *
+ * @param offset The clock offset to adjust the time by.
+ */
+void adjust_time(double offset)
+{
+    FreeRTOS_printf(("Adjusting time by:\n"));
+    FreeRTOS_printf_wrapper_double("", offset);
+    double ntp_time = (offset);
+    prv_update_localTime(offset, 1);
 }
diff --git a/src/NTP_main_utility.h b/src/NTP_main_utility.h
index 76b13c6..62c6a60 100644
--- a/src/NTP_main_utility.h
+++ b/src/NTP_main_utility.h
@@ -3,6 +3,28 @@
 
 #include "NTP_TDMG.h"
 
+typedef struct ntp_packet
+{
+    uint8_t li_vn_mode;       // Eight bits. li, vn, and mode.
+                              // li.   Two bits.   Leap indicator.
+                              // vn.   Three bits. Version number of the protocol.
+                              // mode. Three bits. Client will pick mode 3 for client.
+    uint8_t stratum;          // Eight bits. Stratum level of the local clock.
+    uint8_t poll;             // Eight bits. Maximum interval between successive messages.
+    uint8_t precision;        // Eight bits. Precision of the local clock.
+    uint32_t rootDelay;       // 32 bits. Total round trip delay time.
+    uint32_t rootDispersion;  // 32 bits. Max error aloud from primary clock source.
+    uint32_t refId;           // 32 bits. Reference clock identifier.
+    uint32_t refTm_s;         // 32 bits. Reference time-stamp seconds.
+    uint32_t refTm_f;         // 32 bits. Reference time-stamp fraction of a second.
+    uint32_t origTm_s;        // 32 bits. Originate time-stamp seconds.
+    uint32_t origTm_f;        // 32 bits. Originate time-stamp fraction of a second.
+    uint32_t rxTm_s;          // 32 bits. Received time-stamp seconds.
+    uint32_t rxTm_f;          // 32 bits. Received time-stamp fraction of a second.
+    uint32_t txTm_s;          // 32 bits. Transmit time-stamp seconds.
+    uint32_t txTm_f;          // 32 bits. Transmit time-stamp fraction of a second.
+} ntp_packet;
+
 // mobilize - allocates and initializes a new peer structure
 ntp_p *mobilize(uint32_t srcaddr,  // IP source address
                 uint32_t dstaddr,  // IP destination address
diff --git a/src/NTP_peer.c b/src/NTP_peer.c
index 288fc7e..c97c92b 100644
--- a/src/NTP_peer.c
+++ b/src/NTP_peer.c
@@ -1,18 +1,5 @@
 #include "NTP_peer.h"
 
-// A.5.1.1.  packet()
-
-double log2d(int a)
-{
-    if (a < 0)
-    {
-        return 1.0 / (1L << -a);
-    }
-    else
-    {
-        return (double)(1L << a);
-    }
-}
 /*
  * Dispatch matrix
  *              active  passv  client server bcast */
@@ -24,9 +11,13 @@ int table[7][5] = {
     /* server  */ {DSCRD, DSCRD, DSCRD, DSCRD, DSCRD},
     /* bcast   */ {DSCRD, DSCRD, DSCRD, DSCRD, DSCRD},
     /* bclient */ {DSCRD, DSCRD, DSCRD, DSCRD, PROC}};
-/*
- * packet() - process packet and compute offset, delay, and
- * dispersion.
+
+/**
+ * This function processes a received NTP packet and updates the peer structure accordingly.
+ * It calculates the offset, delay, and dispersion of the received packet and passes them to the clock filter.
+ *
+ * @param p - A pointer to the peer structure to be updated.
+ * @param r - A pointer to the received packet.
  */
 void packet(struct ntp_p *p, /* peer structure pointer */
             struct ntp_r *r  /* receive packet pointer */
@@ -50,6 +41,7 @@ void packet(struct ntp_p *p, /* peer structure pointer */
         p->stratum = r->stratum;
     p->pmode = r->mode;
     p->ppoll = r->poll;
+    p->rootdelay = FP2D(r->rootdelay);
     p->rootdisp = FP2D(r->rootdisp);
     p->refid = r->refid;
     p->reftime = r->reftime;
@@ -92,36 +84,26 @@ void packet(struct ntp_p *p, /* peer structure pointer */
     else
     {
         offset = LFP2D(add_int64_t(subtract_uint64_t(r->rec, r->org), subtract_uint64_t(r->dst, r->xmt))) / 2;
+        FreeRTOS_printf(("offset: %f\n", offset));
+        FreeRTOS_printf_wrapper_double("offset: ", offset);
 
         delay = max(LFP2D(subtract_uint64_t((subtract_uint64_t(r->dst, r->org)), (subtract_uint64_t(r->rec, r->xmt)))),
                     LOG2D(s.precision));
         disp = LOG2D(r->precision) + LOG2D(s.precision) + LFP2D(PHI * subtract_uint64_t(r->dst, r->org));
     }
-    double local_delay = (r->dst - r->org) - (r->xmt - r->rec);
-    p->rootdelay = r->rootdelay + delay;
-
-    // FreeRTOS_printf(("\n\ntesting\n"));
-    // FreeRTOS_printf_wrapper_double("", LFP2D(PHI * subtract_uint64_t(r->dst, r->org)));
-    // FreeRTOS_printf_wrapper_double(
-    //     "", LOG2D(r->precision) + LOG2D(s.precision) + LFP2D(PHI * subtract_uint64_t(r->dst, r->org)));
-    // FreeRTOS_printf(("\n\noffset\n"));
-    // FreeRTOS_printf_wrapper_double("", offset);
-    // FreeRTOS_printf(("\n\ndelay\n"));
-    // FreeRTOS_printf_wrapper_double("", delay);
-    // FreeRTOS_printf(("\n\ndisp\n"));
-    // FreeRTOS_printf_wrapper_double("", disp);
-    // FreeRTOS_printf(("\n\n\n lets see if offset is working: %d\n\n\n", offset)); // = 0
-    // FreeRTOS_printf(("\n\n\ndelay is %d\n\n\n", delay));
-    // FreeRTOS_printf(("\n\n\ndisp is %d\n\n\n", disp));
-    // FreeRTOS_printf(("I AM CALLING CLOCK_FILTER\n\n"));
+
     clock_filter(p, offset, delay, disp);
 }
 
-// A.5.2.  clock_filter()
-
-/*
- * clock_filter(p, offset, delay, dispersion) - select the best from the
- * latest eight delay/offset samples.
+/**
+ * This function implements the clock filter algorithm for NTP.
+ * It takes a new sample (offset, delay, dispersion) and updates the peer structure accordingly.
+ * The function also maintains a sorted list of the eight most recent samples and calculates the peer jitter.
+ *
+ * @param p - A pointer to the peer structure to be updated.
+ * @param offset - The clock offset of the new sample.
+ * @param delay - The roundtrip delay of the new sample.
+ * @param disp - The dispersion of the new sample.
  */
 void clock_filter(struct ntp_p *p, /* peer structure pointer */
                   double offset,   /* clock offset */
@@ -144,9 +126,8 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
      * place the (offset, delay, disp, time) in the vacated
      * rightmost tuple.
      */
-    gettime(1);  // update c.localTime to the current time
-    double tmpDisp = LFP2D(
-        PHI * (subtract_uint64_t(c.localTime, p->t)));  // this is a static value so it should only be calculated once
+    double tmpDisp =
+        LFP2D(PHI * (subtract_uint64_t(c.t, p->t)));  // this is a static value so it should only be calculated once
 
     for (i = 1; i < NSTAGE; i++)
     {
@@ -154,7 +135,7 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
         p->f[i].disp += tmpDisp;
         f[i] = p->f[i];
     }
-    p->f[0].t = c.localTime;
+    p->f[0].t = c.t;
     p->f[0].offset = offset;
     p->f[0].delay = delay;
     p->f[0].disp = disp;
@@ -175,7 +156,7 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
         unsigned int denominator = 1 << exp;
         double result = f[i].disp / denominator;
         p->disp += result;
-        p->jitter += SQUARE(f[i].offset - f[0].offset);
+        p->jitter += SQUARE(fabs(f[i].offset - f[0].offset));
     }
 
     p->jitter = max(sqrt(p->jitter), LOG2D(s.precision));
@@ -188,10 +169,7 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
 
     if (subtract_uint64_t(f[0].t, p->t) <= 0 && s.leap != NOSYNC)
     {
-        // FreeRTOS_printf_wrapper_double("", subtract_uint64_t(f[0].t, p->t));
-        // FreeRTOS_printf_wrapper_double("", f[0].t);
-        // FreeRTOS_printf_wrapper_double("", p->t);
-        // FreeRTOS_printf(("f[0].t - p->t <= 0\n"));
+        FreeRTOS_printf(("f[0].t - p->t <= 0, killing\n"));
         return;
     }
 
@@ -202,16 +180,15 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
      * less than twice the system poll interval, dump the spike.
      * Otherwise, and if not in a burst, shake out the truechimers.
      */
-    if (fabs(p->offset - dtemp) > SGATE * p->jitter && (f[0].t - p->t) < 2 * s.poll)
+    if (fabs(p->offset - dtemp) > SGATE * p->jitter && (subtract_uint64_t(f[0].t, p->t)) < 2 * s.poll)
     {
-        // FreeRTOS_printf(("Popcorn spike found\n"));
+        FreeRTOS_printf(("Popcorn spike found, killing\n"));
         return;
     }
 
     p->t = f[0].t;
     if (p->burst == 0)
     {
-        // FreeRTOS_printf(("p->burst == 0\n\n\n"));
         assoc_table_update(p);
         clock_select();
     }
@@ -219,8 +196,10 @@ void clock_filter(struct ntp_p *p, /* peer structure pointer */
     return;
 }
 
-/*
- * fit() - test if association p is acceptable for synchronization
+/**
+ * This function checks if a given NTP peer is acceptable for synchronization.
+ *
+ * @param p - A pointer to the peer structure to be checked.
  */
 int fit(struct ntp_p *p /* peer structure pointer */
 )
@@ -236,7 +215,6 @@ int fit(struct ntp_p *p /* peer structure pointer */
      * distance threshold plus an increment equal to one poll
      * interval.
      */
-
     if (root_dist(p) > MAXDIST + PHI * (double)LOG2D(s.poll)) return (FALSE);
 
     /*
@@ -262,9 +240,12 @@ int fit(struct ntp_p *p /* peer structure pointer */
     return (TRUE);
 }
 
-/*
+/**
  * clear() - reinitialize for persistent association, demobilize
  * for ephemeral association.
+ *
+ * @param p - A pointer to the peer structure to be cleared.
+ * @param kiss - The kiss code indicating the reason for clearing the peer.
  */
 void clear(struct ntp_p *p, /* peer structure pointer */
            int kiss         /* kiss code */
@@ -306,12 +287,10 @@ void clear(struct ntp_p *p, /* peer structure pointer */
      * clients have just been stirred up after a long absence of the
      * broadcast server.
      */
-    p->outdate = p->t = c.localTime;
+    p->outdate = p->t = c.t;
     p->nextdate = p->outdate + (random() & ((1 << MINPOLL) - 1));
 }
 
-// A.5.3.  fast_xmit()
-
 /*
  * fast_xmit() - transmit a reply packet for receive packet r
  */
@@ -354,6 +333,7 @@ void fast_xmit(struct ntp_r *r, /* receive packet pointer */
      * MAC.  Use the key ID in the received packet and the key in
      * the local key cache.
      */
+    FreeRTOS_printf(("fast_xmit: auth = %d\n", auth));
     if (auth != A_NONE)
     {
         if (auth == A_CRYPTO)
@@ -369,8 +349,6 @@ void fast_xmit(struct ntp_r *r, /* receive packet pointer */
     xmit_packet(&x);
 }
 
-// A.5.4.  access()
-
 /*
  * access() - determine access restrictions
  */
@@ -544,11 +522,8 @@ void receive(struct ntp_r *r /* receive packet pointer */
             if (!(s.flags & S_BCSTENAB)) return; /* broadcast not enabled */
 
             p = mobilize(r->srcaddr, r->dstaddr, r->version, M_BCLN, r->keyid, P_EPHEM);
-            break; /* processing continues *
+            break; /* processing continues */
 
-/*
-* Process packet.  Placeholdler only.
-*/
         case PROC:
             // p = mobilize(r->srcaddr, r->dstaddr, r->version, M_SERV,
             //              r->keyid, P_EPHEM); // TODO //
@@ -603,13 +578,13 @@ void receive(struct ntp_r *r /* receive packet pointer */
     {
         if ((rOrgInSeconds == 0) && (rOrgFrac == 0))
         {
-            // FreeRTOS_printf(("rOrgInSeconds == 0\n"));
+            FreeRTOS_printf(("rOrgInSeconds == 0\n"));
             synch = FALSE; /* unsynchronized */
         }
 
         else if ((rOrgInSeconds != pXmtInSeconds) && (rOrgFrac != pXmtFrac))
         {
-            // FreeRTOS_printf(("rOrgInSeconds != pXmtInSeconds\n"));
+            FreeRTOS_printf(("rOrgInSeconds != pXmtInSeconds\n"));
             synch = FALSE; /* bogus packet */
         }
     }
diff --git a/src/NTP_poll.c b/src/NTP_poll.c
index 44c632a..a81b9a9 100644
--- a/src/NTP_poll.c
+++ b/src/NTP_poll.c
@@ -1,12 +1,16 @@
-
 #include "NTP_poll.h"
 
 #include "NTPTask.h"
 #include "NTP_main_utility.h"
 #include "NTP_peer.h"
-// A.5.7.1.  poll()
-/*
- * poll() - determine when to send a packet for association p->
+
+/**
+ * @brief Executes the poll routine for a given NTP peer.
+ *
+ * This routine is called when the current time catches up to the next poll time for the peer.
+ * It updates the next execution time for the peer and performs the necessary actions based on the peer's mode.
+ *
+ * @param p Pointer to the NTP peer structure.
  */
 void poll(struct ntp_p *p)  // peer structure pointer
 {
@@ -39,6 +43,7 @@ void poll(struct ntp_p *p)  // peer structure pointer
      */
     if (p->hmode == M_CLNT && p->flags & P_MANY)
     {
+        printf("\n\n\npoll phmode\n\n\n");
         p->outdate = c.t;
         if (p->unreach > BEACON)
         {
@@ -57,6 +62,7 @@ void poll(struct ntp_p *p)  // peer structure pointer
     }
     if (p->burst == 0)
     {
+        printf("\n\n\npoll 0\n\n\n");
         /*
          * We are not in a burst.  Shift the reachability
          * register to the left.  Hopefully, some time before
@@ -66,9 +72,14 @@ void poll(struct ntp_p *p)  // peer structure pointer
         oreach = p->reach;
         p->outdate = c.t;
         p->reach = p->reach << 1;
-        if (!(p->reach & 0x7)) clock_filter(p, 0, 0, MAXDISP);
+        if (!(p->reach & 0x7))
+        {
+            printf("\n\n\npoll 1\n\n\n");
+            clock_filter(p, 0, 0, MAXDISP);
+        }
         if (!p->reach)
         {
+            printf("\n\n\npoll 2\n\n\n");
             /*
              * The server is unreachable, so bump the
              * unreach counter.  If the unreach threshold
@@ -89,6 +100,7 @@ void poll(struct ntp_p *p)  // peer structure pointer
         }
         else
         {
+            printf("\n\n\npoll 3\n\n\n");
             /*
              * The server is reachable.  Set the poll
              * interval to the system poll interval.  Send a
@@ -101,6 +113,7 @@ void poll(struct ntp_p *p)  // peer structure pointer
     }
     else
     {
+        printf("\n\n\npoll 4\n\n\n");
         /*
          * If in a burst, count it down.  When the reply comes
          * back the clock_filter() routine will call
@@ -108,17 +121,20 @@ void poll(struct ntp_p *p)  // peer structure pointer
          */
         p->burst--;
     }
+    printf("\n\n\npoll 5\n\n\n");
     /*
      * Do not transmit if in broadcast client mode.
      */
     if (p->hmode != M_BCLN) peer_xmit(p);
     poll_update(p, hpoll);
+    printf("\n\n\npoll end\n\n\n");
 }
 
-// A.5.7.2.  poll_update()
-
-/*
- * poll_update() - update the poll interval for association p
+/**
+ * Updates the poll interval for a given NTP peer.
+ *
+ * @param p - Pointer to the peer structure.
+ * @param poll - The poll interval in log2 seconds.
  *
  * Note: This routine is called by both the packet() and poll() routine.
  * Since the packet() routine is executed when a network packet arrives
@@ -129,6 +145,8 @@ void poll(struct ntp_p *p)  // peer structure pointer
 void poll_update(struct ntp_p *p, /* peer structure pointer */
                  int poll)        /* poll interval (log2 s) */
 {
+    static double last_nextdate = 0; /* keep track of the last assigned nextdate */
+
     /*
      * This routine is called by both the poll() and packet()
      * routines to determine the next poll time.  If within a burst
@@ -155,28 +173,40 @@ void poll_update(struct ntp_p *p, /* peer structure pointer */
         p->nextdate = p->outdate + (1 << max(min(p->ppoll, p->hpoll), MINPOLL));
     }
 
+    /* Ensure nextdate is at least 5 seconds later than the last one */
+    if (p->nextdate <= p->nextdate + 5 && last_nextdate <= (5 * (NMAX)))
+    {
+        p->nextdate += last_nextdate;
+        last_nextdate += 5;
+    }
+
+    /* Update last_nextdate */
+
     /*
      * It might happen that the due time has already passed.  If so,
      * make it one second in the future.
      */
-    if (p->nextdate <= c.t) p->nextdate = c.t + 1;
+    if (p->nextdate <= c.t) p->nextdate = add_int64_t(c.t, 1);
+    // FreeRTOS_printf_wrapper_double("poll_update: p->nextdate: ", p->nextdate);
+    // printf("\n\n\npoll_update end\n\n\n");
 }
 
-// A.5.7.3.  peer_xmit()
-
-/*
- * transmit() - transmit a packet for association p
+/**
+ * Transmits an NTP packet to a peer.
+ *
+ * @param p Pointer to the peer structure.
  */
-void peer_xmit(struct ntp_p *p  // peer structure pointer)
-)
+void peer_xmit(struct ntp_p *p)
 {
     struct ntp_x x; /* transmit packet */
 
     /*
      * Initialize header and transmit timestamp
      */
-    x.srcaddr = p->dstaddr;
-    x.dstaddr = p->srcaddr;
+    NTP1_server_IP = p->srcaddr;
+
+    x.srcaddr = p->srcaddr;
+    x.dstaddr = p->dstaddr;
     x.leap = s.leap;
     x.version = p->version;
     x.mode = p->hmode;
@@ -192,10 +222,13 @@ void peer_xmit(struct ntp_p *p  // peer structure pointer)
     x.reftime = s.reftime;
     x.org = p->org;
     x.rec = p->rec;
-    // gettime(0);
+    gettime(0);
     x.xmt = c.t;
     p->xmt = x.xmt;
 
+    FreeRTOS_printf(("\n\nSending to : %lu.%lu.%lu.%lu\n\n", (x.srcaddr & 0xFF), ((x.srcaddr >> 8) & 0xFF),
+                     ((x.srcaddr >> 16) & 0xFF), ((x.srcaddr >> 24) & 0xFF)));
+
     /*
      * If the key ID is nonzero, send a valid MAC using the key ID
      * of the association and the key in the local key cache.  If
@@ -203,12 +236,12 @@ void peer_xmit(struct ntp_p *p  // peer structure pointer)
      * packet; just reset the association and stop until the problem
      * is fixed.
      */
-    if (p->keyid)
-        if (/* p->keyid invalid */ 0)
-        {
-            clear(p, X_NKEY);
-            return;
-        }
-    x.dgst = md5(p->keyid);
+    // if (p->keyid)
+    //     if (/* p->keyid invalid */ 0)
+    //     {
+    //         clear(p, X_NKEY);
+    //         return;
+    //     }
+    // x.dgst = md5(p->keyid);
     xmit_packet(&x);
 }
\ No newline at end of file
diff --git a/src/NTP_system_process.c b/src/NTP_system_process.c
index 23a5871..642bce4 100644
--- a/src/NTP_system_process.c
+++ b/src/NTP_system_process.c
@@ -4,10 +4,16 @@
 #include "NTP_peer.h"
 #include "NTP_vfo.h"
 
-// A.5.5.  System Process
-
-// A.5.5.1.  clock_select()
-// Comparison function to sort ntp_m by edge value (ascending)
+/**
+ * This function is a comparator for the qsort function. It compares two ntp_m structures.
+ *
+ * @param a - The first void pointer that will be cast to an ntp_m structure.
+ * @param b - The second void pointer that will be cast to an ntp_m structure.
+ *
+ * @return - It returns -1 if the edge of the first structure is less than the edge of the second,
+ *           1 if the edge of the first structure is greater than the edge of the second,
+ *           and 0 if the edges of both structures are equal.
+ */
 int compare_ntp_m(const void *a, const void *b)
 {
     struct ntp_m *elem1 = (struct ntp_m *)a;
@@ -18,67 +24,59 @@ int compare_ntp_m(const void *a, const void *b)
     return 0;
 }
 
+/**
+ * This function is used to cull the falsetickers from the server population, leaving only the truechimers.
+ * The correctness interval for association p is the interval from offset - root_dist() to offset + root_dist().
+ * The function constructs a chime list of tuples (p, type, edge) and sorts the list by edge from lowest to highest.
+ *
+ * @param n - The pointer to an integer that will be incremented by 3 for each peer that passes the fit function.
+ */
 void cull(int *n)
 {
+    int i = 0;
     struct ntp_p *p;
-    int idx = 0;
 
-    /*
-     * We first cull the falsetickers from the server population,
-     * leaving only the truechimers.  The correctness interval for
-     * association p is the interval from offset - root_dist() to
-     * offset + root_dist().  The object of the game is to find a
-     * majority clique; that is, an intersection of correctness
-     * intervals numbering more than half the server population.
-     *
-     * First, construct the chime list of tuples (p, type, edge) as
-     * shown below, then sort the list by edge from lowest to
-     * highest.
-     */
-
-    struct ntp_m m1[NMAX], m2[NMAX], m3[NMAX];
-    memset(m1, 0, sizeof(m1));
-    memset(m2, 0, sizeof(m2));
-    memset(m3, 0, sizeof(m3));
+    FreeRTOS_printf(("\n\ncull\n\n"));
 
     for (int index = 0; index < NMAX; index++)
     {
         p = assoc_table->peers[index];
         if (fit(p))
         {
-            m1[idx].p = p;
-            m1[idx].type = -1;
-            m1[idx].edge = p->offset - root_dist(p);
-
-            m2[idx].p = p;
-            m2[idx].type = 0;
-            m2[idx].edge = p->offset;
-
-            m3[idx].p = p;
-            m3[idx].type = +1;
-            m3[idx].edge = p->offset + root_dist(p);
-
-            *n += 3;
-            idx++;
+            s.m[i].p = p;
+            s.m[i].type = -1;
+            s.m[i].edge = p->offset - root_dist(p);
+            i++;
+
+            s.m[i].p = p;
+            s.m[i].type = 0;
+            s.m[i].edge = p->offset;
+            i++;
+
+            s.m[i].p = p;
+            s.m[i].type = +1;
+            s.m[i].edge = p->offset + root_dist(p);
+            i++;
         }
     }
 
-    qsort(m1, idx, sizeof(struct ntp_m), compare_ntp_m);
-    qsort(m2, idx, sizeof(struct ntp_m), compare_ntp_m);
-    qsort(m3, idx, sizeof(struct ntp_m), compare_ntp_m);
+    qsort(&s.m, i, sizeof(struct ntp_m), compare_ntp_m);
 
-    for (int i = 0; i < idx; i++)
-    {
-        s.m[i] = m1[i];
-        s.m[i + idx] = m2[i];
-        s.m[i + idx * 2] = m3[i];
-    }
-    for (int i = 0; i < *n; i++)
+    FreeRTOS_printf(("\n\nadded\n\n"));
+    for (int index = 0; index < i; index++)
     {
-        FreeRTOS_printf_wrapper_double("", s.m[i].edge);
+        FreeRTOS_printf_wrapper_double("\n\n%s\n\n", s.m[index].edge);
     }
+    *n = i;
 }
 
+/**
+ * Finds the largest contiguous intersection of correctness intervals.
+ *
+ * @param n The number of correctness intervals.
+ * @param low Pointer to store the lower endpoint of the intersection.
+ * @param high Pointer to store the higher endpoint of the intersection.
+ */
 void intersection(int n, double *low, double *high)
 {
     /*
@@ -141,8 +139,14 @@ void intersection(int n, double *low, double *high)
     }
 }
 
-/*
- * clock_select() - find the best clocks
+/**
+ * Selects the best clock from the list of survivors based on the clustering algorithm.
+ * The survivors are selected based on their correctness interval extents and clustering metrics.
+ * At least NSANE survivors are required to satisfy the correctness assertions.
+ * For each association, the selection jitter is calculated as the square root of the sum of squares
+ * of the offset differences between the associations.
+ * The function continues to discard the survivor with maximum selection jitter until a termination condition is met.
+ * Finally, the best clock is picked based on the old system peer and the first survivor on the list.
  */
 void clock_select()
 {
@@ -158,8 +162,6 @@ void clock_select()
 
     intersection(n, &low, &high);
 
-    // n = 0;
-
     /*
      * Clustering algorithm.  Construct a list of survivors (p,
      * metric) from the chime list, where metric is dominated first
@@ -180,8 +182,8 @@ void clock_select()
         s.v[s.n].p = p;
         s.v[s.n].metric = (double)(MAXDIST * p->stratum) + root_dist(p);
         s.n++;
-        FreeRTOS_printf(("\n\nsurvivor found\n"));
-        FreeRTOS_printf_wrapper_double("", s.m[i].edge);
+        // FreeRTOS_printf(("\n\nsurvivor found\n"));
+        // FreeRTOS_printf_wrapper_double("", s.m[i].edge);
     }
 
     /*
@@ -195,7 +197,6 @@ void clock_select()
         FreeRTOS_printf(("nsane survivors dead\n"));
         return;
     }
-    FreeRTOS_printf(("nsane survivors not dead\n"));
 
     /*
      * For each association p in turn, calculate the selection
@@ -264,14 +265,24 @@ void clock_select()
         s.p = s.v[0].p;
     }
 
-    memset(s.m, 0, sizeof(s.m));  // Clear temporary response data
+    // memset(s.m, 0, sizeof(s.m));  // Clear temporary response data
 
     // FreeRTOS_printf(("calling clock update\n"));
     clock_update(s.p);
 }
 
-/*
- * root_dist() - calculate root distance
+/**
+ * This function calculates the root synchronization distance for a given peer.
+ * The root synchronization distance is the maximum error due to all causes of the local clock relative to the primary
+ * server.
+ *
+ * @param p - A pointer to the peer structure for which the root synchronization distance is to be calculated.
+ *
+ * @return - The function returns the root synchronization distance, which is calculated as half the total delay plus
+ * total dispersion plus peer jitter. The total delay is the maximum of MINDISP and the sum of the root delay and delay
+ * for the peer. The total dispersion is the sum of the root dispersion and dispersion for the peer. The peer jitter is
+ * added as is. Finally, a term proportional to the product of the constant PHI and the difference between the current
+ * time and the peer's time is added.
  */
 double root_dist(struct ntp_p *p /* peer structure pointer */
 )
@@ -282,12 +293,11 @@ double root_dist(struct ntp_p *p /* peer structure pointer */
      * It is defined as half the total delay plus total dispersion
      * plus peer jitter.
      */
+
     return (max(MINDISP, p->rootdelay + p->delay) / 2 + p->rootdisp + p->disp +
             PHI * LFP2D(subtract_uint64_t(c.t, p->t)) + p->jitter);
 }
 
-// A.5.5.3.  accept()
-
 /*
  * accept() - test if association p is acceptable for synchronization
  */
@@ -330,8 +340,6 @@ int accept(struct ntp_p *p /* peer structure pointer */
     return (TRUE);
 }
 
-// A.5.5.4.  clock_update()
-
 /*
  * clock_update() - update the system clock
  */
@@ -345,12 +353,11 @@ void clock_update(struct ntp_p *p /* peer structure pointer */
      * system peer change, avoid it.  We never use an old sample or
      * the same sample twice.
      */
-    // FreeRTOS_printf(("CLOCK UPDATE\n"));
-    if (s.t >= p->t)
+    FreeRTOS_printf_wrapper_double("s.t: ", s.t);
+    FreeRTOS_printf_wrapper_double("p->t: ", p->t);
+    if (s.t > p->t)
     {
-        // FreeRTOS_printf_wrapper_double("", s.t);
-        // FreeRTOS_printf_wrapper_double("", p->t);
-        // FreeRTOS_printf(("s.t >= p->t, kill\n"));
+        FreeRTOS_printf(("s.t > p->t, kill\n"));
         return;
     }
     /*
@@ -360,6 +367,8 @@ void clock_update(struct ntp_p *p /* peer structure pointer */
     s.t = p->t;
     clock_combine();
 
+    // FreeRTOS_printf(("calling local\n"));
+    // FreeRTOS_printf_wrapper_double("", s.offset);
     switch (local_clock(p, s.offset))
     {
         /*
@@ -415,10 +424,10 @@ void clock_update(struct ntp_p *p /* peer structure pointer */
     }
 }
 
-// A.5.5.5.  clock_combine()
-
-/*
- * clock_combine() - combine offsets
+/**
+ * This function combines the offsets of the survivors from the clustering algorithm using a weighted average.
+ * The weight is determined by the root distance. It also computes the selection jitter as the weighted RMS difference
+ * between the first survivor and the remaining survivors.
  */
 void clock_combine()
 {
@@ -445,8 +454,15 @@ void clock_combine()
         x = root_dist(p);
         y += 1 / x;
         z += p->offset / x;
-        w += SQUARE(p->offset - s.v[0].p->offset) / x;
+        w += SQUARE(fabs(p->offset - s.v[0].p->offset)) / x;
+        printf("combine: %d\n\n\n\n\n\n\n", i);
+        FreeRTOS_printf_wrapper_double("combine: p->offset: ", z);
+        FreeRTOS_printf_wrapper_double("combine: p->offset: ", p->offset);
+        FreeRTOS_printf_wrapper_double("combine: p->offset: ", x);
     }
+    FreeRTOS_printf_wrapper_double("combine: p->offset: ", z);
+    FreeRTOS_printf_wrapper_double("combine: p->offset: ", y);
+    FreeRTOS_printf_wrapper_double("combine: p->offset: ", z / y);
     s.offset = z / y;
     s.jitter = SQRT(w / y);
 }
\ No newline at end of file
diff --git a/src/NTP_vfo.c b/src/NTP_vfo.c
index 357d86a..dce5243 100644
--- a/src/NTP_vfo.c
+++ b/src/NTP_vfo.c
@@ -5,8 +5,12 @@
 #include "NTP_main_utility.h"
 #include "NTP_poll.h"
 
-/*
- * local_clock() - discipline the local clock
+/**
+ * local_clock - Discipline the local clock based on offset and state.
+ *
+ * @p: Pointer to the peer structure.
+ * @offset: Clock offset from combine().
+ * @return: Result code indicating the adjustment status.
  */
 int                          /* return code */
 local_clock(struct ntp_p *p, /* peer structure pointer */
@@ -42,7 +46,7 @@ local_clock(struct ntp_p *p, /* peer structure pointer */
     FreeRTOS_printf_wrapper_double("", fabs(offset));
     if (fabs(offset) > STEPT)
     {
-        // FreeRTOS_printf(("fabs(offset) > STEPT\n"));
+        FreeRTOS_printf(("fabs(offset) > STEPT\n"));
         switch (c.state)
         {
             /*
@@ -121,17 +125,23 @@ local_clock(struct ntp_p *p, /* peer structure pointer */
     }
     else
     {
-        // FreeRTOS_printf(("fabs(offset) <= STEPT\n"));
+        FreeRTOS_printf(("fabs(offset) <= STEPT\n"));
         /*
          * Compute the clock jitter as the RMS of exponentially
          * weighted offset differences.  This is used by the
          * poll-adjust code.
          */
         etemp = SQUARE(c.jitter);
+        // FreeRTOS_printf(("c.state = %d\n", c.state));
         dtemp = SQUARE(max(fabs(offset - c.last), LOG2D(s.precision)));
-        c.jitter = SQRT(etemp + (dtemp - etemp) / AVG);
-
         // FreeRTOS_printf(("c.state = %d\n", c.state));
+        // FreeRTOS_printf_wrapper_double("etemp", offset);
+        // FreeRTOS_printf_wrapper_double("etemp", c.last);
+        // FreeRTOS_printf_wrapper_double("etemp", etemp);
+        // FreeRTOS_printf_wrapper_double("dtemp", dtemp);
+        c.jitter = SQRT((etemp + (dtemp - etemp)) / AVG);
+
+        FreeRTOS_printf(("c.state = %d\n", c.state));
         switch (c.state)
         {
             /*
@@ -248,36 +258,37 @@ local_clock(struct ntp_p *p, /* peer structure pointer */
     return (rval);
 }
 
-// A.5.5.7.  rstclock()
-
-/*
- * rstclock() - clock state machine
+/**
+ * rstclock - Update the clock state machine.
+ *
+ * @state: New state for the clock.
+ * @offset: New offset value.
+ * @t: New update time.
  */
-void rstclock(int state,     /* new state */
-              double offset, /* new offset */
-              double t       /* new update time */
+void rstclock(int state,    /* new state */
+              tstamp t,     /* new update time */
+              double offset /* new offset */
 )
 {
-    // FreeRTOS_printf(("RSTCLOCK\n"));
     /*
      * Enter new state and set state variables.  Note, we use the
      * time of the last clock filter sample, which must be earlier
      * than the current time.
      */
     c.state = state;
+    FreeRTOS_printf(("c.state = %d\n", c.state));
+    FreeRTOS_printf_wrapper_double("offset", offset);
+    FreeRTOS_printf_wrapper_double("t: \n", t);
     c.last = c.offset = offset;
     s.t = t;
-    // FreeRTOS_printf(("c.state = %d\n", c.state));
+    c.t++;
 }
 
-// A.5.6.  Clock Adjust Process
-
-// A.5.6.1.  clock_adjust()
-
-/*
- * clock_adjust() - runs at one-second intervals
+/**
+ * clock_adjust - Periodic clock adjustment function.
+ * Runs at one-second intervals to adjust the local clock.
  */
-void clock_adjust()
+void clock_adjust(ntp_r *r)
 {
     double dtemp;
 
@@ -300,7 +311,13 @@ void clock_adjust()
      * at the longer poll intervals.
      */
     dtemp = c.offset / (PLL * min(LOG2D(s.poll), ALLAN));
+    FreeRTOS_printf(("coffset \n"));
+    FreeRTOS_printf_wrapper_double("c.offset", c.offset);
+    FreeRTOS_printf_wrapper_double("PLL * min(LOG2D(s.poll), ALLAN)", PLL * min(LOG2D(s.poll), ALLAN));
     c.offset -= dtemp;
+    FreeRTOS_printf_wrapper_double("c.offset", dtemp);
+    FreeRTOS_printf_wrapper_double("c.offset", c.offset);
+    FreeRTOS_printf_wrapper_double("c.offset", c.freq + dtemp);
 
     /*
      * This is the kernel adjust time function, usually implemented
@@ -312,11 +329,25 @@ void clock_adjust()
      * Peer timer.  Call the poll() routine when the poll timer
      * expires.
      */
-    while (/* all associations */ 0)
+
+    // Check for duplicate peers
+    for (int i = 0; i < NMAX; i++)
     {
-        struct ntp_p *p; /* dummy peer structure pointer */
+        ntp_p *assoc = assoc_table->peers[i];
+
+        uint32_t c_t_low = (uint32_t)(c.t & 0xFFFFFFFF);
 
-        if (c.t >= p->nextdate) poll(p);
+        // FreeRTOS_printf(("peer %d\n", i));
+        // FreeRTOS_printf_wrapper_double("c_t_low", c_t_low);
+        // FreeRTOS_printf_wrapper_double("assoc->nextdate", assoc->nextdate);
+
+        if (c_t_low >= assoc->nextdate)
+        {
+            FreeRTOS_printf(("Polling peer %d\n", i));
+            poll(assoc);
+            recv_packet(r);  // Receive response
+            receive(r);
+        }
     }
 
     /*
diff --git a/src/NTP_vfo.h b/src/NTP_vfo.h
index 2c60942..90a0b59 100644
--- a/src/NTP_vfo.h
+++ b/src/NTP_vfo.h
@@ -19,7 +19,7 @@
 #define PGATE 4         /* poll-adjust gate */
 
 int local_clock(struct ntp_p *, double); /* clock discipline */
-void rstclock(int, double, double);      /* clock state transition */
-void clock_adjust();                     /* one-second timer process */
+void rstclock(int, tstamp, double);      /* clock state transition */
+void clock_adjust(ntp_r *);              /* one-second timer process */
 
 #endif /* NTP_VFO_H */
\ No newline at end of file