#include <NewPing.h>
#include <GyverOLED.h>

constexpr uint8_t  PIN_MASTER_SIGNAL = 2;
constexpr uint8_t  PIN_END_SIGNAL    = A0;
constexpr uint8_t  PIN_ACTIVE_LED    = 13;
constexpr uint32_t DEBOUNCE_MS       = 300;         

constexpr uint8_t POT_L = A1;
constexpr uint8_t POT_R = A2;
constexpr uint8_t TRIG_L  = 8;
constexpr uint8_t ECHO_L  = 7;
constexpr uint8_t RGB_L_R = 11;
constexpr uint8_t RGB_L_G = 10;
constexpr uint8_t RGB_L_B = 9;

constexpr uint8_t TRIG_R  = 3;
constexpr uint8_t ECHO_R  = 12;
constexpr uint8_t RGB_R_R = 6;
constexpr uint8_t RGB_R_G = 5;
constexpr uint8_t RGB_R_B = 4;

const uint8_t Codice[4] = {9, 9, 9, 9};

static const uint8_t patterns[6][8][8] PROGMEM = {
{  // A
    {0,0,1,1,1,0,0,0},
    {0,1,0,0,0,1,0,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {1,1,1,1,1,1,1,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {0,0,0,0,0,0,0,0}
  },
  {  // B
    {1,1,1,1,1,1,0,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {1,1,1,1,1,1,0,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {1,1,1,1,1,1,0,0},
    {0,0,0,0,0,0,0,0}
  },
  {  // C
    {0,0,1,1,1,1,0,0},
    {0,1,0,0,0,0,1,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {0,1,0,0,0,0,1,0},
    {0,0,1,1,1,1,0,0},
    {1,0,0,0,0,0,0,1}
  },
  {  // D
    {1,1,1,1,1,0,0,0},
    {1,0,0,0,0,1,0,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,0,1,0},
    {1,0,0,0,0,1,0,0},
    {1,1,1,1,1,0,0,0},
    {0,0,0,0,0,0,0,0}
  },
  {  // E
    {1,1,1,1,1,1,1,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,1,1,1,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,1,1,1,1,1,1,0},
    {0,0,0,0,0,0,0,0}
  },
  {  // F
    {1,1,1,1,1,1,1,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,1,1,1,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {1,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0}
  }
};

struct VictoryAnim {
    bool     active       = false;
    uint8_t  letterIdx    = 0;          
    uint32_t lastSwitch   = 0;
    uint32_t switchInterval = 1500;     
    uint32_t startTime    = 0;
} victory;

constexpr uint8_t VICTORY_SPACING = 6;
constexpr uint8_t VICTORY_RADIUS  = 2;

enum class SlaveState : uint8_t {
    IDLE,
    PHASE1,
    PHASE2,
    DONE,
    ABORT
};

struct Slave {
    SlaveState state      = SlaveState::IDLE;
    uint32_t   phaseStart = 0;
} slave;

struct GameConfig {
    static constexpr unsigned long MAX_PING          = 6000;   
    static constexpr unsigned long TARGET_L          = 1800;    
    static constexpr unsigned long TARGET_R          = 2100;   
    static constexpr unsigned long TOLERANCE         =  300;    
    static constexpr unsigned long FAR_THRESHOLD     = 5000;   
    static constexpr uint8_t   HOLD_TIME_SEC         = 5;

    static constexpr unsigned long MEAS_INTERVAL     = 100;
    static constexpr unsigned long BLINK_FAST        = 40;
    static constexpr unsigned long GREEN_BASE        = 500;
    static constexpr unsigned long RAINBOW_INTERVAL  = 50;
    static constexpr unsigned long STATE_DRAW_IV     = 100;

    static constexpr uint8_t   BAR_Y                = 0;
    static constexpr uint8_t   BAR_HEIGHT           = 6;
    static constexpr uint8_t   BAR_MAX_WIDTH        = 128;

    static constexpr uint8_t   SYMBOL_AREA_Y        = 12;
    static constexpr uint8_t   SYMBOL_HEIGHT        = 18;
    static constexpr uint8_t   SYMBOL_CENTER_XL     = 32;
    static constexpr uint8_t   SYMBOL_CENTER_XR     = 96;
    static constexpr uint8_t   TARGET_LINE_Y        = SYMBOL_AREA_Y + SYMBOL_HEIGHT / 2;

    static constexpr uint8_t   DIST_Y               = 45;
    static constexpr uint8_t   DIST_SCALE           = 1;

    static constexpr unsigned long SCALE_FOR_DISPLAY = 100000UL;


};

GyverOLED<SSD1306_128x64> oled;
NewPing sonarL(TRIG_L, ECHO_L, 100);   
NewPing sonarR(TRIG_R, ECHO_R, 100);

struct SideState {
    unsigned long pingUs     = GameConfig::MAX_PING;
    unsigned long tRangeStart = 0;
    bool inRange = false;

    void updateRange(bool nowInRange, unsigned long now) {
        if (nowInRange) {
            if (!inRange) tRangeStart = now;
            inRange = true;
        } else {
            inRange = false;
        }
    }
};

struct TimerState {
    bool active = false;
    unsigned long tStart = 0;
    uint8_t holdSeconds = 0;
};

SideState left, right;
TimerState timer;
GameConfig cfg;

bool won = false;
bool ledBlinkState = false;
bool displayDirty = false;

unsigned long tLastMeasure   = 0;
unsigned long tLastBlink     = 0;
unsigned long tLastRainbow   = 0;
unsigned long tLastStateDraw = 0;

inline bool inRange(unsigned long ping, unsigned long target) {
    return ping >= target - cfg.TOLERANCE 
        && ping <= target + cfg.TOLERANCE;
}

void setRGB(uint8_t side, uint8_t r, uint8_t g, uint8_t b) {
    if (side == 0) {
        analogWrite(RGB_L_R, r);
        analogWrite(RGB_L_G, g);
        analogWrite(RGB_L_B, b);
    } else {
        analogWrite(RGB_R_R, r);
        analogWrite(RGB_R_G, g);
        analogWrite(RGB_R_B, b);
    }
}

void updateLED(uint8_t side, bool isInRange, unsigned long pingUs, bool timerActive) {
    if (timerActive) {
        setRGB(side, 0, ledBlinkState ? 220 : 0, 0);
    } else if (isInRange) {
        setRGB(side, 0, 220, 0);
    } else if (pingUs > cfg.FAR_THRESHOLD) {
        setRGB(side, 0, 0, ledBlinkState ? 220 : 0);
    } else {
        setRGB(side, 220, 0, 0);
    }
}

void rainbow() {
    uint8_t h = millis() >> 6;
    uint8_t r = 0, g = 0, b = 0;
    uint8_t sector = h / 43;
    uint8_t offset = (h % 43) * 6;

    switch (sector) {
        case 0: r = 255; g = offset; break;
        case 1: r = 255 - offset; g = 255; break;
        case 2: g = 255; b = offset; break;
        case 3: g = 255 - offset; b = 255; break;
        case 4: r = offset; b = 255; break;
        default: r = 255; b = 255 - offset; break;
    }
    setRGB(0, r, g, b);
    setRGB(1, r, g, b);
}

void drawBar(uint8_t sec) {
    static int lastWidth = -1;
    uint8_t width = min(sec, 5) * (cfg.BAR_MAX_WIDTH / 5);
    if (width == lastWidth) return;
    lastWidth = width;

    oled.rect(0, cfg.BAR_Y, cfg.BAR_MAX_WIDTH, cfg.BAR_HEIGHT, OLED_CLEAR);
    oled.rect(0, cfg.BAR_Y, width, cfg.BAR_HEIGHT, OLED_FILL);
    displayDirty = true;
}

void drawSideSymbol(uint8_t cx, bool inRangeNow, long deviation) {
    oled.line(cx - 10, cfg.TARGET_LINE_Y, cx + 10, cfg.TARGET_LINE_Y);

    if (inRangeNow) {
        oled.line(cx - 7, cfg.SYMBOL_AREA_Y + 2, 
                  cx + 7, cfg.SYMBOL_AREA_Y + cfg.SYMBOL_HEIGHT - 2);
        oled.line(cx - 7, cfg.SYMBOL_AREA_Y + cfg.SYMBOL_HEIGHT - 2, 
                  cx + 7, cfg.SYMBOL_AREA_Y + 2);
        return;
    }

    oled.line(cx, cfg.SYMBOL_AREA_Y, cx, cfg.SYMBOL_AREA_Y + cfg.SYMBOL_HEIGHT);

    int offset = (abs(deviation) / 350) % 13;
    if (offset > 10) offset = 20 - offset;
    if ((millis() >> (cx >> 5)) & 1) offset = -offset;

    int dir = deviation > 0 ? 1 : -1;
    int y = constrain(cfg.TARGET_LINE_Y + offset * dir,
                      cfg.SYMBOL_AREA_Y + 2,
                      cfg.SYMBOL_AREA_Y + cfg.SYMBOL_HEIGHT - 3);

    oled.line(cx - 6, y, cx + 6, y);
}

void drawState() {
    oled.rect(0, cfg.SYMBOL_AREA_Y - 4, 128, cfg.SYMBOL_HEIGHT + 8, OLED_CLEAR);

    drawSideSymbol(cfg.SYMBOL_CENTER_XL, left.inRange,
                   (long)left.pingUs - cfg.TARGET_L);

    drawSideSymbol(cfg.SYMBOL_CENTER_XR, right.inRange,
                   (long)right.pingUs - cfg.TARGET_R);

    displayDirty = true;
}

void updateDistanceDisplay() {
    char buf[32];

    unsigned long displayL = left.pingUs;
    unsigned long displayR = right.pingUs;

    if (left.pingUs == 0 || left.pingUs >= cfg.FAR_THRESHOLD) {
        displayL = 0;
    }
    if (right.pingUs == 0 || right.pingUs >= cfg.FAR_THRESHOLD) {
        displayR = 0;
    }

    unsigned long showL = (displayL * cfg.SCALE_FOR_DISPLAY) / 100UL; 
    unsigned long showR = (displayR * cfg.SCALE_FOR_DISPLAY) / 100UL;

    snprintf(buf, sizeof(buf), "%08lu  %08lu", showL, showR);

    oled.rect(0, cfg.DIST_Y, 128, 64 - cfg.DIST_Y, OLED_CLEAR);
    oled.setScale(cfg.DIST_SCALE);
    oled.setCursorXY(8, cfg.DIST_Y);
    oled.print(buf);

    displayDirty = true;
}

void victoryCode(uint32_t now) {
    if (!victory.active) {
        victory.active     = true;
        victory.letterIdx  = 0;
        victory.lastSwitch = now;
        victory.startTime  = now;
        oled.clear();
        oled.update();
        displayDirty = true;   
        return;
    }

    if (now - victory.lastSwitch >= victory.switchInterval) {
        victory.letterIdx = (victory.letterIdx + 1) % 6;
        victory.lastSwitch = now;

    }

    oled.clear();   

    int offsetX = (128 - 8 * VICTORY_SPACING) / 2;  
    int offsetY = (64  - 8 * VICTORY_SPACING) / 2;  

    uint8_t idx = victory.letterIdx;

    for (uint8_t row = 0; row < 8; row++) {
        for (uint8_t col = 0; col < 8; col++) {
            uint8_t bit = pgm_read_byte(&patterns[idx][row][col]);

            int x = offsetX + col * VICTORY_SPACING;
            int y = offsetY + row * VICTORY_SPACING;

            if (bit) {
                oled.circle(x + VICTORY_RADIUS, y + VICTORY_RADIUS, VICTORY_RADIUS, OLED_FILL);
            } else {
                // oled.circle(x + VICTORY_RADIUS, y + VICTORY_RADIUS, VICTORY_RADIUS, OLED_CLEAR);
            }
        }
    }


    oled.update();
    displayDirty = true;
}

bool phase1(uint32_t now) {
    uint32_t elapsed = now - slave.phaseStart;

    static int last_codeLeft  = -1;
    static int last_codeRight = -1;

    int valL = analogRead(POT_L);
    int valR = analogRead(POT_R);

    int codeLeft  = map(valL, 0, 1023, 0, 99);
    int codeRight = map(valR, 0, 1023, 0, 99);

    if (codeLeft != last_codeLeft || codeRight != last_codeRight) {
        last_codeLeft  = codeLeft;
        last_codeRight = codeRight;

        uint8_t d1 = codeLeft  / 10;
        uint8_t d2 = codeLeft  % 10;
        uint8_t d3 = codeRight / 10;
        uint8_t d4 = codeRight % 10;

        char buf[16];
        snprintf(buf, sizeof(buf), "%d%d%d%d", d4, d2, d3, d1);

        oled.rect(0, 25, 128, 35, OLED_CLEAR);  
        oled.setScale(3);
        oled.setCursorXY(30, 20);   
        oled.print(buf);

        oled.update();
    }

    bool isCorrect = (codeLeft / 10  == Codice[3]) &&
                     (codeLeft % 10  == Codice[1]) &&
                     (codeRight / 10 == Codice[2]) &&
                     (codeRight % 10 == Codice[0]);

    return isCorrect;
}

bool phase2(uint32_t now) {
    if (!won && now - tLastMeasure >= cfg.MEAS_INTERVAL) {
        tLastMeasure = now;

        unsigned long usL = sonarL.ping_median(5);
        unsigned long usR = sonarR.ping_median(5);

        left.pingUs  = usL ? usL : cfg.MAX_PING;
        right.pingUs = usR ? usR : cfg.MAX_PING;

        updateDistanceDisplay();
    }

    bool inL = inRange(left.pingUs,  cfg.TARGET_L);
    bool inR = inRange(right.pingUs, cfg.TARGET_R);
    bool bothIn = inL && inR;

    left.updateRange(inL, now);
    right.updateRange(inR, now);

    if (bothIn && !won) {
        if (!timer.active) {
            unsigned long earliest = min(left.tRangeStart, right.tRangeStart);
            if (now - earliest >= 1000) {
                timer.active = true;
                timer.tStart = now;
                timer.holdSeconds = 0;
                drawBar(0);
                displayDirty = true;
            }
        } else {
            uint8_t sec = (now - timer.tStart) / 1000;
            if (sec > timer.holdSeconds) {
                timer.holdSeconds = sec;
                drawBar(sec);

                if (sec >= cfg.HOLD_TIME_SEC) {
                    won = true;
                    oled.clear();
                    oled.setScale(4);
                    oled.setCursorXY(25, 12);
                    oled.print("WIN");
                    oled.update();
                    return true;
                }
            }
        }
    } else if (timer.active || timer.holdSeconds) {
        timer = {};
        drawBar(0);
        displayDirty = true;
    }

    if (!won && now - tLastStateDraw >= cfg.STATE_DRAW_IV) {
        tLastStateDraw = now;
        drawState();
    }

    if (!won) {
        unsigned long blinkIv = timer.active
            ? max(cfg.GREEN_BASE - timer.holdSeconds * 100UL, 120UL)
            : cfg.BLINK_FAST;

        if (now - tLastBlink >= blinkIv) {
            tLastBlink = now;
            ledBlinkState = !ledBlinkState;
        }

        updateLED(0, inL, left.pingUs,  timer.active);
        updateLED(1, inR, right.pingUs, timer.active);
    }

    if (displayDirty) {
        oled.update();
        displayDirty = false;
    }
    return false;
}

void setup() {
    pinMode(PIN_MASTER_SIGNAL, INPUT);
    pinMode(PIN_END_SIGNAL,    OUTPUT);
    pinMode(PIN_ACTIVE_LED,    OUTPUT);
    
    digitalWrite(PIN_END_SIGNAL, LOW);
    digitalWrite(PIN_ACTIVE_LED,  LOW);

    pinMode(RGB_L_R, OUTPUT); pinMode(RGB_L_G, OUTPUT); pinMode(RGB_L_B, OUTPUT);
    pinMode(RGB_R_R, OUTPUT); pinMode(RGB_R_G, OUTPUT); pinMode(RGB_R_B, OUTPUT);

    setRGB(0, 0, 0, 0);
    setRGB(1, 0, 0, 0);

    oled.init();
    oled.clear();
    oled.update();
}

void loop() {
    uint32_t now    = millis();
    bool     master = digitalRead(PIN_MASTER_SIGNAL);

    if (!master) {
        if (slave.state != SlaveState::ABORT &&
            slave.state != SlaveState::IDLE) {
            
            slave.state = SlaveState::ABORT;
            digitalWrite(PIN_END_SIGNAL, LOW);
            digitalWrite(PIN_ACTIVE_LED, LOW);
            setRGB(0, 0, 0, 0);
            setRGB(1, 0, 0, 0);
            oled.clear();
            oled.update();

        }
        return;
    }

    switch (slave.state) {

        case SlaveState::IDLE: {
            static bool     prev = LOW;
            static uint32_t riseTime = 0;

            if (master && !prev) {
                riseTime = now;
            }
            prev = master;

            if (master && (now - riseTime >= DEBOUNCE_MS)) {
                slave.state      = SlaveState::PHASE1;
                slave.phaseStart = now;
                digitalWrite(PIN_ACTIVE_LED, HIGH);
            }
            break;
        }

        case SlaveState::PHASE1:
            if (phase1(now)) {
                oled.clear();
                oled.update();
                slave.state      = SlaveState::PHASE2;
                slave.phaseStart = now;

                oled.clear();
                updateDistanceDisplay();
                drawBar(0);
                drawState();
                oled.update();
            }
            break;

        case SlaveState::PHASE2:
            if (phase2(now)) {
                digitalWrite(PIN_ACTIVE_LED, LOW);     
                slave.state = SlaveState::DONE;
                digitalWrite(PIN_END_SIGNAL, HIGH);
                setRGB(0, 0, 0, 0);
                setRGB(1, 0, 0, 0);
            }
            break;

        case SlaveState::DONE: {
            static uint32_t lastStableHigh = 0;

            if (master) {
                lastStableHigh = now;           
                digitalWrite(PIN_END_SIGNAL, HIGH);
            } else {
                if (now - lastStableHigh >= DEBOUNCE_MS) {
                    digitalWrite(PIN_END_SIGNAL, LOW);
                }
            }

            victoryCode(now);            

            if (now - tLastRainbow >= cfg.RAINBOW_INTERVAL) {
                  tLastRainbow = now;
                  rainbow();
              }

        }            
        break;

        case SlaveState::ABORT:
            digitalWrite(PIN_END_SIGNAL, LOW);
            digitalWrite(PIN_ACTIVE_LED, LOW);
            setRGB(0, 0, 0, 0);
            setRGB(1, 0, 0, 0);
            oled.clear();
            oled.update();

            while (1);
            break;
    }
}