#include <assert.h>
#include <ESP32Servo.h>
#include <ArduinoJson.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <WebSocketsClient.h>
#include <limits>

// To get this working on the simulator, I had to tweak
// the URL to limit number of samples per second to only 10.

// ================= Primary settings section =================== //
#define PIN_SERVO_PWM         15              ///< The logical number of the pin a servo is connected to.
#define PIN_SERMON_RXD        16              ///< The logical number of the pin used as Serial2 RX 
#define PIN_SERMON_TXD        17              ///< The logical number of the pin used as Serial2 TX
#define WIFICLIENT_SSID       "Wokwi-GUEST"   ///< The ID of the wifi net to connect to. "Wokwi-GUEST" is simulated.
#define WIFICLIENT_PASSWORD   ""              ///< The password of the wifi net to connect to. The simulated net has empty password. 
#define WS_IP                 "68.183.65.4"   ///< IP of the target server.
#define WS_PORT               80              ///< Port of the target server.
#define WS_RESOURCE_PATH      "/ws/channel_10_999_2"  ///< Path to the desired resource on the target server.
#define WS_RECONN_INTERVAL_MS 5000            ///< WebSocketsClient reconnect interval time in milliseconds.
#define SAMPLES_BUF_SIZE      2000            ///< Number of samples in the circular buffer


/// \brief Size of the statically-allocated JSON document
///
/// The single JSON object size estimation is 80 bytes,
/// the max expected amount of objects is 10 + one object holding the array itself.
/// Use the https://arduinojson.org/v6/assistant for reference.
///
/// Rounded up to 2048 to have enough margin.
#define JSON_DOC_SIZE         1024


namespace { // Serial port handling
  void serial_setup() {
    Serial.begin(115200);
    Serial.println("Serial initialized");

    Serial2.begin(115200, SERIAL_8N1, PIN_SERMON_RXD, PIN_SERMON_TXD);
  }
} // End of Serial port handling


namespace { // Wifi handling
  void wifi_setup() {
    // Connect to the WiFi network, simulated by this platform (wokwi).
    Serial.print("Connecting to WiFi");
    WiFi.begin(WIFICLIENT_SSID, WIFICLIENT_PASSWORD);
    while (WiFi.status() != WL_CONNECTED) {
      delay(100);
      Serial.print(".");
    }
    Serial.println(" Connected!");  
  }
} // End of Wifi handling


class MySamples { // Mysamples circular buffer
private:
  size_t      idx_wr{0};
  size_t      idx_rd{0};
  size_t      count{0};
  uint32_t    time_youngest{0};

public:
  static constexpr   size_t BUF_SIZE = SAMPLES_BUF_SIZE;
  static constexpr   size_t IDX_INVALID = SAMPLES_BUF_SIZE;

  uint32_t    time[BUF_SIZE];
  uint8_t     angle[BUF_SIZE];


  void clear() {
    idx_wr = 0;
    idx_rd = 0;
    count  = 0;
    time_youngest = 0;
  }
  
  void setup() {
    clear();
  }

  /// \brief Seeks the sample that should be used at the time moment \p now_millis.
  /// \details
  /// This function scans the circular buffer and removes all samples that has timestamp <= now_millis.
  /// After completion, this function returns the index of the latest removed sample,
  /// or `SAMPLE_IDX_INVALID` if none had been found.
  size_t get(uint32_t now_millis) {
    size_t    sample_idx = IDX_INVALID;
    while (count) {
      if (time[idx_rd] > now_millis) {
        // Found first "in-the-future" sample, there will be no more "old" samples.
        break;
      }

      // Found next "old" sample
      sample_idx      = idx_rd;
      
      // remove it from the buffer
      idx_rd  = (idx_rd + 1) % BUF_SIZE;
      count  -= 1;
    }
    return sample_idx;
  }

  /// \brief Reserves a space in a circular buffer for a new sample.
  /// \returns If this function succeeds (i.e. there is space), it returns the index of the created sample, 
  /// otherwise it returns SAMPLE_IDX_INVALID.
  size_t append(uint32_t sample_time) {
    if (sample_time < time_youngest) {
      // Error: samples should be inserted in the time-ascending order.
      return IDX_INVALID;
    }

    if (count >= BUF_SIZE) {
      // Error: No more space in the circular bufer
      return IDX_INVALID;
    }

    size_t sample_idx = idx_wr;
    time[sample_idx] = sample_time;
    time_youngest = sample_time;

    idx_wr = (idx_wr + 1) % BUF_SIZE;
    count += 1;
    
    return sample_idx;
  }
}; // End of samples 


class MyServo { // Servo handling
  MySamples&    samples;
  Servo         myservo;  


public:
  explicit MyServo(MySamples& my_samples) : samples(my_samples) {}

  void setup() {
    Serial.print("Initializing the servo driver... ");
    myservo.setPeriodHertz(50);   // The simulated servo requires 50 Hz to work correctly
    myservo.attach(PIN_SERVO_PWM); 
    myservo.write(90);
    Serial.println("Done.");
  }

