#include <SPI.h>

// ---- тип точки объявлен сразу ----
struct Pt { int16_t x, y; };

// Прототипы
void drawSegment(Pt a, Pt b, uint16_t color);
void playSegmentBresenham(Pt a, Pt b);

// ===================== Mini ILI9341 (минимальный драйвер) =====================
#define ILI9341_TFTWIDTH  240
#define ILI9341_TFTHEIGHT 320

// Команды ILI9341
#define ILI9341_SWRESET  0x01
#define ILI9341_SLPOUT   0x11
#define ILI9341_DISPON   0x29
#define ILI9341_CASET    0x2A
#define ILI9341_PASET    0x2B
#define ILI9341_RAMWR    0x2C
#define ILI9341_MADCTL   0x36
#define ILI9341_COLMOD   0x3A

#define TFT_DC   2
#define TFT_CS   3

class MiniILI9341 {
public:
  MiniILI9341(uint8_t cs, uint8_t dc) : _cs(cs), _dc(dc) {}
  void begin() {
    pinMode(_cs, OUTPUT);
    pinMode(_dc, OUTPUT);
    digitalWrite(_cs, HIGH);
    digitalWrite(_dc, HIGH);
    SPI.begin();
    SPI.beginTransaction(SPISettings(40000000, MSBFIRST, SPI_MODE0)); // 40MHz ок в Wokwi

    // Короткая инициализация
    writeCmd(ILI9341_SWRESET); delay(150);
    writeCmd(ILI9341_SLPOUT);  delay(120);
    writeCmd(ILI9341_COLMOD);  writeData8(0x55); // 16-bit color
    writeCmd(ILI9341_MADCTL);  writeData8(0x48); // портрет, RGB
    writeCmd(ILI9341_DISPON);  delay(20);
  }

  void fillScreen(uint16_t color) {
    setAddrWindow(0, 0, ILI9341_TFTWIDTH-1, ILI9341_TFTHEIGHT-1);
    writeCmd(ILI9341_RAMWR);
    startData();
    uint32_t px = (uint32_t)ILI9341_TFTWIDTH * ILI9341_TFTHEIGHT;
    uint8_t hi = color >> 8, lo = color & 0xFF;
    for (uint32_t i=0; i<px; i++) { SPI.transfer(hi); SPI.transfer(lo); }
    endData();
  }

  void drawPixel(int16_t x, int16_t y, uint16_t color) {
    if (x<0 || y<0 || x>=ILI9341_TFTWIDTH || y>=ILI9341_TFTHEIGHT) return;
    setAddrWindow(x, y, x, y);
    writeCmd(ILI9341_RAMWR);
    writeData16(color);
  }

  void fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color) {
    if (w<=0 || h<=0) return;
    if (x<0) { w += x; x = 0; }
    if (y<0) { h += y; y = 0; }
    if (x+w > ILI9341_TFTWIDTH)  w = ILI9341_TFTWIDTH - x;
    if (y+h > ILI9341_TFTHEIGHT) h = ILI9341_TFTHEIGHT - y;
    if (w<=0 || h<=0) return;

    setAddrWindow(x, y, x+w-1, y+h-1);
    writeCmd(ILI9341_RAMWR);
    startData();
    uint32_t px = (uint32_t)w * h;
    uint8_t hi = color >> 8, lo = color & 0xFF;
    for (uint32_t i=0; i<px; i++) { SPI.transfer(hi); SPI.transfer(lo); }
    endData();
  }

  // Простой Брезенхем
  void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color) {
    int16_t dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
    int16_t dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
    int16_t err = dx + dy, e2;
    while (true) {
      drawPixel(x0, y0, color);
      if (x0 == x1 && y0 == y1) break;
      e2 = 2 * err;
      if (e2 >= dy) { err += dy; x0 += sx; }
      if (e2 <= dx) { err += dx; y0 += sy; }
    }
  }

private:
  uint8_t _cs, _dc;

  void writeCmd(uint8_t cmd) {
    digitalWrite(_dc, LOW);
    digitalWrite(_cs, LOW);
    SPI.transfer(cmd);
    digitalWrite(_cs, HIGH);
  }
  void writeData8(uint8_t d) {
    digitalWrite(_dc, HIGH);
    digitalWrite(_cs, LOW);
    SPI.transfer(d);
    digitalWrite(_cs, HIGH);
  }
  void writeData16(uint16_t d) {
    digitalWrite(_dc, HIGH);
    digitalWrite(_cs, LOW);
    SPI.transfer(d >> 8); SPI.transfer(d & 0xFF);
    digitalWrite(_cs, HIGH);
  }
  void startData() { digitalWrite(_dc, HIGH); digitalWrite(_cs, LOW); }
  void endData()   { digitalWrite(_cs, HIGH); }

  void setAddrWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
    writeCmd(ILI9341_CASET);
    startData();
    SPI.transfer(x0 >> 8); SPI.transfer(x0 & 0xFF);
    SPI.transfer(x1 >> 8); SPI.transfer(x1 & 0xFF);
    endData();

    writeCmd(ILI9341_PASET);
    startData();
    SPI.transfer(y0 >> 8); SPI.transfer(y0 & 0xFF);
    SPI.transfer(y1 >> 8); SPI.transfer(y1 & 0xFF);
    endData();
  }
};

