diff --git a/src/UDPC_Defines.h b/src/UDPC_Defines.h
index 83c1b30..63a6e5a 100644
--- a/src/UDPC_Defines.h
+++ b/src/UDPC_Defines.h
@@ -31,4 +31,20 @@ static const char *UDPC_ERR_MTXFAIL_STR = "Failed to create mutex";
 static const char *UDPC_ERR_CVFAIL_STR = "Failed to create condition variable";
 static const char *UDPC_ERR_THREADFAIL_STR = "Failed to create thread";
 
+#define UDPC_CD_AMOUNT 32
+
+#define UDPC_GOOD_MODE_SEND_INTERVAL (1.0f/30.0f)
+#define UDPC_BAD_MODE_SEND_INTERVAL (1.0f/10.0f)
+#define UDPC_TIMEOUT_SECONDS 10.0f
+#define UDPC_HEARTBEAT_PKT_INTERVAL (15.0f/100.0f)
+#define UDPC_PKT_PROTOCOL_ID 1357924680
+
+#define UDPC_ID_CONNECT        0x80000000
+#define UDPC_ID_PING           0x40000000
+#define UDPC_ID_NO_REC_CHK     0x20000000
+#define UDPC_ID_RESENDING      0x10000000
+
+#define UDPC_SENT_PKTS_MAX_SIZE 34
+#define UDPC_SENT_PKTS_ALLOC_SIZE 35
+
 #endif
diff --git a/src/UDPConnection.c b/src/UDPConnection.c
index 6f239a1..4debe47 100644
--- a/src/UDPConnection.c
+++ b/src/UDPConnection.c
@@ -1,6 +1,7 @@
 #include "UDPConnection.h"
 
 #include <stdlib.h>
