#include <FastLED.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCData.h>

#define HEATER_PIN        21   // ESP32 pin GPIO21 - heater control
#define NEOPIXEL_PIN      22   // ESP32 pin that connects to WS2812B
//#define ONBOARD_LED_PIN   8
#define NUM_LEDS     1  // The number of LEDs (pixels) on WS2812B
#define LED_TYPE     WS2812B
#define COLOR_ORDER  GRB
// MAX_MILLIWATTS can only be changed at compile-time. Use 0 to disable limit.
// Brightness can be changed at runtime via serial with 'b' and 'B'
#define MAX_MILLIWATTS 0

#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
// Defining the WiFi channel speeds up the connection:
#define WIFI_CHANNEL 6
#define LISTEN_PORT 5000
#define TIMEOUT_MILLIS 5000

#define SUMMARY_ELEMENT_NAME "muse_metrics28"
#define FILTER_COEF 0.05
#define FILTER_STEP 100
#define CALM_MAX_LEVEL 0.3;
#define PART1_MAX_LEVEL 200;
// 255-PART2_MAX_LEVEL
#define PART2_MAX_LEVEL 55;
#define MAX_CALM_PERIOD 120000;

uint8_t ledBrightness = 0;
uint8_t heaterLevel = 0;
unsigned long wifiInterval = 30000;
char* ssid = WIFI_SSID;
char* password = WIFI_PASSWORD;
uint8_t apChannel = WIFI_CHANNEL;
uint8_t wifiChannel = WIFI_CHANNEL;
char* hostname = "esp32-node-mind";
const char *passwordAP = "mind1234"; // Change this to your desired password
uint8_t counter = 0;

float calmMaxLevel = CALM_MAX_LEVEL;
float filterCoef = FILTER_COEF;
int filterStep = FILTER_STEP;
unsigned long filter_timer;
unsigned long calmTimer=0;

float curMindIndex = 20; // стартовый показатель делаем плохим (больше 1 - гасим лампочку)

long timeoutTargetMS = 0;
bool monitorOnline = false;

OSCErrorCode error;
WiFiUDP Udp;
CRGB leds[NUM_LEDS];
CRGB onb_led[1];

void setup() {
  Serial.begin(115200);
  Serial.println("Starting, ESP32-C6!");

  redLED();

  initHeater();
  initLedStrip();

  WiFi.mode(WIFI_AP_STA);
  if (!initWifi()) {
    initAP();
    blueLED();
  } else {
    greenLED();
  }

  initOsc();
}

void loop() {
  // counter++;
  // Serial.println(counter);
  receiveOscMessages(); // сообщения обновляются где-то раз в сек, но по 30 показателям. т.е. 30 сообщений в сек

  updateLed();
  updateHeater();

  delay(100); // todo уменьшить, чтобы успеть выгрести поток
}

void receiveOscMessages() {
  OSCBundle bundle;
  int size = Udp.parsePacket();

  if (size > 0) {
    while (size--) {
      bundle.fill(Udp.read());
    }
    if (!bundle.hasError()) {
      monitorOnline = true;
      bundle.dispatch(SUMMARY_ELEMENT_NAME, updateSummary);
    } else {
      error = bundle.getError();
      Serial.print("error: ");
      Serial.println(error);
    }
    //Reset timeout
    timeoutTargetMS = millis() + TIMEOUT_MILLIS;
  } else {    
    //Check for timeout
    if(monitorOnline && millis()>timeoutTargetMS){
      monitorOnline=false;
      turnOffLED();
      turnOffHeater();
    }
  }
}

void turnOffLED() {
  leds[0] = CRGB::Black;
  FastLED.show();
}

void turnOffHeater() {
  analogWrite(HEATER_PIN, 0);
}

void updateSummary(OSCMessage &msg) {
  if (millis() - filter_timer > filterStep) {
    filter_timer = millis();    // просто таймер
    curMindIndex = expRunningAverage(msg.getFloat(0));

    Serial.print("avg OSC 28: ");
    Serial.println(curMindIndex);
  }
  updateBrightness();
  updateHeaterLevel();
}

void updateBrightness() {
  // todo учитывать длительность нахождения в диапазоне, а не только curMindIndex
  if (curMindIndex > calmMaxLevel) {
    calmTimer = millis(); // сбросить счетчик времени покоя
  }
  if (curMindIndex > 1) {
    ledBrightness = 0;
  } else {
    int part1 = (int) (1.0-curMindIndex)*PART1_MAX_LEVEL; // значение muse28
    int part2 = 0; // показатель длительности периода непревышения порога расслабления
    if (calmTimer > 0) {
      unsigned long calmPeriod= (millis()-calmTimer);
      if (calmPeriod > MAX_CALM_PERIOD) {
        part2 = PART2_MAX_LEVEL;
      } else {
        part2 = (int) PART2_MAX_LEVEL*(MAX_CALM_PERIOD-calmPeriod)/MAX_CALM_PERIOD;
      }
      ledBrightness = part1 + part2;  
    }
  }
}

