if (ctx != NULL) {
        Serial.println(F("[wolfSSLSetup]: ctx not null, so freed"));

        wolfSSL_CTX_free(ctx); ctx = NULL;
    }

#include <Arduino.h>


// Type definitions
typedef unsigned int   word32;

struct timeval {
  long tv_sec;
  long tv_usec;
};

// Custom RNG Function
int  myRngFunc(byte* output, unsigned int sz)
{
    unsigned int i;
    randomSeed(analogRead(0));

    for (i = 0; i < sz; i++) {
        output[i] = random(0, 254);
    }

    return 0;
}


// Get Unix timestmap via NTP


// A UDP instance to let us send and receive packets over UDP
EthernetUDP Udp;
unsigned int localPort = 8888;       // local port to listen for UDP packets

//long int CURRENT_TIMESTAMP = 2208988800UL;


long int getUnixNTP()
{

    int repeat = 20;
    int res = 0;

    unsigned long epoch =  2208988800U;
    char timeServer[] = "time.nist.gov"; // time.nist.gov NTP server
    const int NTP_PACKET_SIZE = 48; // NTP time stamp is in the first 48 bytes of the message
    byte packetBuffer[ NTP_PACKET_SIZE]; //buffer to hold incoming and outgoing packets


    for (int i = 0; i < repeat; i++) {
        res = 0;

        res = Udp.begin(localPort);
        if (res == 0) {
            Serial.println("UDPE0");
            Udp.stop();
            return -1;
        }
    
    
    
    
    
        // Send an NTP Packet to the given address
    
        // set all bytes in the buffer to 0
        memset(packetBuffer, 0, NTP_PACKET_SIZE);
        // Initialize values needed to form NTP request
        // (see URL above for details on the packets)
        packetBuffer[0] = 0b11100011;   // LI, Version, Mode
        packetBuffer[1] = 0;     // Stratum, or type of clock
        packetBuffer[2] = 6;     // Polling Interval
        packetBuffer[3] = 0xEC;  // Peer Clock Precision
        // 8 bytes of zero for Root Delay & Root Dispersion
        packetBuffer[12]  = 49;
        packetBuffer[13]  = 0x4E;
        packetBuffer[14]  = 49;
        packetBuffer[15]  = 52;
    
        // all NTP fields have been given values, now
        // you can send a packet requesting a timestamp:
        Serial.println(F("before udp begin packet"));
        res = Udp.beginPacket(timeServer, 123); //NTP requests are to port 123
        if (res == 0) {
            Serial.println("UDPE1");
            Udp.stop();
            return -1;
        }
        Udp.write(packetBuffer, NTP_PACKET_SIZE);
        res = Udp.endPacket();
        if (res == 0) {
            Serial.println("UDPE2");
            Udp.stop();
            return -1;
        }
        Serial.println(F("afterx udp end"));
    
        // wait to see if a reply is available
        delay(1000);
    
        if ( Udp.parsePacket() ) {
            Serial.println(F("UDP1"));
    
            // We've received a packet, read the data from it
            Udp.read(packetBuffer, NTP_PACKET_SIZE); // read the packet into the buffer
            Serial.println(F("UDP2"));
    
            //the timestamp starts at byte 40 of the received packet and is four bytes,
            // or two words, long. First, esxtract the two words:
    
            unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
            unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
            Serial.println(F("UDP3"));
    
            // combine the four bytes (two words) into a long integer
            // this is NTP time (seconds since Jan 1 1900):
            unsigned long secsSince1900 = highWord << 16 | lowWord;
            Serial.print(F("Seconds since Jan 1 1900 = "));
            Serial.println(secsSince1900);
    
            // now convert NTP time into everyday time:
            Serial.print(F("Unix time = "));
            // Unix time starts on Jan 1 1970. In seconds, that's 2208988800:
            const unsigned long seventyYears = 2208988800UL;
            // subtract seventy years:
            epoch = secsSince1900 - seventyYears;
            // print Unix time:
            Serial.println(epoch);
    
            /*
                // print the hour, minute and second:
                Serial.print(F("The UTC time is "));       // UTC is the time at Greenwich Meridian (GMT)
                Serial.print((epoch  % 86400L) / 3600); // print the hour (86400 equals secs per day)
                Serial.print(':');
                if ( ((epoch % 3600) / 60) < 10 ) {
                // In the first 10 minutes of each hour, we'll want a leading '0'
                Serial.print('0');
                }
                Serial.print((epoch  % 3600) / 60); // print the minute (3600 equals secs per minute)
                Serial.print(':');
                if ( (epoch % 60) < 10 ) {
                // In the first 10 seconds of each minute, we'll want a leading '0'
                Serial.print('0');
                }
                Serial.println(epoch % 60); // print the second
            */
           Udp.stop();
           break;
        }
        Udp.stop();
        if (i == repeat - 1) {
            Serial.println("[ntp]: reached 20 err");
            return -1;
        }
    }

    return epoch;
}