static inline uint16_t Color565(uint8_t r, uint8_t g, uint8_t b){
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

// ===================== Пины UI/шаговики =====================
#define PIN_POT_X   A0
#define PIN_POT_Y   A1
#define PIN_BTN_SAVE 4
#define PIN_BTN_PLAY 5

#define X_STEP 6
#define X_DIR  7
#define Y_STEP 8
#define Y_DIR  9
#define STEPPERS_EN 10

// ===================== Константы дисплея/цвета =====================
const int16_t W = ILI9341_TFTWIDTH;
const int16_t H = ILI9341_TFTHEIGHT;

const uint16_t BG_COLOR      = Color565(0,0,0);
const uint16_t PATH_COLOR    = Color565(255,255,0);   // статическая жёлтая «тропа»
const uint16_t CURSOR_COLOR  = Color565(255,0,0);
const uint16_t PLAY_COLOR    = Color565(0,255,0);     // зелёный — «перекрашивание» при PLAY

const uint8_t  CURSOR_SZ = 5;

// ====== Параметры «кинематики» симуляции ======
const uint16_t STEP_PULSE_US        = 4;      // чуть длиннее импульс
const uint16_t STEP_SPACING_US      = 400;    // разнос шагов X/Y при диагонали
const uint32_t FEED_DELAY_PER_PX_US = 20000;  // задержка на пиксель (20 мс)

// ===================== Данные =====================
Pt points[200];
uint16_t pointCount = 0;

int16_t curX = W/2, curY = H/2;
int16_t prevX = -1,  prevY = -1;

// ===================== Экран =====================
MiniILI9341 tft(TFT_CS, TFT_DC);

// ===================== Вспомогательные =====================
int16_t map12(int v, int inMin, int inMax, int outMin, int outMax){
  long m = (long)(v - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
  if (m < outMin) m = outMin;
  if (m > outMax) m = outMax;
  return (int16_t)m;
}

static inline void pulsePin(uint8_t pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(STEP_PULSE_US);
  digitalWrite(pin, LOW);
  delayMicroseconds(STEP_PULSE_US);
}

bool buttonPressed(uint8_t pin) {
  static uint32_t tLast[16]={0}; static uint8_t state[16]={0};
  static bool lastRaw[16]={0};
  bool raw = (digitalRead(pin)==LOW);
  uint32_t now=millis();
  const uint16_t DEBOUNCE=20;
  if (raw!=lastRaw[pin]) { tLast[pin]=now; lastRaw[pin]=raw; }
  if (now - tLast[pin] < DEBOUNCE) return false;
  if (raw && !state[pin]) { state[pin]=1; return true; }
  if (!raw && state[pin]) { state[pin]=0; }
  return false;
}

static inline void drawCursor(int16_t x, int16_t y, uint16_t color){
  tft.fillRect(x - CURSOR_SZ/2, y - CURSOR_SZ/2, CURSOR_SZ, CURSOR_SZ, color);
}

static inline bool rectIntersectsLineBox(int16_t rx, int16_t ry, int16_t rw, int16_t rh, const Pt& a, const Pt& b) {
  int16_t minX = (a.x < b.x) ? a.x : b.x;
  int16_t maxX = (a.x > b.x) ? a.x : b.x;
  int16_t minY = (a.y < b.y) ? a.y : b.y;
  int16_t maxY = (a.y > b.y) ? a.y : b.y;
  int16_t rMaxX = rx + rw - 1;
  int16_t rMaxY = ry + rh - 1;
  if (maxX < rx || minX > rMaxX) return false;
  if (maxY < ry || minY > rMaxY) return false;
  return true;
}

void drawSegment(Pt a, Pt b, uint16_t color){
  tft.drawLine(a.x, a.y, b.x, b.y, color);
}

void redrawScene(){
  tft.fillScreen(BG_COLOR);
  for (uint16_t i=1;i<pointCount;i++) drawSegment(points[i-1], points[i], PATH_COLOR);
  drawCursor(curX, curY, CURSOR_COLOR);
  prevX = curX; prevY = curY;
}

// === Ключевая функция: «проигрывание» сегмента по пикселю с реальными шагами ===
void playSegmentBresenham(Pt a, Pt b){
  int dx = abs(b.x - a.x);
  int dy = abs(b.y - a.y);
  int sx = (a.x < b.x) ? 1 : -1;
  int sy = (a.y < b.y) ? 1 : -1;
  int err = dx - dy;

  // Направления осей
  digitalWrite(X_DIR, (sx>0)?HIGH:LOW);
  digitalWrite(Y_DIR, (sy>0)?HIGH:LOW);

  int x=a.x, y=a.y;

  while (true){
    // 1) перекрашиваем текущий пиксель в ЗЕЛЁНЫЙ (поверх уже нарисованной жёлтой траектории)
    tft.drawPixel(x, y, PLAY_COLOR);

    // 2) рассчитываем следующий шаг по Брезенхему
    int e2 = 2*err;
    bool stepX=false, stepY=false;
    if (e2 > -dy){ err -= dy; x += sx; stepX=true; }
    if (e2 <  dx){ err += dx; y += sy; stepY=true; }

    // 3) «реальные» шаги моторов (слегка разнесём по времени для диагонали — приятно глазу)
    if (stepX) { pulsePin(X_STEP); }
    if (stepX && stepY) { delayMicroseconds(STEP_SPACING_US); }
    if (stepY) { pulsePin(Y_STEP); }

    // 4) «скорость подачи» — пауза на один пиксель
    delayMicroseconds(FEED_DELAY_PER_PX_US);

    if (x==b.x && y==b.y) {
      // красим финишный пиксель тоже зелёным
      tft.drawPixel(x, y, PLAY_COLOR);
      break;
    }
  }
}

void playAll(){
  if (pointCount < 2) return;

  // Включаем драйверы и НИЧЕГО не перерисовываем:
  // жёлтые линии уже есть — мы их «заметаем» зелёным при проигрывании
  digitalWrite(STEPPERS_EN, LOW);

  for (uint16_t i=1;i<pointCount;i++) {
    playSegmentBresenham(points[i-1], points[i]);
  }

  digitalWrite(STEPPERS_EN, HIGH);

  // Курсор в конец пути
  curX = points[pointCount-1].x;
  curY = points[pointCount-1].y;
  drawCursor(curX, curY, CURSOR_COLOR);
  prevX = curX; prevY = curY;
}

// ===================== setup/loop =====================
void setup(){
  pinMode(PIN_BTN_SAVE, INPUT_PULLUP);
  pinMode(PIN_BTN_PLAY, INPUT_PULLUP);

  pinMode(X_STEP, OUTPUT);
  pinMode(X_DIR,  OUTPUT);
  pinMode(Y_STEP, OUTPUT);
  pinMode(Y_DIR,  OUTPUT);
  pinMode(STEPPERS_EN, OUTPUT);
  digitalWrite(STEPPERS_EN, HIGH);

  tft.begin();
  tft.fillScreen(BG_COLOR);

  // стартовая отрисовка только курсора
  drawCursor(curX, curY, CURSOR_COLOR);
  prevX = curX; prevY = curY;
}

void loop(){
  // 1) Читаем ползунки (ADC у STM32 — 12 бит)
  int ax = analogRead(PIN_POT_X);
  int ay = analogRead(PIN_POT_Y);
  int16_t nx = map12(ax, 0, 1023, 0, W-1);
  int16_t ny = map12(ay, 0, 1023, 0, H-1);

  // 2) Частичная перерисовка курсора
  if (nx != curX || ny != curY) {
    if (prevX >= 0 && prevY >= 0) {
      int16_t rx = prevX - CURSOR_SZ/2;
      int16_t ry = prevY - CURSOR_SZ/2;
      tft.fillRect(rx, ry, CURSOR_SZ, CURSOR_SZ, BG_COLOR);
      if (pointCount >= 2) {
        for (uint16_t i = 1; i < pointCount; ++i) {
          if (rectIntersectsLineBox(rx, ry, CURSOR_SZ, CURSOR_SZ, points[i-1], points[i])) {
            drawSegment(points[i-1], points[i], PATH_COLOR);
          }
        }
      }
    }
    curX = nx; curY = ny;
    drawCursor(curX, curY, CURSOR_COLOR);
    prevX = curX; prevY = curY;
  }

  // 3) SAVE: добавить точку + дорисовать статический жёлтый сегмент
  if (buttonPressed(PIN_BTN_SAVE)){
    if (pointCount < (sizeof(points)/sizeof(points[0])) ){
      Pt p{curX, curY};
      if (pointCount >= 1) drawSegment(points[pointCount-1], p, PATH_COLOR);
      points[pointCount++] = p;
      tft.fillRect(p.x-1, p.y-1, 3, 3, PATH_COLOR); // метка точки
      drawCursor(curX, curY, CURSOR_COLOR);         // курсор поверх
      prevX = curX; prevY = curY;
    }
  }

  // 4) PLAY: зелёное «заметание» пути с реальными шагами
  if (buttonPressed(PIN_BTN_PLAY)){
    playAll();
  }

  delay(1);
}