HansonServo/websocket_client.cpp

#include "websocket_client.h"
#include "sensors.h"
#include "protocol.h"
#include <ArduinoWebsockets.h>
#include <WiFi.h>

using namespace websockets;

static WebsocketsClient client;
static bool s_connected = false;
static unsigned long lastReconnectAttempt = 0;
constexpr unsigned long RECONNECT_INTERVAL = 5000;

// ============================================================================
// Packet parsing for WebSocket binary messages
// Uses the same protocol format: 0xA5 0x5A TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2)
// ============================================================================

static void processPacketPayload(const char tag[4], const uint8_t* payload, uint16_t len) {
  if (memcmp(tag, Tag::FACE, 4) == 0) {
    // Face detection data - feed to FaceDetect sensor
    // SensorManager will send the FACE packet over serial
    faceDetect.feedPayload(payload, len);
  }
  else if (memcmp(tag, Tag::ALIV, 4) == 0) {
    // ALIV payload: [component_id:1][alive:1]
    if (len >= 2) {
      uint8_t componentId = payload[0];
      uint8_t alive = payload[1];
      
      // Component 3 = Face detection (Radxa)
      if (componentId == 3) {
        faceDetect.setAlive(alive != 0);
      }
    }
    // Forward ALIV as a proper protocol packet over serial
    sendPacket(Tag::ALIV, payload, len);
  }
}

static void parseProtocolMessage(const uint8_t* data, size_t len) {
  // Walk through the message looking for protocol packets
  // Format: SYNC0(0xA5) SYNC1(0x5A) TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2)
  size_t pos = 0;
  
  while (pos + 12 <= len) {  // Minimum packet: 2 sync + 4 tag + 2 len + 2 seq + 0 payload + 2 crc = 12
    // Look for sync bytes
    if (data[pos] != 0xA5 || data[pos + 1] != 0x5A) {
      pos++;
      continue;
    }
    
    // Read tag
    char tag[4];
    memcpy(tag, &data[pos + 2], 4);
    
    // Read length (LE)
    uint16_t payloadLen = data[pos + 6] | (data[pos + 7] << 8);
    
    // Sanity check
    size_t totalPacketLen = 2 + 4 + 2 + 2 + payloadLen + 2;  // sync + tag + len + seq + payload + crc
    if (pos + totalPacketLen > len) {
      // Incomplete packet
      break;
    }
    
    // Payload starts after sync(2) + tag(4) + len(2) + seq(2) = offset 10
    const uint8_t* payload = &data[pos + 10];
    
    // Verify CRC over tag + len + seq + payload
    const uint8_t* crcData = &data[pos + 2];  // starts at tag
    uint16_t crcDataLen = 4 + 2 + 2 + payloadLen;  // tag + len + seq + payload
    uint16_t computed = crc16Compute(crcData, crcDataLen);
    
    uint16_t received = data[pos + 10 + payloadLen] | (data[pos + 10 + payloadLen + 1] << 8);
    
    if (computed == received) {
      processPacketPayload(tag, payload, payloadLen);
    }
    
    pos += totalPacketLen;
  }
}

// ============================================================================
// WebSocket callbacks
// ============================================================================

static void onMessage(WebsocketsMessage message) {
  std::string raw = message.rawData();
  size_t len = raw.size();
  if (len == 0) return;

  const uint8_t* data = reinterpret_cast<const uint8_t*>(raw.data());
  parseProtocolMessage(data, len);
}

static void onEvent(WebsocketsEvent event, String data) {
  switch (event) {
    case WebsocketsEvent::ConnectionOpened:
      s_connected = true;
      Serial.println("[WebSocket] Connected");
      break;
    case WebsocketsEvent::ConnectionClosed:
      s_connected = false;
      Serial.println("[WebSocket] Disconnected");
      break;
    case WebsocketsEvent::GotPing:
      break;
    case WebsocketsEvent::GotPong:
      break;
    default:
      break;
  }
}

// ============================================================================
// Public API
// ============================================================================

void websocketSetup() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(WebSocketConfig::WIFI_SSID, WebSocketConfig::WIFI_PASSWORD);
  Serial.print("[WebSocket] WiFi connecting");
  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 30) {
    delay(500);
    Serial.print(".");
    attempts++;
  }
  Serial.println();
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("[WebSocket] WiFi failed");
    return;
  }
  Serial.print("[WebSocket] WiFi OK ");
  Serial.println(WiFi.localIP());

  client.onMessage(onMessage);
  client.onEvent(onEvent);

  String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
  Serial.println("[WebSocket] Connecting to " + url);
  if (client.connect(url)) {
    s_connected = true;
    Serial.println("[WebSocket] Connected");
  } else {
    Serial.println("[WebSocket] Connect failed (will retry in loop)");
  }
}

void websocketLoop() {
  if (WiFi.status() != WL_CONNECTED) {
    s_connected = false;
    return;
  }

  if (!s_connected && !client.available()) {
    unsigned long now = millis();
    if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) {
      lastReconnectAttempt = now;
      String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
      if (client.connect(url)) {
        s_connected = true;
        Serial.println("[WebSocket] Reconnected");
      }
    }
  }

  if (client.available()) {
    client.poll();
  }
}

bool websocketConnected() {
  return s_connected && client.available();
}