int _gettimeofday( struct timeval *tv, void *tzvp)
{
  //uint64_t t = __your_system_time_function_here__();  // get uptime in nanoseconds
  //tv->tv_sec = t / 1000000000;  // convert to seconds
  //tv->tv_usec = ( t % 1000000000 ) / 1000;  // get remaining microseconds
  /*
  long mt = millis();
  tv->tv_sec = mt/1000;
  tv->tv_usec = mt*1000;
  */
  //long int mt = 1550041049;

  //long int mt = 1550633024 ;

    long int CURRENT_TIMESTAMP = 2208988800UL;

    CURRENT_TIMESTAMP = getUnixNTP();

    if (CURRENT_TIMESTAMP == -1) {
        return -1;
    }

    Serial.print("[_gettimeofday]: "); Serial.println(CURRENT_TIMESTAMP);
    tv->tv_sec = CURRENT_TIMESTAMP;
    tv->tv_usec = 0;
    return 0;  // return non-zero for error
} // end _gettimeofday()



// Custom logger
void wolfssl_custom_logging_cb(const int logLevel, const char *const logMessage) {
    Serial.print(F("lg ")); Serial.print(logLevel); Serial.print(F(": ")); Serial.println(logMessage);
 }


// Connection variables and functions

WOLFSSL_CTX* ctx = NULL;
WOLFSSL* ssl = NULL;

int EthernetSend(WOLFSSL* ssl, char* msg, int sz, void* ctx);
int EthernetReceive(WOLFSSL* ssl, char* reply, int sz, void* ctx);
EthernetClient client;


int EthernetSend(WOLFSSL* ssl, char* msg, int sz, void* ctx) {
    Serial.println(F("[EthernetSend]"));
    int sent = 0;
    Serial.print(F("[EthernetSend] msg "));
    Serial.println(msg);
    Serial.print(F("[EthernetSend] sz "));
    Serial.println(sz);
    sent = client.write((byte*)msg, sz);


    // size_t EthernetClient::write(uint8_t b) {
    //     return write(&b, 1);
    //   }
    //   size_t EthernetClient::write(const uint8_t *buf, size_t size) {
    //     if (_sock == MAX_SOCK_NUM) {
    //       setWriteError();
    //       return 0;
    //     }
    //     if (!send(_sock, buf, size)) {
    //       setWriteError();
    //       return 0;
    //     }
    //     return size;
    //   }

    // Error handling
    // Max Sock Num
    // Error in writing
    if (sent == 0) {
        sent = -1;
    }

    return sent;
}