+#include <string.h>
 
 UDPC_Context* UDPC_init(uint16_t listenPort, int isClient)
 {
@@ -171,16 +172,222 @@ const char* UDPC_get_error_str(uint32_t error)
 void UDPC_update(UDPC_Context *ctx)
 {
     // get dt
-    struct timespec ts;
-    timespec_get(&ts, TIME_UTC);
-    float dt = UDPC_ts_diff_to_seconds(&ts, &ctx->lastUpdated);
-    ctx->lastUpdated = ts;
+    struct timespec tsNow;
+    timespec_get(&tsNow, TIME_UTC);
+    float dt = UDPC_ts_diff_to_seconds(&tsNow, &ctx->lastUpdated);
+    ctx->lastUpdated = tsNow;
 
-    // check rtt
+    // check timed-out/rtt/send-interval
+    UDPC_INTERNAL_ConnectionData *cd;
+    UDPC_Deque *removedQueue = UDPC_Deque_init(
+        ctx->connected->alloc_size / sizeof(UDPC_INTERNAL_ConnectionData) * sizeof(int));
     for(int x = 0; x * sizeof(UDPC_INTERNAL_ConnectionData) < ctx->connected->size; ++x)
     {
-        // TODO after fixing Deque
+        cd = UDPC_Deque_index_ptr(ctx->connected, sizeof(UDPC_INTERNAL_ConnectionData), x);
+
+        // check if connected timed out
+        if(UDPC_ts_diff_to_seconds(&tsNow, &cd->received) >= UDPC_TIMEOUT_SECONDS)
+        {
+            UDPC_Deque_push_back(removedQueue, &x, sizeof(int));
+            // TODO log timed out connection
+            continue;
+        }
+
+        // check good/bad mode
+        cd->toggleTimer += dt;
+        cd->toggledTimer += dt;
+        if((cd->flags & 0x2) != 0 && (cd->flags & 0x4) == 0)
+        {
+            // good mode, bad rtt
+            // TODO log switching to bad mode
+            cd->flags = cd->flags & 0xFFFFFFFD;
+            if(cd->toggledTimer >= 10.0f)
+            {
+                cd->toggleT *= 2.0f;
+                if(cd->toggleT > 60.0f)
+                {
+                    cd->toggleT = 60.0f;
+                }
+            }
+            cd->toggledTimer = 0.0f;
+        }
+        else if((cd->flags & 0x2) != 0)
+        {
+            // good mode, good rtt
+            if(cd->toggleTimer >= 10.0f)
+            {
+                cd->toggleTimer = 0.0f;
+                cd->toggleT /= 2.0f;
+                if(cd->toggleT < 1.0f)
+                {
+                    cd->toggleT = 1.0f;
+                }
+            }
+        }
+        else if((cd->flags & 0x2) == 0 && (cd->flags & 0x4) != 0)
+        {
+            // bad mode, good rtt
+            if(cd->toggledTimer >= cd->toggleT)
+            {
+                cd->toggleTimer = 0.0f;
+                cd->toggledTimer = 0.0f;
+                // TODO log switching to good mode
+                cd->flags |= 0x2;
+            }
+        }
+        else
+        {
+            // bad mode, bad rtt
+            cd->toggledTimer = 0.0f;
+        }
+
+        // check send interval
+        cd->timer += dt;
+        if(cd->timer >= ((cd->flags & 0x2) != 0
+            ? UDPC_GOOD_MODE_SEND_INTERVAL : UDPC_BAD_MODE_SEND_INTERVAL))
+        {
+            cd->timer = 0.0f;
+            cd->flags |= 0x1;
+        }
+
     }
+    // remove timed out
+    for(int x = 0; x * sizeof(int) < removedQueue->size; ++x)
+    {
+        UDPC_Deque_remove(ctx->connected, sizeof(UDPC_INTERNAL_ConnectionData),
+            *((int*)UDPC_Deque_index_rev_ptr(removedQueue, sizeof(int), x)));
+    }
+    UDPC_Deque_destroy(removedQueue);
+
+    // check triggerSend to send packets to connected
+    for(int x = 0; x * sizeof(UDPC_INTERNAL_ConnectionData) < ctx->connected->size; ++x)
+    {
+        cd = UDPC_Deque_index_ptr(ctx->connected, sizeof(UDPC_INTERNAL_ConnectionData), x);
+        if((cd->flags & 0x1) != 0)
+        {
+            cd->flags &= 0xFFFFFFFE;
+            if(cd->sendPktQueue->size == 0)
+            {
+                // send packet queue is empty, send heartbeat packet
+                if(UDPC_ts_diff_to_seconds(&tsNow, &cd->sent) < UDPC_HEARTBEAT_PKT_INTERVAL)
+                {
+                    continue;
+                }
+
+                char *data = malloc(20);
+                UDPC_INTERNAL_prepare_pkt(data, cd->id, cd->rseq, cd->ack, &cd->lseq, cd->addr, 0);
+
+                struct sockaddr_in destinationInfo;
+                destinationInfo.sin_family = AF_INET;
+                destinationInfo.sin_addr.s_addr = htonl(cd->addr);
+                destinationInfo.sin_port = htons(cd->port);
+                long int sentBytes = sendto(
+                    ctx->socketHandle,
+                    data,
+                    20,
+                    0,
+                    (struct sockaddr*) &destinationInfo,
+                    sizeof(struct sockaddr_in));
+                if(sentBytes != 20)
+                {
+                    // TODO log fail send packet
+                }
+                else
+                {
+                    UDPC_INTERNAL_PacketInfo sentInfo = {
+                        cd->addr,
+                        cd->lseq - 1,
+                        0,
+                        NULL,
+                        0,
+                        tsNow
+                    };
+                    UDPC_Deque_push_back(
+                        cd->sentPkts, &sentInfo, sizeof(UDPC_INTERNAL_PacketInfo));
+                    while(cd->sentPkts->size / sizeof(UDPC_INTERNAL_PacketInfo)
+                        > UDPC_SENT_PKTS_MAX_SIZE)
+                    {
+                        UDPC_INTERNAL_PacketInfo *pinfo = UDPC_Deque_get_front_ptr(cd->sentPkts, sizeof(UDPC_INTERNAL_PacketInfo));
+                        if(pinfo->data && pinfo->size != 0)
+                        {
+                            free(pinfo->data);
+                        }
+                        UDPC_Deque_pop_front(cd->sentPkts, sizeof(UDPC_INTERNAL_PacketInfo));
+                    }
+                }
+                free(data);
+            }
+            else // sendPktQueue not empty
+            {
+                UDPC_INTERNAL_PacketInfo *pinfo = UDPC_Deque_get_front_ptr(
+                    cd->sendPktQueue, sizeof(UDPC_INTERNAL_PacketInfo));
+                char *data = malloc(20 + pinfo->size);
+                UDPC_INTERNAL_prepare_pkt(data, cd->id, cd->rseq, cd->ack, &cd->lseq, cd->addr, ((pinfo->flags & 0x3) << 1));
+                memcpy(&data[20], pinfo->data, pinfo->size);
+
+                struct sockaddr_in destinationInfo;
+                destinationInfo.sin_family = AF_INET;
+                destinationInfo.sin_addr.s_addr = htonl(cd->addr);
+                destinationInfo.sin_port = htons(cd->port);
+                long int sentBytes = sendto(
+                    ctx->socketHandle,
+                    data,
+                    20 + pinfo->size,
+                    0,
+                    (struct sockaddr*) &destinationInfo,
+                    sizeof(struct sockaddr_in));
+                if(sentBytes != 20 + pinfo->size)
+                {
+                    // TODO log fail sent packet
+                }
+                else
+                {
+                    if((pinfo->flags & 0x2) != 0)
+                    {
+                        UDPC_INTERNAL_PacketInfo sentInfo = {
+                            cd->addr,
+                            cd->lseq - 1,
+                            pinfo->flags & 0x2,
+                            data,
+                            20 + pinfo->size,
+                            tsNow
+                        };
+                        UDPC_Deque_push_back(
+                            cd->sentPkts, &sentInfo, sizeof(UDPC_INTERNAL_PacketInfo));
+                    }
+                    else
+                    {
+                        UDPC_INTERNAL_PacketInfo sentInfo = {
+                            cd->addr,
+                            cd->lseq - 1,
+                            0,
+                            NULL,
+                            0,
+                            tsNow
+                        };
+                        UDPC_Deque_push_back(
+                            cd->sentPkts, &sentInfo, sizeof(UDPC_INTERNAL_PacketInfo));
+                    }
+
+                    while(cd->sentPkts->size / sizeof(UDPC_INTERNAL_PacketInfo)
+                        > UDPC_SENT_PKTS_MAX_SIZE)
+                    {
+                        UDPC_INTERNAL_PacketInfo *pinfoCached = UDPC_Deque_get_front_ptr(cd->sentPkts, sizeof(UDPC_INTERNAL_PacketInfo));
+                        if(pinfoCached->data && pinfoCached->size != 0)
+                        {
+                            free(pinfoCached->data);
+                        }
+                        UDPC_Deque_pop_front(cd->sentPkts, sizeof(UDPC_INTERNAL_PacketInfo));
+                    }
+                }
+                free(pinfo->data);
+                UDPC_Deque_pop_front(cd->sendPktQueue, sizeof(UDPC_INTERNAL_PacketInfo));
+            }
+        }
+    }
+
+    // receive packet
+    // TODO
 }
 
 float UDPC_ts_diff_to_seconds(struct timespec *ts0, struct timespec *ts1)
@@ -259,3 +466,41 @@ int UDPC_INTERNAL_threadfn(void *context)
 
     return 0;
 }
