// Copyright (c) 2026 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix

// Proxy for ESP-NOW connection

#include <vector>
#include <WiFi.h>
#include <ESP32_NOW_Serial.h>
#include <MacAddress.h>
#include <MAVLink.h>
#include <Preferences.h>
#include "../../flix/util.h"

const bool DISABLE_SWARM = true;
const int CHANNEL = -1; // -1 means auto search
char key[ESP_NOW_KEY_LEN + 1] = {0}; // with trailing null

Preferences storage;

std::vector<ESPNOWSerial *> peers;
bool stop = false;

void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
	if (len != 4 || memcmp(data, "flix", 4) != 0) return; // check if discovery message

	if (stop) return;

	Serial.printf("New peer: " MACSTR "\n", MAC2STR(info->src_addr));
	ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, WiFi.channel(), WIFI_IF_STA);
	link->begin();
	link->setKey((const uint8_t *)key);
	peers.push_back(link);
}

void setup() {
	Serial.begin(115200);
	WiFi.mode(WIFI_STA);
	WiFi.setSleep(false);

	ESP_NOW.onNewPeer(onNewPeer, NULL);
	ESP_NOW.begin();

	storage.begin("espnow-proxy");
	if (!storage.isKey("key")) {
		generateRandomKey();
		storage.putString("key", key);
	}
	strcpy(key, storage.getString("key").c_str());
}

void generateRandomKey() {
	const char chars[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789!@#$%^&*-_+=";
	for (int i = 0; i < ESP_NOW_KEY_LEN; i++) {
		key[i] = chars[random(0, strlen(chars))];
	}
}

void loop() {
	uint8_t buf[5000];

	static int channelIndex = 0;
	static const int channels[] = {6, 11, 1, 2, 6, 11, 3, 4, 5, 6, 1, 11, 8, 6, 9, 10, 11, 6, 1, 12, 13}; // 6, 1 and 11 are most common

	static unsigned long last = 0;
	if (!stop && millis() - last > 500) {
		// Change search channel
		last = millis();
		channelIndex = (channelIndex + 1) % (sizeof(channels) / sizeof(channels[0]));
		int channel = CHANNEL < 0 ? channels[channelIndex] : CHANNEL;
		Serial.printf("Run on Flix: espnow %s %s\n", WiFi.STA.macAddress().c_str(), key);
		Serial.printf("Searching channel %d\n", channel);
		WiFi.setChannel(channel);
	}

	// Send from Serial to ESP-NOW
	while (Serial.available() > 0) {
		int b = Serial.read();
		if (b < 0) {
			break;
		}

		mavlink_message_t msg;
		mavlink_status_t status;
		if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)b, &msg, &status)) {
			int len = mavlink_msg_to_send_buffer(buf, &msg);
			for (ESPNOWSerial *link : peers) {
				link->write(buf, len);
			}
		}
	}

	// Send from ESP-NOW to Serial
	for (ESPNOWSerial *link : peers) {
		int len = link->read(buf, sizeof(buf));
		if (!stop) {
			for (int i = 0; i < len; i++) {
				if (buf[i] == MAVLINK_STX) {
					// Got MAVLink message, stop discovery
					Serial.printf("Received MAVLink from " MACSTR "\n", MAC2STR(link->addr()));
					if (DISABLE_SWARM) stop = true;
				}
			}
		}
		if (len > 0) {
			Serial.write(buf, len);
		}
	}
}