#include <ESP8266WiFi.h>
#include <espnow.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

uint8_t macRobota[] = {0x08, 0xF9, 0xE0, 0x75, 0xDE, 0x28};
String ostatnieDane = "";
bool noweDane = false;

#define PRZOD  D5
#define TYL    D4
#define LEWO   D6
#define PRAWO  D7
#define STOP   D0
#define SELECT_MODE D3

int mode = 0;
String label = "AX";
float val = 0;
float lastAX = 0, lastAY = 0, lastAZ = 0, lastModA = 0;
float lastPitch = 0, lastRoll = 0;
float velocity = 0;
float lastUpdateTime = 0;

unsigned long lastDebounce = 0;
const unsigned long debounceDelay = 250;
bool lastButtonState = HIGH;

#define MAX_POINTS 128
float buffer[MAX_POINTS];
int bufIndex = 0;

// === OFFSETY CZUJNIKA ===
const float offsetAX = -0.8;
const float offsetAZ =  0.1;
const float offsetPitch = 5.0;

void clearBuffer() {
  for (int i = 0; i < MAX_POINTS; i++) buffer[i] = 0;
  bufIndex = 0;
}

void onReceive(uint8_t *mac, uint8_t *data, uint8_t len) {
  ostatnieDane = "";
  for (int i = 0; i < len; i++) {
    ostatnieDane += (char)data[i];
  }
  noweDane = true;
}

void sendMsg(const char* msg) {
  esp_now_send(macRobota, (uint8_t*)msg, strlen(msg));
  Serial.print("Wyslano: ");
  Serial.println(msg);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(D2, D1);

  pinMode(PRZOD, INPUT_PULLUP);
  pinMode(TYL, INPUT_PULLUP);
  pinMode(LEWO, INPUT_PULLUP);
  pinMode(PRAWO, INPUT_PULLUP);
  pinMode(STOP, INPUT_PULLUP);
  pinMode(SELECT_MODE, INPUT_PULLUP);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Konsolka startuje...");
  display.display();

  WiFi.mode(WIFI_STA);
  if (esp_now_init() != 0) {
    display.println("ESP-NOW ERR");
    display.display();
    return;
  }

  esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
  esp_now_add_peer(macRobota, ESP_NOW_ROLE_COMBO, 1, NULL, 0);
  esp_now_register_recv_cb(onReceive);
  display.println("ESP-NOW OK");
  display.display();
  delay(1000);
  display.clearDisplay();

  clearBuffer();
  lastUpdateTime = millis();
}

void rysujWykres() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.println("Tryb: " + label);

  float minVal = -11;
  float maxVal = 11;
  if (label == "V") {
    minVal = 0;
    maxVal = 2;  // dopasuj w razie potrzeby
  }

  float scale = maxVal - minVal;
  int zeroY = SCREEN_HEIGHT - 1 - (int)((0 - minVal) / scale * (SCREEN_HEIGHT - 10));
  if (zeroY >= 10 && zeroY <= SCREEN_HEIGHT - 1) {
    display.drawLine(0, zeroY, SCREEN_WIDTH, zeroY, SSD1306_WHITE);
  }

  for (int x = 0; x < MAX_POINTS - 1; x++) {
    int y1 = SCREEN_HEIGHT - 1 - (int)((buffer[(x + bufIndex) % MAX_POINTS] - minVal) / scale * (SCREEN_HEIGHT - 10));
    int y2 = SCREEN_HEIGHT - 1 - (int)((buffer[(x + 1 + bufIndex) % MAX_POINTS] - minVal) / scale * (SCREEN_HEIGHT - 10));
    display.drawLine(x, y1, x + 1, y2, SSD1306_WHITE);
  }

  display.setCursor(64, 0);
  display.setTextSize(1);
  if (label == "AX") display.print("AX:"), display.print(lastAX, 1);
  else if (label == "AY") display.print("AY:"), display.print(lastAY, 1);
  else if (label == "|AZ|") display.print("AZ:"), display.print(lastAZ, 1);
  else if (label == "|A|") display.print("|A|:"), display.print(lastModA, 1);
  else if (label == "V") display.print("V:"), display.print(velocity, 2);

  display.display();
}

void loop() {
  if (digitalRead(PRZOD) == LOW) sendMsg("FWD");
  else if (digitalRead(TYL) == LOW) sendMsg("BACK");
  else if (digitalRead(LEWO) == LOW) sendMsg("LEFT");
  else if (digitalRead(PRAWO) == LOW) sendMsg("RIGHT");
  else if (digitalRead(STOP) == LOW) {
    sendMsg("STOP");
    velocity = 0;
  }

  bool currentButtonState = digitalRead(SELECT_MODE);
  if (lastButtonState == HIGH && currentButtonState == LOW) {
    if (millis() - lastDebounce > debounceDelay) {
      mode = (mode + 1) % 7;
      switch(mode) {
        case 0: label = "AX"; clearBuffer(); break;
        case 1: label = "AY"; clearBuffer(); break;
        case 2: label = "|AZ|"; clearBuffer(); break;
        case 3: label = "|A|"; clearBuffer(); break;
        case 4: label = "Roll"; break;
        case 5: label = "Pitch"; break;
        case 6: label = "V"; clearBuffer(); velocity = 0; lastUpdateTime = millis(); break;
      }
      lastDebounce = millis();
    }
  }
  lastButtonState = currentButtonState;

  if (noweDane) {
    float ax, ay, az, p, r;
    if (sscanf(ostatnieDane.c_str(), "AX:%f AY:%f AZ:%f P:%f R:%f", &ax, &ay, &az, &p, &r) == 5) {
      
      // ✅ Zastosowanie offsetów
      ax += offsetAX;
      az += offsetAZ;
      p  += offsetPitch;

      lastAX = ax;
      lastAY = ay;
      lastAZ = az;
      lastModA = sqrt(ax * ax + ay * ay);

      if (fabs(lastModA) < 0.1) lastModA = 0;

      lastPitch = p;
      lastRoll = r;

      switch(mode) {
        case 0: val = ax; break;
        case 1: val = ay; break;
        case 2: val = fabs(az); break;
        case 3: val = lastModA; break;
        case 6: {
          float currentTime = millis();
          float dt = (currentTime - lastUpdateTime) / 1000.0;
          velocity += lastModA * dt;
          val = velocity;
          lastUpdateTime = currentTime;
          break;
        }
      }

      if ((mode >= 0 && mode <= 3) || mode == 6) {
        buffer[bufIndex] = val;
        bufIndex = (bufIndex + 1) % MAX_POINTS;
      }
    }
    noweDane = false;
  }

  if ((mode >= 0 && mode <= 3) || mode == 6) {
    rysujWykres();
  } else {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.setTextSize(2);
    display.println(label);
    display.setTextSize(2);
    display.setCursor(0, 30);
    if (mode == 4) {
      display.print(lastRoll, 1); display.print(" deg");
    }
    if (mode == 5) {
      display.print(lastPitch, 1); display.print(" deg");
    }
    display.display();
  }

  delay(50);
}