  void loop() {
    uint32_t  time = millis();
    size_t sample_idx = samples.get(time);
    if (sample_idx == samples.IDX_INVALID) {
      return;
    }

    myservo.write(samples.angle[sample_idx]);
  }
}; // End of Servo handling


class Protocol { // JSON and application protocol handling
  MySamples&        samples;

  WebSocketsClient  web_socket;
  uint32_t          time_connected;

  // This is a huge waste of memory space. 
  // Such JSON format is not very good for small embedded targets.
  // Suggest switching to some binary formatter, i.e. Google's protobuf.
  // But until then dealing with what we've got. 
  StaticJsonDocument<JSON_DOC_SIZE> json_doc;

public:
  Protocol(MySamples& my_samples) : samples(my_samples) {}

  void on_connected() {
    time_connected = millis();
  }

  void on_message(uint8_t* data, size_t length) {
    DeserializationError error = deserializeJson(json_doc, data, length);
    if (error) {
      Serial.print("deserializeJson() failed: ");
      Serial.println(error.c_str());
      return;
    }

    for (JsonObject item : json_doc.as<JsonArray>()) {
      // Get fields from the item. 
      // Probably, should decide the format of the JSON based on the "channel" value.
      const char* channel = item["channel"];      // "channel_1000_999_2", "channel_1000_999_2", ...
      int         frequency = item["frequency"];  // 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000
      double      value = item["value"];          // 0.001129512957157704, 0.018265938937704698, 0.018265938937704698, ...
      double      time = item["time"];            // 0.00008988380432128906, 0.0014536380767822266, 0.0014536380767822266, ...

      (void)channel; // not used, left for debug
      (void)frequency; // not used, left for debug

      // convert double [-1.0 ... +1.0] value to the int [0 ... 180] target angle of the servo.
      // assuming the following conversion rules:
      //    value=-1.0 --> angle=0
      //    value=0    --> angle=90
      //    value=+1.0 --> angle=180
      //    value out of range --> drop this sample
      if (value < -1.0 || value > 1.0) {
        // Error: received value is out of range.
        continue;
      }

      uint8_t     angle = (uint8_t)(90 + (value * 90));

      // Convert the double time since the connection started 
      // to the integer number of milliseconds since connection started.
      // If the date can't be converted to milliseconds (due to int overflow),
      // then drop this sample
      if (time < 0 || time >= (std::numeric_limits<uint32_t>::max()/1000)) {
        // Error: received timestamp is out of range
        continue;
      }
      
      uint32_t    time_since_connected = (uint32_t)(time * 1000);
      uint32_t    sample_time = time_connected + time_since_connected;
      if (sample_time < time_since_connected) {
        // Error: sample time overflowed. This module does not support this.
        continue;
      }

      size_t sample_idx = samples.append(sample_time);
      if (sample_idx == samples.IDX_INVALID) {
        // Error: No space left in the buffer
        continue;
      }

      // This piece exposes the close coupling between `samples` module and the `protocol` module,
      // but there is no actual need to add another layer of abstraction over the samples' storage.
      samples.angle[sample_idx] = angle; 
      
      // Dump original sample to the console to use the plotter feature 
      // of the simulator.
      Serial2.println(value);
    } // for (JsonObject item : ....
  }

  void on_ws_event(WStype_t type, uint8_t* payload, size_t length) {
    switch(type) {
      case WStype_DISCONNECTED: 
        Serial.println("WS disconnected"); 
        break;

      case WStype_CONNECTED:
        Serial.println("WS connected");
        time_connected = millis();
        break;

      case WStype_TEXT:
        Protocol::on_message(payload, length);
        break;

      case WStype_BIN:                    // Fall-through
      case WStype_ERROR:			            // Fall-through
      case WStype_FRAGMENT_TEXT_START:    // Fall-through
      case WStype_FRAGMENT_BIN_START:     // Fall-through
      case WStype_FRAGMENT:               // Fall-through
      case WStype_FRAGMENT_FIN:           // Fall-through
        Serial.printf("WS event %d", (int)type);
        break;
    }
  }

  /// \warning This function shall be called only after the wifi is initialized.
  void setup() {
    time_connected = 0;
    web_socket.begin(WS_IP, WS_PORT, WS_RESOURCE_PATH);
    web_socket.onEvent(
        std::bind(
            &Protocol::on_ws_event, 
            this, 
            std::placeholders::_1, 
            std::placeholders::_2, 
            std::placeholders::_3
        )
    );
    web_socket.setReconnectInterval(WS_RECONN_INTERVAL_MS);
  }

  void loop() {
    web_socket.loop();
  }
}; // End of JSON and application protocol handler


MySamples samples;
MyServo servo{samples};
Protocol protocol{samples};


void setup() {
  serial_setup();
  wifi_setup();
  servo.setup();
  protocol.setup();
}


void loop() {
  protocol.loop();
  servo.loop();
  //delay(10); // this speeds up the simulation
}