#include <SPI.h>

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

// Прототипы
void drawSegmentSlow(Pt a, Pt b, uint16_t color); // Медленная отрисовка
void moveToPoint(Pt target);

// ===================== 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_SET_POINT 4    // Маленькая кнопка - установка точек
#define PIN_BTN_DRAW_PATH 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 CURSOR_COLOR  = Color565(255,0,0);
const uint16_t POINT_COLOR   = Color565(255,255,255);
const uint16_t ORIGIN_COLOR  = Color565(0,255,0);    // Зеленый для начала координат

// Цвета для линий (чередуются)
const uint16_t LINE_COLORS[] = {
  Color565(255,255,0),   // Желтый
  Color565(0,255,255),   // Голубой  
  Color565(255,0,255),   // Пурпурный
  Color565(0,255,0),     // Зеленый
  Color565(255,128,0),   // Оранжевый
  Color565(128,0,255)    // Фиолетовый
};
const uint8_t NUM_COLORS = 6;

const uint8_t  CURSOR_SZ = 5;

// === НАСТРОЙКИ СКОРОСТИ ОТРИСОВКИ ===
const uint16_t STEP_PULSE_US = 10;
const uint16_t STEP_DELAY_US = 5000;  
const float STEPS_PER_PIXEL = 0.5;    
const uint16_t POINT_DELAY_MS = 1000; 

// === НОВЫЕ НАСТРОЙКИ ДЛЯ МЕДЛЕННОЙ ОТРИСОВКИ ЛИНИЙ ===
const uint16_t LINE_DRAW_DELAY_MS = 10 ;// Задержка между точками при рисовании линии

// ===================== Данные =====================
Pt points[200];
uint16_t pointCount = 0;
uint8_t currentColorIndex = 0;  // Индекс текущего цвета

int16_t curX = W/2, curY = H/2;
int16_t prevX = -1,  prevY = -1;
Pt origin = {W/2, H/2};  // Начало координат

// ===================== Экран =====================
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);
}

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 void drawPoint(int16_t x, int16_t y, uint16_t color){
  tft.fillRect(x-1, y-1, 3, 3, color);
}

static inline void drawOrigin(int16_t x, int16_t y){
  tft.fillRect(x-2, y-2, 5, 5, ORIGIN_COLOR);
  // Рисуем крестик для начала координат
  tft.drawLine(x-4, y, x+4, y, ORIGIN_COLOR);
  tft.drawLine(x, y-4, x, y+4, ORIGIN_COLOR);
}

// Быстрая отрисовка линии (оригинальная)
void drawSegment(Pt a, Pt b, uint16_t color){
  tft.drawLine(a.x, a.y, b.x, b.y, color);
}

// МЕДЛЕННАЯ отрисовка линии с задержками
void drawSegmentSlow(Pt a, Pt b, uint16_t color) {
  int16_t dx = abs(b.x - a.x);
  int16_t dy = abs(b.y - a.y);
  int16_t sx = (a.x < b.x) ? 1 : -1;
  int16_t sy = (a.y < b.y) ? 1 : -1;
  int16_t err = dx - dy;

  int16_t x = a.x;
  int16_t y = a.y;

  while (true) {
    // Рисуем текущий пиксель
    tft.drawPixel(x, y, color);
    
    // Если достигли конечной точки - выходим
    if (x == b.x && y == b.y) break;
    
    // Вычисляем следующий шаг по алгоритму Брезенхема
    int16_t e2 = 2 * err;
    if (e2 > -dy) {
      err -= dy;
      x += sx;
    }
    if (e2 < dx) {
      err += dx;
      y += sy;
    }
    
    // ЗАДЕРЖКА между точками для медленной отрисовки
    delay(LINE_DRAW_DELAY_MS);
  }
}

// Функция отрисовки всего маршрута текущим цветом (МЕДЛЕННАЯ)
void drawCurrentRouteSlow() {
  if (pointCount < 2) return;
  
  uint16_t color = LINE_COLORS[currentColorIndex];
  for (uint16_t i = 1; i < pointCount; i++) {
    drawSegmentSlow(points[i-1], points[i], color);
  }
}

