/* ESP32 WiFi Scanning example */
// anne juliet stem
#include "WiFi.h"
#include <WiFiUdp.h>

#define XPLANE_IP "10.101.1.162"
#define XPLANE_PORT 49000

unsigned int localPort = 57506;  
WiFiUDP udpDREF;
WiFiUDP udp;
char buffer[1024];

void setup() {
  Serial.begin(9600);
  Serial.print("Connecting to WiFi");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  
  udpDREF.begin(localPort);
 // subscribe("sim/cockpit2/radios/actuators/com1_standby_frequency_hz_833", 1, 42);

}

void subscribe(char *dref, uint32_t freq, uint32_t index) {
  struct {
    char dummy[3]; // For alignment
    char hdr[5] = "RREF";
    uint32_t dref_freq;
    uint32_t dref_en;
    char dref_string[400];
  } req __attribute__((packed));

  req.dref_freq = freq;
  req.dref_en = index;
  for (int x = 0; x < sizeof(req.dref_string); x++)
    req.dref_string[x] = 0x20;
  strcpy((char *) req.dref_string, (char *) dref);
  
  udpDREF.beginPacket(XPLANE_IP, XPLANE_PORT);
  udpDREF.write((uint8_t *)req.hdr, sizeof(req) - sizeof(req.dummy));
  udpDREF.endPacket();
  Serial.print("Subscribed to dref \"");
  Serial.print(dref);
  Serial.println("\"");
}

  
void loop() {
  uint8_t stby_com1_fine_up[50] = "CMND\0sim/radios/stby_com1_fine_up";
  uint8_t stby_com1_coarse_up[50] = "CMND\0sim/radios/stby_com1_coarse_up";
  uint8_t com1_standy_flip[50] = "CMND\0sim/radios/com1_standy_flip";
  
  // send(stby_com1_fine_up);
  // delay(1000);
  // send(stby_com1_coarse_up);
  // delay(1000);
  // send(stby_com1_fine_up);
  // delay(1000);
  // send(stby_com1_coarse_up);
  // delay(1000);
  // send(com1_standy_flip);
  // delay(1000);
  int packetSize = udp.parsePacket();
  if(packetSize){
    Serial.println(packetSize);
    char *buff = &buffer[3];  // For alignment
    udp.read(buff, packetSize);
    String type =  String(buff).substring(0,4);
    if (type == "RREF") {
      for (int offset = 5; offset < packetSize; offset += 8) {
        int code = *((int *) (buff+offset));
        float value = *((float *) (buff+offset+4));
        Serial.println(code);
        Serial.println(value);
      }
    }
  }

}

void send(uint8_t* command){
  udp.beginPacket(XPLANE_IP, 49000);
  udp.write(command, 50);
  udp.endPacket();
}