void updateHeaterLevel() {
  // todo
  //heaterLevel = ledBrightness;
  heaterLevel++;  
}

// бегущее среднее (! одноразовая - использует статическую переменную. переделать в класс)
float expRunningAverage(float newVal) {
  static float filVal = 0;
  filVal += (newVal - filVal) * filterCoef;
  return filVal;
}

// float getAveragePSD(OSCMessage &msg) {
//   if (msg.size()==1) {
//     return msg.getFloat(0); //Combined average can be sent by Muse Monitor
//   } else {
//     return (msg.getFloat(0)+msg.getFloat(1)+msg.getFloat(2)+msg.getFloat(3))/4.0; //TP9, AF7, AF8, TP10
//   }
// }

void initOsc() {
  Udp.begin(LISTEN_PORT);
  Serial.print("Listening port: ");
  Serial.println(LISTEN_PORT);
}

void initAP() {
  Serial.println();
  Serial.print("Starting access Point SSID: ");
  Serial.println(hostname);

  // Initialize ESP32 WiFi module in AP mode
  //WiFi.softAPConfig(IPAddress(4, 3, 2, 1), IPAddress(4, 3, 2, 1), IPAddress(255, 255, 255, 0));
  WiFi.softAP(hostname, passwordAP, apChannel);

  Serial.print("IP address: ");
  Serial.println(WiFi.softAPIP());
}

void greenLED() {
    rgbLedWrite(RGB_BUILTIN, 0, RGB_BRIGHTNESS, 0);  // onboard LED: Green
}

void blueLED() {
    rgbLedWrite(RGB_BUILTIN, 0, 0, RGB_BRIGHTNESS);  // onboard LED: Blue
}

void redLED() {
  rgbLedWrite(RGB_BUILTIN, RGB_BRIGHTNESS, 0, 0);  // onboard LED: Red
}

void WiFiStationConnected(WiFiEvent_t event, WiFiEventInfo_t info) {
  Serial.println("Connected to AP successfully!");
  //greenLED();
}

void printIP() {
  Serial.println("WiFi connected");
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.print("ESP32 HostName: ");
  Serial.println(WiFi.getHostname());  

  // print the received signal strength:
  Serial.print("RSSI : ");
  Serial.println(WiFi.RSSI());

   // print your MAC address:
  byte mac[6];
  WiFi.macAddress(mac);
  Serial.print("MAC address: ");
  Serial.print(mac[5], HEX);
  Serial.print(":");
  Serial.print(mac[4], HEX);
  Serial.print(":");
  Serial.print(mac[3], HEX);
  Serial.print(":");
  Serial.print(mac[2], HEX);
  Serial.print(":");
  Serial.print(mac[1], HEX);
  Serial.print(":");
  Serial.println(mac[0], HEX);
}

void WiFiGotIP(WiFiEvent_t event, WiFiEventInfo_t info) {
  printIP();
  //greenLED();
}

void WiFiStationDisconnected(WiFiEvent_t event, WiFiEventInfo_t info) {
  // todo проверить режим WIFI_STA

  Serial.println("Disconnected from WiFi access point");
  Serial.print("WiFi lost connection. Reason: ");
  Serial.println(info.wifi_sta_disconnected.reason);
  redLED();
  Serial.print("Trying to Reconnect to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
}

boolean initWifi() {
  WiFi.disconnect();
  //WiFi.mode(WIFI_STA);
  
  delay(100);
  WiFi.setHostname(hostname);

  WiFi.onEvent(WiFiStationConnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_CONNECTED);
  WiFi.onEvent(WiFiGotIP, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_GOT_IP);
  WiFi.onEvent(WiFiStationDisconnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_DISCONNECTED);

  WiFi.begin(ssid, password, wifiChannel);
  //WiFi.config(ip, gateway, subnet);

  Serial.print("Connecting to WiFi ");
  Serial.println(ssid);
  // Wait for connection
  unsigned long currentMillis = millis();
  unsigned long wifiPreviousMillis = 0;
  while (WiFi.status() != WL_CONNECTED && (currentMillis - wifiPreviousMillis < wifiInterval)) {
    // todo doBlinking();
    wifiPreviousMillis = currentMillis;
    delay(100);
  }
  return (WiFi.status() == WL_CONNECTED);
}

void initLedStrip() {
  FastLED.addLeds<LED_TYPE, NEOPIXEL_PIN, COLOR_ORDER>(leds, NUM_LEDS); //.setCorrection(TypicalLEDStrip);
  FastLED.setBrightness(ledBrightness);  
  if (MAX_MILLIWATTS > 0) FastLED.setMaxPowerInMilliWatts(MAX_MILLIWATTS);
}

void initHeater() {
  pinMode(HEATER_PIN, OUTPUT);
}

void updateLed() {
  leds[0] = CRGB::Blue;
  FastLED.setBrightness(ledBrightness);  
  FastLED.show();
}

void updateHeater() {
  // м.б. при уровне меньше какого-то порога надо ставить 0, чтобы нагреватель не включался совсем
  analogWrite(HEATER_PIN, heaterLevel);
}