int EthernetReceive(WOLFSSL* ssl, char* reply, int sz, void* ctx) {
    Serial.println(F("[EthernetReceive]"));
    int ret = 0;

    if (client.available() > 0) {
        Serial.println(F("ETR0"));
    }

    if (ret < sz) {
        Serial.println(F("ETR1"));
    }
    Serial.print("ret: ");
    Serial.println(ret);
    Serial.print("sz: ");
    Serial.println(sz);
    Serial.println("ETRLOOP");
    while (client.available() > 0 && ret < sz) {

        reply[ret++] = client.read();
        

    }
    Serial.println(ret);

    //Serial.println(reply);
    Serial.println("ETR2");
    if (ret != sz) {
        Serial.println("ETR3");
        Serial.println(ret);
        Serial.println(sz);
        return -1; //  return SOCKET_ERROR_E;

    }
    return ret;
}



int8_t wolfSSLSetup(byte * ROOT_CERTS_PEM, unsigned int size_cert, const char * host) {
    WOLFSSL_METHOD* method;

    freeRam();

    // If not init, then ctx is not null
    if (ctx != NULL) {
        Serial.println(F("[wolfSSLSetup]: ctx not null, so freed"));

        wolfSSL_CTX_free(ctx); ctx = NULL;
    }

    // Enable logging

    wolfSSL_Debugging_ON();

    wolfSSL_SetLoggingCb(&wolfssl_custom_logging_cb);

    // WOLFSSL_ECC_X25519
    //wolfSSL_CTX_UseSupportedCurve(ssl, WOLFSSL_ECC_X25519);
    wolfSSL_UseSupportedCurve(ssl, WOLFSSL_ECC_SECP160K1);
    method = wolfTLSv1_2_client_method();
    //method =  wolfSSLv23_client_method();
    //method = wolfTLSv1_3_client_method();
    if (method == NULL) {
        Serial.println(F("[wolfTLSv1_2_client_method] unable to get method"));
        return -1;
    }

    //wolfSSL_CTX_set_verify(ctx, SSL_VERIFY_NONE, 0);




    ctx = wolfSSL_CTX_new(method);
    if (ctx == NULL) {
        Serial.println(F("[wolfSSL_CTX_new] unable to get ctx"));
        return -1;
    }
    /*
    Serial.println((char *)ROOT_CERTS_PEM);
    Serial.println(sizeof((&ROOT_CERTS_PEM)[0]));
    Serial.println(sizeof (&ROOT_CERTS_PEM)[0]);
    Serial.println((char *)ROOT_CERTS_PEM_0);
    Serial.println(sizeof(ROOT_CERTS_PEM_0));
    Serial.println(sizeof ROOT_CERTS_PEM_0);
    */
    /* Add cert to ctx */
    int x =     wolfSSL_CTX_load_verify_buffer(ctx, ROOT_CERTS_PEM,size_cert, WOLFSSL_FILETYPE_PEM);
    int err            = 0;
    char errBuf[81];

    if (x != WOLFSSL_SUCCESS) {
        Serial.println(F("WolfSSL not success with ctx load"));
        err = wolfSSL_get_error(ssl, 0);
        wolfSSL_ERR_error_string_n(err, errBuf, 80);
        Serial.print(F("ERR wolfSSL_CTX_Load_verify_buffer: "));
        Serial.println(errBuf);
        return -1;
    }

    if (wolfSSL_CTX_UseSNI(ctx, 0, host, (word16) XSTRLEN(host)) != WOLFSSL_SUCCESS) {
        Serial.println(F("WolfSSL failed UseSNI"));

        wolfSSL_CTX_free(ctx); ctx = NULL;
        Serial.println(F("UseSNI failed"));
        //err_sys("UseSNI failed"));
        return -1;
    }
    // initialize wolfSSL using callback functions
    //wolfSSL_CTX_set_verify(ctx, SSL_VERIFY_NONE, 0);
    wolfSSL_CTX_set_verify(ctx, SSL_VERIFY_PEER, 0);

    wolfSSL_SetIOSend(ctx, EthernetSend);
    wolfSSL_SetIORecv(ctx, EthernetReceive);

    Serial.println(F("Setup success!"));
    freeRam();

    return 0;
}

