// Car — 14-port board, four DC motors (the kit car), Distance Sensor, Screen, Button.
// Tap the button = drive / stop. It drives forward; anything closer than 20 cm makes it stop, back up, turn right and carry on.
// Hold the button 1 s = slow mode on/off (half speed). The screen shows the distance in cm and what the car is doing.
#include <Arduino.h>
#include <Wire.h>
#include "modules/display/st7735_tft.h"
#include "modules/input/button.h"
#include "modules/motor/dc_motor.h"
#include "modules/distance/vl53l5cx.h"

ST7735_TFT      screen(SCR_SCLK, SCR_CS, SCR_MOSI, SCR_DC);
ButtonModule    button(BTN_SW);
VL53L5CX_Sensor distance(Wire);
DCMotor         motors[4] = { {M1A, M1B}, {M2A, M2B}, {M3A, M3B}, {M4A, M4B} };   // left top, left bottom, right top, right bottom
const int       MOTOR_SIGN[4] = { 1, 1, -1, -1 };                                   // right side faces the other way

static const int      STOP_CM   = 20;     // closer than this = obstacle
static const uint32_t BACK_MS   = 400;    // reverse this long
static const uint32_t TURN_MS   = 500;    // then turn right this long
static const int      FULL      = 200;    // forward speed (0..255)

enum State { READY, DRIVE, BACK, TURN };
State    state = READY;
bool     slow = false, sensorOk = false;
int      cm = -1;                          // last distance in cm, -1 = no reading
uint32_t stateAt = 0, lastDraw = 0;
const char* NAME[] = { "READY", "DRIVE", "BACK", "TURN" };

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

void drive(int left, int right) {
    for (int i = 0; i < 4; i++) { int v = (i < 2 ? left : right) * MOTOR_SIGN[i]; motors[i].setSpeed((int16_t)v); }
}
void stopCar() { for (auto& m : motors) m.brake(); }
int  speed() { return slow ? FULL / 2 : FULL; }

void setState(State s) {
    state = s; stateAt = millis();
    int v = speed();
    if (s == READY)      stopCar();
    else if (s == DRIVE) drive(v, v);
    else if (s == BACK)  drive(-v, -v);
    else                 drive(v, -v);     // right turn: left wheels forward, right wheels back
    digitalWrite(BTN_LED, s == READY ? LOW : HIGH);
    Serial.printf("[car] %s  (%d cm%s)\n", NAME[s], cm, slow ? ", slow" : "");
}

// Nearest thing in the middle 4x4 of the 8x8 grid (the outer zones see the floor and the wheels), in cm.
int readCm() {
    if (!sensorOk || !distance.isReady()) return -1;
    int16_t grid[VL53L5CX_Sensor::GRID_SIZE]; distance.getDistanceGrid(grid);
    int16_t best = 32767;
    for (int r = 2; r < 6; r++) for (int c = 2; c < 6; c++) { int16_t d = grid[r * 8 + c]; if (d > 0 && d < best) best = d; }
    return best == 32767 ? -1 : (best + 5) / 10;
}

void draw() {
    screen.fillScreen(ST7735_TFT::COLOR_BLACK);
    char buf[16];
    if (cm < 0) snprintf(buf, sizeof buf, "--"); else snprintf(buf, sizeof buf, "%d", cm);
    uint16_t ink = cm >= 0 && cm < STOP_CM ? rgb(255, 60, 50) : ST7735_TFT::COLOR_WHITE;
    screen.setTextSize(5); screen.setTextColor(ink);
    int w = screen.textWidth(buf); screen.setCursor((160 - w) / 2 - 10, 4); screen.print(buf);
    screen.setTextSize(1); screen.setTextColor(rgb(170, 170, 170)); screen.setCursor((160 + w) / 2 - 6, 30); screen.print("cm");
    screen.setTextSize(2); screen.setTextColor(state == READY ? rgb(170, 170, 170) : rgb(90, 210, 110));
    const char* word = NAME[state]; w = screen.textWidth(word); screen.setCursor((160 - w) / 2, 52); screen.print(word);
    screen.setTextSize(1); screen.setTextColor(rgb(100, 100, 100)); screen.setCursor(2, 70);
    screen.print(!sensorOk ? "no distance sensor" : slow ? "slow mode" : "tap = go/stop  hold = slow");
    screen.display();
}

void setup() {
    Serial.begin(115200);
    pinMode(BTN_LED, OUTPUT); digitalWrite(BTN_LED, LOW);
    screen.begin(); screen.setRotation(1);
    button.begin(); button.setGestureTiming(400, 1000);
    for (auto& m : motors) m.begin();
    stopCar();
    Wire.begin(DIST_SDA, DIST_SCL); Wire.setClock(400000);
    sensorOk = distance.begin();
    draw();
    Serial.printf("[car] ready, sensor %s, tap = drive, hold 1 s = slow mode\n", sensorOk ? "ok" : "MISSING");
}

void loop() {
    button.update();
    AtechGesture g = button.getGesture();
    uint32_t now = millis();
    cm = readCm();

    if (g == GESTURE_TAP) setState(state == READY ? DRIVE : READY);
    if (g == GESTURE_LONG_PRESS) { slow = !slow; Serial.printf("[car] slow mode %s\n", slow ? "on" : "off"); if (state != READY) setState(state); }

    if (state == DRIVE && cm >= 0 && cm < STOP_CM) setState(BACK);
    else if (state == BACK && now - stateAt > BACK_MS) setState(TURN);
    else if (state == TURN && now - stateAt > TURN_MS) setState(DRIVE);

    if (now - lastDraw > 150) { lastDraw = now; draw(); }
    delay(5);
}