// Функция движения к точке с использованием шаговых двигателей
void moveToPoint(Pt target) {
  // Рассчитываем относительные координаты от начала координат
  int16_t relX = target.x - origin.x;
  int16_t relY = target.y - origin.y;
  
  // Рассчитываем количество шагов для каждой оси
  int16_t stepsX = abs(relX) * STEPS_PER_PIXEL;
  int16_t stepsY = abs(relY) * STEPS_PER_PIXEL;
  
  // Устанавливаем направление вращения двигателей
  digitalWrite(X_DIR, (relX >= 0) ? HIGH : LOW);
  digitalWrite(Y_DIR, (relY >= 0) ? HIGH : LOW);
  
  // Включаем драйверы двигателей
  digitalWrite(STEPPERS_EN, LOW);
  
  // Двигаемся по алгоритму Брезенхема для плавного движения по диагонали
  int16_t dx = stepsX;
  int16_t dy = stepsY;
  int16_t err = 0;
  
  int16_t x = 0, y = 0;
  
  while (x < stepsX || y < stepsY) {
    if (x < stepsX) {
      err += dy;
      if (err >= dx) {
        err -= dx;
        pulsePin(Y_STEP);
        y++;
        delayMicroseconds(STEP_DELAY_US);
      }
      pulsePin(X_STEP);
      x++;
      delayMicroseconds(STEP_DELAY_US);
    } else {
      pulsePin(Y_STEP);
      y++;
      delayMicroseconds(STEP_DELAY_US);
    }
  }
  
  digitalWrite(STEPPERS_EN, HIGH);
  curX = target.x;
  curY = target.y;
}

// Функция прохождения всего маршрута
void runRoute() {
  if (pointCount < 2) return;
  
  for (uint16_t i = 0; i < pointCount; i++) {
    moveToPoint(points[i]);
    delay(POINT_DELAY_MS);
    
    drawPoint(points[i].x, points[i].y, LINE_COLORS[currentColorIndex]);
    delay(500);
    drawPoint(points[i].x, points[i].y, POINT_COLOR);
  }
  
  drawCursor(curX, curY, CURSOR_COLOR);
  prevX = curX; prevY = curY;
}

// ===================== setup/loop =====================
void setup(){
  pinMode(PIN_BTN_SET_POINT, INPUT_PULLUP);
  pinMode(PIN_BTN_DRAW_PATH, 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);

  drawOrigin(origin.x, origin.y);
  drawCursor(curX, curY, CURSOR_COLOR);
  prevX = curX; prevY = curY;
}

void loop(){
  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);

  if (nx != curX || ny != curY) {
    if (prevX >= 0 && prevY >= 0) {
      tft.fillRect(prevX - CURSOR_SZ/2, prevY - CURSOR_SZ/2, CURSOR_SZ, CURSOR_SZ, BG_COLOR);
      
      for (uint16_t i = 0; i < pointCount; i++) {
        if (abs(points[i].x - prevX) <= CURSOR_SZ/2 && abs(points[i].y - prevY) <= CURSOR_SZ/2) {
          drawPoint(points[i].x, points[i].y, POINT_COLOR);
        }
      }
      
      if (abs(origin.x - prevX) <= CURSOR_SZ/2 && abs(origin.y - prevY) <= CURSOR_SZ/2) {
        drawOrigin(origin.x, origin.y);
      }
    }
    
    curX = nx; curY = ny;
    drawCursor(curX, curY, CURSOR_COLOR);
    prevX = curX; prevY = curY;
  }

  if (buttonPressed(PIN_BTN_SET_POINT)){
    if (pointCount < (sizeof(points)/sizeof(points[0])) ){
      Pt p{curX, curY};
      
      if (pointCount == 0) {
        origin = p;
        drawOrigin(origin.x, origin.y);
      }
      
      // ИСПОЛЬЗУЕМ МЕДЛЕННУЮ ОТРИСОВКУ при добавлении новой точки
      if (pointCount >= 1) {
        drawSegmentSlow(points[pointCount-1], p, LINE_COLORS[currentColorIndex]);
      }
      
      points[pointCount++] = p;
      drawPoint(curX, curY, POINT_COLOR);
      drawCursor(curX, curY, CURSOR_COLOR);
      prevX = curX; prevY = curY;
    }
  }

  if (buttonPressed(PIN_BTN_DRAW_PATH)){
    if (pointCount >= 2) {
      currentColorIndex = (currentColorIndex + 1) % NUM_COLORS;
      
      // ИСПОЛЬЗУЕМ МЕДЛЕННУЮ ОТРИСОВКУ при перерисовке маршрута
      drawCurrentRouteSlow();
      
      runRoute();
    }
  }

  delay(3);
}