int8_t SSLRequest(const char * host, const int port, char * msg, char * errBuf, char * reply)
{
    int err            = 0;
    int input          = 0;
    int total_input    = 0;

    int msgSz = (int)strlen(msg);

    const char* cipherName;

    int bwritten = 0;
    int8_t ifSuccess = 0;

    int reconnect = 10;

    int8_t res = 0;

    while (1) {

        if (reconnect > 0) {
            reconnect--;

            res = client.connect(host, port);
            Serial.print("[client.connect ret]: "); Serial.println(res);
            if (res > 0) {
                Serial.print(F("[SSLRequest] Connected to "));
                Serial.println(host);
                // freeRam();

                if (ctx == NULL) {
                    Serial.println(F("[SSLRequest] null ctx"));
                } 

                ssl = wolfSSL_new(ctx);
                Serial.println(F("SSLRequest"));
                freeRam();
                if (ssl == NULL) {
                    Serial.println(F("[SSLRequest] Unable to allocate SSL object"));
                    //freeRam();

                    err = wolfSSL_get_error(ssl, 0);
                    wolfSSL_ERR_error_string_n(err, errBuf, 80);
                    Serial.print(F("[SSLRequest] wolfSSL_new: "));
                    Serial.println(errBuf);
                    client.stop();
                    return -1;
                }

                err = wolfSSL_connect(ssl);
                freeRam();

                Serial.print(F("[SSLRequest] aft wolfSSL_connect err: ")); Serial.println(err);
                if (err != WOLFSSL_SUCCESS) {
                    err = wolfSSL_get_error(ssl, 0);
                    wolfSSL_ERR_error_string(err, errBuf);
                    Serial.print(F("[SSLRequest] TLS Connect Error: "));
                    Serial.println(errBuf);
                    wolfSSL_shutdown(ssl);
                    wolfSSL_free(ssl);
                    client.stop();

                    return -1;
                }

                Serial.print(F("[SSLRequest] SSL version is "));
                Serial.println(wolfSSL_get_version(ssl));

                cipherName = wolfSSL_get_cipher(ssl);
                Serial.print(F("[SSLRequest] SSL cipher suite is "));
                Serial.println(cipherName);
                Serial.println(F("[msg to send]:"));
                Serial.println(msg);
                Serial.println(strlen(msg));

                bwritten = wolfSSL_write(ssl, (char *) msg, strlen(msg));
                Serial.print(F("[SSLRequest] [Bytes written= ]"));
                Serial.println(bwritten);
                freeRam();

                if (bwritten > 0) {
                    Serial.println(F("[SSLRequest] [Server response]: "));
                    while ( client.available() || wolfSSL_pending(ssl) ) {
                        input = wolfSSL_read(ssl, reply, 1000-1);
                        total_input += input;
                        Serial.println(F("[SSLRequest]: Input"));
                        if ( input > 0 ) {
                            reply[input] = '\0';
                            Serial.print(reply);
                        } else {
                            Serial.println();
                            Serial.println(F("Read error"));
                            //return -1;
                        }
                    }

                    Serial.print(F("[SSLRequest] Bytes read= "));
                    Serial.println(total_input);
                } else {
                    err = wolfSSL_get_error(ssl, 0);
                    wolfSSL_ERR_error_string(err, errBuf);
                    Serial.print(F("[SSLRequest] LS Write Error: "));
                    Serial.println(errBuf);
                    //return -1;
                }
                freeRam();

                Serial.println(F("[SSLRequest] [before shutdown]"));
                wolfSSL_shutdown(ssl);
                wolfSSL_free(ssl);

                client.stop();
                Serial.println(F("[SSLRequest] Connection complete."));
                reconnect = 0;
                freeRam();
                ifSuccess = 1;

            } else {
                Serial.println(F("[SSLRequest] Trying to reconnect..."));
            }
        } else {
            if (ifSuccess == 1) {
                return 0; // Transaction completed succssfully
            } else {
                client.stop();

                return -2; // Transaction completed, but did not send properly
            }
        }
        Serial.println(F("[SSLRequest]: before delay 5000"));
        delay(2000);

    }
}