#include <stdlib.h>
#include <SPI.h>
#include <WiFiNINA.h>

char ssid[] = "***";                // House -- Network SSID (name)
char pass[] = "***";              // House -- SSID password (WPA2-PSK)

#define MAX_CLIENTS 5
#define MAX_LINE_LEN 50
char inputs[MAX_CLIENTS][MAX_LINE_LEN] = {0}; // Instantiate 2D array of
client/received data
int msg_array[] = {0,0};
int net_status = WL_IDLE_STATUS;              // The Wifi radio's status
WiFiServer server(80);           // Create server object on port of WiFi
                                 // server, which will be used to
                                 // respond to commands

WiFiClient *clients[MAX_CLIENTS] = {NULL};    // Create client objects
                                              // limited to size of
                                              // MAX_CLIENTS


byte mac[] = {0xDE,0xAD,0xBE,0xEF,0xFE,0xED}; // Arduino's physical MAC
byte ip[] = {192,168,9,25};              // IP of this controller
byte gateway[] = {192,168,0,1};          // Router address (internet)
byte subnet[] = {255,255,0,0};           // Subnet mask

unsigned long wd_Dside, wd_Cside = millis();   // Gather current tstamp
unsigned long wd_Delap, wd_Celap = 0;          // Create an elapsed time
                                               //counter


void setup(){

   pinMode(4, OUTPUT);                 // pin selected to control D SIDE
   pinMode(5, OUTPUT);                 // pin selected to control C SIDE

   //enable serial data print
   Serial.begin(9600);
   while (!Serial){
       Serial.println("No serial.");
   }

      Serial.println("Serial.begin was initiated.");


   if (WiFi.status() == WL_NO_SHIELD) {  // check for the WiFi module:
      Serial.println("Wi-Fi: Communication with NINA has failed!");
      // do nothing here
      while (true);
   }

   String fv = WiFi.firmwareVersion();
   if (fv < WIFI_FIRMWARE_LATEST_VERSION) {
      Serial.println("Please upgrade the firmware");
   }

   while (net_status != WL_CONNECTED) {
      Serial.print("Attempting to connect to WPA SSID: ");
      Serial.println(ssid);

      // Connect to WPA/WPA2 network:
      net_status = WiFi.begin(ssid, pass); // Attempt to join this SSID

      // wait 5 seconds for connection:
      delay(5000);

      listNetworks();

      server.begin();                      // Start Wi-Fi server
      Serial.print("Server started on ");
      Serial.println(WiFi.localIP());
    }
}


void loop() {

    delay(5);

    WiFiClient newClient = server.available(); // Begin listening for
                                               // client messages
    if (newClient) {
        Serial.println("New client discovered."); // Could use this as
                                                  // an ACK/handshake on
                                                  // client-side host
        for (int i=0; i<MAX_CLIENTS; i++) {
          if (clients[i] == NULL) {
            clients[i] = new WiFiClient(newClient);
            break;
      }
   }
}

    for (int i=0; i<MAX_CLIENTS; i++){      // Check client for data
      if (clients[i] != NULL && clients[i]->available()){
         // If the data isn't garbage and the data is available..

        char newChar = clients[i]->read();
        // Read the data

        if (newChar == 'x') {
        // Check for a termination character for separation

        // Now parse with strtok in order to get parameterization
          char delimiters[] = ",";
          char* valIndex;
          valIndex = strtok(inputs[i], delimiters);
          for (int j=0; j < 2; j++){
            // Expand later to largest anticipated character count
            msg_array[j] = atoi(valIndex);
            Serial.print(msg_array[j]);
            valIndex = strtok(NULL, delimiters);
          }

          switch(msg_array[0]) {
          // Determine type of message received (Character 0 parsed as
          // Message Type)

            case '3':
            // Examine string for client cmds to turn sides on or off.
            if(msg_array[1] == '1')
            // Msg type 3, Instruction 1: Turn D side ON
              {
                digitalWrite(4, HIGH);
                // Set pin 4 high
                Serial.println("Dside started.");
                wd_Delap = 0;
                // Reset elapsed time watchdog time value for D side.
                wd_Dside = millis();
                // Last good heartbeat from client device on D side.
              }
            if(msg_array[1] == '2')
            // Msg type 3, Instruction 2: Turn D side OFF
              {
                digitalWrite(4, LOW);
                // Set pin 4 low
                Serial.println("Dside stopped");
                wd_Delap = 0;
                // Reset elapsed time watchdog time value for D side.
                wd_Dside = millis();
                // Last good heartbeat from client device on D side.
              }
            if(msg_array[1] == '3')
                // Msg type 3, Instruction 3: Turn C side ON
              {
                digitalWrite(5, HIGH);
                // Set pin 5 high
                Serial.println("C side started.");
                wd_Celap = 0;
                // Reset elapsed time watchdog time value for C side.
                wd_Cside = millis();
                // Last good heartbeat from client device on C side.
              }
            if(msg_array[1] == '4')
                // Msg type 3, Instruction 4: Turn C side OFF
              {
                digitalWrite(5, LOW);
                // Set pin 5 low
                Serial.println("C side stopped.");
                wd_Celap = 0;
                // Reset elapsed time watchdog time value for C side.
                wd_Cside = millis();
                // Last good heartbeat from client device on C side.
              }
            break;

            case '6':

                // Examine string for another message type TBD
            break;

            default:
              Serial.println("Default break. Received crap.");
            break;

          }

        inputs[i][0] = '';
        // Clear out this array element for the next message
        // clients[i]->stop();
        // By default, kill client session, just wait for new TCP open
        // delete clients[i];
        // Remove client element's data
        // clients[i] = NULL;
        // Reset to NULL

        }

        else {

           if (strlen(inputs[i]) > MAX_LINE_LEN) {
                // First check for size compliance
           Serial.println("Max line length exceeded from client.");
           inputs[i][0] = '0';
                // Clear out this array element for the next message
           }
           else {
           **strcat(inputs[i], newChar);**
                // Add character to the received string
           }

          Serial.println();
          Serial.println(newChar);
        }
      }

    // Update watchdog timers

    wd_Delap += (millis()-wd_Dside);
    wd_Celap += (millis()-wd_Cside);

    if (wd_Delap>=60000)
          {
            Serial.print("Elapsed time on watchdog, D side: ");
            Serial.println(wd_Delap);
            Serial.print("Last message received: ");
            Serial.println(wd_Dside);
            digitalWrite(4, LOW);    // set pin 4 low
            Serial.println("D side didn't hear from a client in 60s;
                           defaulting to OFF.");
          }

    if (wd_Celap>=60000)
          {
            Serial.print("Elapsed time on watchdog, C side: ");
            Serial.println(wd_Celap);
            Serial.print("Last message recieved: ");
            Serial.println(wd_Cside);
            digitalWrite(5, LOW);    // set pin 5 low
            Serial.println("C side didn't hear from a client in 60s;
                            defaulting to OFF.");
          }

   }
}