+
+void UDPC_INTERNAL_prepare_pkt(
+    void *data,
+    uint32_t conID,
+    uint32_t rseq,
+    uint32_t ack,
+    uint32_t *seqID,
+    uint32_t addr,
+    int flags)
+{
+    char *d = data;
+    uint32_t temp;
+
+    temp = htonl(UDPC_PKT_PROTOCOL_ID);
+    memcpy(d, &temp, 4);
+    if((flags & 0x4) == 0)
+    {
+        temp = htonl(conID | UDPC_ID_NO_REC_CHK);
+        memcpy(&d[4], &temp, 4);
+    }
+    else if((flags & 0x1) != 0)
+    {
+        temp = htonl(conID | UDPC_ID_PING);
+        memcpy(&d[4], &temp, 4);
+    }
+    else
+    {
+        temp = htonl(conID | ((flags & 0x2) != 0 ? UDPC_ID_RESENDING : 0));
+        memcpy(&d[4], &temp, 4);
+    }
+    temp = htonl(*seqID);
+    ++(*seqID);
+    memcpy(&d[8], &temp, 4);
+    temp = htonl(rseq);
+    memcpy(&d[12], &temp, 4);
+    temp = htonl(ack);
+    memcpy(&d[16], &temp, 4);
+}
diff --git a/src/UDPConnection.h b/src/UDPConnection.h
index d1ce977..d6ae457 100644
--- a/src/UDPConnection.h
+++ b/src/UDPConnection.h
@@ -24,7 +24,6 @@
   #define CleanupSocket(x) ((void)0)
 #endif
 
-#define UDPC_CD_AMOUNT 32
 
 /// This struct should not be used outside of this library
 typedef struct
@@ -33,11 +32,12 @@ typedef struct
     uint32_t id;
     /*
      * 0x1 - is resending
-     * 0x2 - is not received checked
+     * 0x2 - is check received packet
      * 0x4 - has been re-sent
      */
     uint32_t flags;
-    char *data;
+    char *data; // no-header in sendPktQueue, header in sentPkts
+    uint32_t size;
     struct timespec sent;
 } UDPC_INTERNAL_PacketInfo;
 
@@ -58,6 +58,7 @@ typedef struct
     float toggleT;
     float toggleTimer;
     float toggledTimer;
+    uint32_t addr;
     uint16_t port;
     UDPC_Deque *sentPkts;
     UDPC_Deque *sendPktQueue;
@@ -106,4 +107,18 @@ float UDPC_ts_diff_to_seconds(struct timespec *ts0, struct timespec *ts1);
 
 int UDPC_INTERNAL_threadfn(void *context); // internal usage only
 
+/*
+ * 0x1 - is ping
+ * 0x2 - is resending
+ * 0x4 - is checked received packet
+ */
+void UDPC_INTERNAL_prepare_pkt(
+    void *data,
+    uint32_t conID,
+    uint32_t rseq,
+    uint32_t ack,
+    uint32_t *seqID,
+    uint32_t addr,
+    int flags);
+
 #endif