/*
 * ============================================================
 *  ELEVATOR SCHEDULER  –  SCAN FSM Controller
 *  Target: ST Nucleo-C031C6  |  Simulator: Wokwi
 *  Author: rewritten for correctness
 * ============================================================
 *
 *  FSM States : IDLE, MOVING_UP, MOVING_DOWN, DOOR_OPEN, OVERLOAD
 *  Scheduling : SCAN (elevator algorithm) on uint8_t bitmask
 *  Timers     : millis()-based, ZERO delay() calls
 *  Inputs     : 4 cabin btns, 7 hall btns, slide-switch door,
 *               potentiometer weight sensor
 *  Outputs    : 4 floor LEDs, 2 direction LEDs, buzzer, LCD 16x2
 */

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

/* ============================================================
 *  COMPILE-TIME CONFIGURATION
 * ============================================================ */
#define NUM_FLOORS          4
#define FLOOR_TRAVEL_MS     1500UL   /* ms to move one floor          */
#define DOOR_OPEN_MS        2000UL   /* ms door stays open            */
#define DEBOUNCE_MS         50UL     /* button debounce window        */
#define WEIGHT_MAX_KG       500      /* pot full-scale kg             */
#define OVERLOAD_KG         400      /* overload threshold kg         */
#define LCD_UPDATE_MS       200UL    /* LCD refresh interval          */
#define WEIGHT_SAMPLES      4        /* moving-avg samples for pot    */

/* ============================================================
 *  PIN ASSIGNMENTS
 * ============================================================ */
/* Cabin buttons (INPUT_PULLUP – active LOW) */
static const uint8_t PIN_CABIN[NUM_FLOORS] = { PA_0, PA_1, PA_2, PA_3 };

/*
 * Hall UP buttons  – floors 0,1,2  (no UP on floor 3)
 * Hall DOWN buttons – floors 1,2,3  (no DOWN on floor 0)
 */
static const uint8_t PIN_HALL_UP[NUM_FLOORS]   = { PA_4, PA_5, PA_6, 0xFF }; /* 0xFF = no pin */
static const uint8_t PIN_HALL_DOWN[NUM_FLOORS] = { 0xFF, PA_7, PB_0, PB_1 };

/* Sensors */
#define PIN_DOOR_SENSOR   PB_2    /* slide switch, INPUT_PULLUP       */
#define PIN_WEIGHT        PA_9    /* potentiometer, analog            */

/* Outputs */
static const uint8_t PIN_FLOOR_LED[NUM_FLOORS] = { PC_0, PC_1, PC_2, PC_3 };
#define PIN_LED_UP        PC_4
#define PIN_LED_DOWN      PC_5
#define PIN_BUZZER        PB_12
/* ============================================================
 *  FSM STATE DEFINITION
 * ============================================================ */
typedef enum {
    STATE_IDLE        = 0,
    STATE_MOVING_UP   = 1,
    STATE_MOVING_DOWN = 2,
    STATE_DOOR_OPEN   = 3,
    STATE_OVERLOAD    = 4
} ElevatorState;

/* ============================================================
 *  GLOBAL STATE
 * ============================================================ */
static ElevatorState gState     = STATE_IDLE;
static ElevatorState gPrevState = STATE_IDLE; /* for overload resume */
static int8_t        gFloor     = 0;          /* current floor 0-3   */
static int8_t        gDir       = 1;          /* +1=up, -1=down      */

/* Request bitmask: bit N = floor N has a pending request */
static uint8_t       gRequests  = 0x00;

/* millis() timers */
static uint32_t gFloorTimer  = 0;
static uint32_t gDoorTimer   = 0;
static uint32_t gLcdTimer    = 0;

/* Button debounce: last raw read and timestamp per logical button
 * Order: cabin0-3, hallUp0-2, hallDown1-3  (10 buttons total)    */
#define NUM_BUTTONS 10
static uint8_t   gBtnPin[NUM_BUTTONS];   /* filled in setup()        */
static uint8_t   gBtnLast[NUM_BUTTONS];  /* last stable state        */
static uint32_t  gBtnTime[NUM_BUTTONS];  /* timestamp of last change */

/* Weight */
static uint16_t gWeightKg = 0;

/* LCD object (I2C addr 0x27, 16 cols, 2 rows) */
static LiquidCrystal_I2C lcd(0x27, 16, 2);

/* ============================================================
 *  UTILITY: REQUEST BITMASK
 * ============================================================ */
static inline void setRequest(int8_t floor)
{
    if (floor >= 0 && floor < NUM_FLOORS)
        gRequests |= (uint8_t)(1u << floor);
}

static inline void clearRequest(int8_t floor)
{
    if (floor >= 0 && floor < NUM_FLOORS)
        gRequests &= (uint8_t)~(1u << floor);
}

static inline bool hasRequest(int8_t floor)
{
    if (floor < 0 || floor >= NUM_FLOORS) return false;
    return (gRequests & (uint8_t)(1u << floor)) != 0;
}

static inline bool anyRequest(void)
{
    return gRequests != 0;
}

/* ============================================================
 *  UTILITY: SCAN – next floor to visit
 *  Returns -1 if no requests exist.
 *  Scans ahead in gDir first; if nothing found, reverses.
 * ============================================================ */
static int8_t scanNextFloor(void)
{
    /* scan in current direction */
    if (gDir == 1) {
        for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++) {
            if (hasRequest(f)) return f;
        }
        /* reverse: scan downward */
        for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--) {
            if (hasRequest(f)) return f;
        }
    } else {
        for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--) {
            if (hasRequest(f)) return f;
        }
        /* reverse: scan upward */
        for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++) {
            if (hasRequest(f)) return f;
        }
    }
    return -1; /* nothing */
}

/* ============================================================
 *  WEIGHT SENSOR – moving average on analog pot
 * ============================================================ */
static void readWeight(void)
{
    static uint32_t samples[WEIGHT_SAMPLES] = {0};
    static uint8_t  idx = 0;
    static uint32_t sum = 0;

    sum -= samples[idx];
    uint32_t raw = (uint32_t)analogRead(PIN_WEIGHT);
    samples[idx] = raw;
    sum += raw;
    idx = (uint8_t)((idx + 1) % WEIGHT_SAMPLES);

    /* analogRead on STM32: 12-bit (0-4095) */
    gWeightKg = (uint16_t)((sum / WEIGHT_SAMPLES) * WEIGHT_MAX_KG / 4095u);
}

/* ============================================================
 *  BUTTON DEBOUNCE INIT
 * ============================================================ */
static void initButtons(void)
{
    uint8_t idx = 0;

    /* Cabin buttons 0-3 */
    for (uint8_t i = 0; i < NUM_FLOORS; i++) {
        gBtnPin[idx++] = PIN_CABIN[i];
    }
    /* Hall UP buttons (skip 0xFF = floor 3) */
    for (uint8_t i = 0; i < NUM_FLOORS; i++) {
        if (PIN_HALL_UP[i] != 0xFF)
            gBtnPin[idx++] = PIN_HALL_UP[i];
    }
    /* Hall DOWN buttons (skip 0xFF = floor 0) */
    for (uint8_t i = 0; i < NUM_FLOORS; i++) {
        if (PIN_HALL_DOWN[i] != 0xFF)
            gBtnPin[idx++] = PIN_HALL_DOWN[i];
    }

    for (uint8_t i = 0; i < NUM_BUTTONS; i++) {
        pinMode(gBtnPin[i], INPUT_PULLUP);
        gBtnLast[i] = HIGH; /* unpressed state for PULLUP */
        gBtnTime[i] = 0;
    }
}

/* ============================================================
 *  BUTTON POLL – debounced, fires once on press (HIGH→LOW)
 *  Returns bitmask of buttons that fired this call (by index).
 * ============================================================ */
static uint16_t pollButtons(void)
{
    uint32_t now = millis();
    uint16_t fired = 0;

    for (uint8_t i = 0; i < NUM_BUTTONS; i++) {
        uint8_t reading = (uint8_t)digitalRead(gBtnPin[i]);
        if (reading != gBtnLast[i]) {
            gBtnTime[i] = now; /* start debounce window */
            gBtnLast[i] = reading;
        }
        if ((now - gBtnTime[i]) >= DEBOUNCE_MS) {
            /* stable LOW → pressed edge */
            if (reading == LOW) {
                fired |= (uint16_t)(1u << i);
            }
        }
    }
    return fired;
}

/*
 * Map button index back to a floor request.
 * Index 0-3   → cabin  floor 0-3
 * Index 4-6   → hall UP  floor 0-2
 * Index 7-9   → hall DOWN floor 1-3
 */
static void dispatchButtonFired(uint16_t fired)
{
    for (uint8_t i = 0; i < NUM_BUTTONS; i++) {
        if (!(fired & (uint16_t)(1u << i))) continue;

        if (i < 4) {
            /* cabin button: floor = index */
            setRequest((int8_t)i);
        } else if (i < 7) {
            /* hall UP: index 4→floor0, 5→floor1, 6→floor2 */
            setRequest((int8_t)(i - 4));
        } else {
            /* hall DOWN: index 7→floor1, 8→floor2, 9→floor3 */
            setRequest((int8_t)(i - 6));
        }
    }
}

/* ============================================================
 *  OUTPUT HELPERS
 * ============================================================ */
static void updateLEDs(void)
{
    for (uint8_t i = 0; i < NUM_FLOORS; i++)
        digitalWrite(PIN_FLOOR_LED[i], (i == (uint8_t)gFloor) ? HIGH : LOW);

    digitalWrite(PIN_LED_UP,   (gDir ==  1) ? HIGH : LOW);
    digitalWrite(PIN_LED_DOWN, (gDir == -1) ? HIGH : LOW);
}

/* LCD helpers – write fixed-width fields to avoid lcd.clear() flicker */
static void lcdWriteAt(uint8_t col, uint8_t row,
                        const char *str, uint8_t width)
{
    lcd.setCursor(col, row);
    uint8_t len = 0;
    while (str[len] && len < width) { lcd.write(str[len]); len++; }
    while (len < width) { lcd.write(' '); len++; }
}

/* Integer to fixed-width decimal string, right-aligned, no String class */
static void itoa_fixed(int32_t val, char *buf, uint8_t width)
{
    /* write from right */
    buf[width] = '\0';
    bool neg = (val < 0);
    if (neg) val = -val;
    for (int8_t i = (int8_t)(width - 1); i >= 0; i--) {
        buf[i] = (char)('0' + (val % 10));
        val /= 10;
        if (val == 0 && i > 0) {
            for (int8_t j = i - 1; j >= 0; j--)
                buf[j] = (neg && j == 0) ? '-' : ' ';
            break;
        }
    }
}

static void updateLCD(void)
{
    uint32_t now = millis();
    if ((now - gLcdTimer) < LCD_UPDATE_MS) return;
    gLcdTimer = now;

    char buf[17];

    /* ── Line 0:  F:2 UP  WT:350kg  (16 chars) ── */
    /* "F:" */
    buf[0] = 'F'; buf[1] = ':';
    buf[2] = (char)('0' + gFloor);
    buf[3] = ' ';
    /* direction */
    if (gState == STATE_MOVING_UP || gDir == 1) {
        buf[4]='U'; buf[5]='P'; buf[6]=' '; buf[7]=' ';
    } else {
        buf[4]='D'; buf[5]='N'; buf[6]=' '; buf[7]=' ';
    }
    /* "WT:" */
    buf[8]='W'; buf[9]='T'; buf[10]=':';
    /* weight up to 3 digits */
    char wbuf[4];
    itoa_fixed((int32_t)gWeightKg, wbuf, 3);
    buf[11]=wbuf[0]; buf[12]=wbuf[1]; buf[13]=wbuf[2];
    buf[14]='k'; buf[15]='g';
    buf[16]='\0';
    lcdWriteAt(0, 0, buf, 16);

    /* ── Line 1:  ST:MOVING  RQ:0,3  (16 chars) ── */
    buf[0]='S'; buf[1]='T'; buf[2]=':';
    const char *stName;
    switch (gState) {
        case STATE_IDLE:        stName = "IDLE   "; break;
        case STATE_MOVING_UP:   stName = "MOV_UP "; break;
        case STATE_MOVING_DOWN: stName = "MOV_DN "; break;
        case STATE_DOOR_OPEN:   stName = "DOOR   "; break;
        case STATE_OVERLOAD:    stName = "OVERLD "; break;
        default:                stName = "???????"; break;
    }
    uint8_t pos = 3;
    for (uint8_t i = 0; stName[i] && pos < 10; i++, pos++)
        buf[pos] = stName[i];
    /* "RQ:" */
    buf[pos++]='R'; buf[pos++]='Q'; buf[pos++]=':';
    /* list requested floors */
    bool any = false;
    for (uint8_t f = 0; f < NUM_FLOORS && pos < 15; f++) {
        if (hasRequest(f)) {
            if (any && pos < 15) buf[pos++] = ',';
            buf[pos++] = (char)('0' + f);
            any = true;
        }
    }
    if (!any && pos < 16) buf[pos++] = '-';
    while (pos < 16) buf[pos++] = ' ';
    buf[16] = '\0';
    lcdWriteAt(0, 1, buf, 16);
}

/* ============================================================
 *  SETUP
 * ============================================================ */
void setup(void)
{
    /* LCD */
    lcd.init();
    lcd.backlight();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Elevator Ready");

    /* Buttons */
    initButtons();

    /* Door sensor */
    pinMode(PIN_DOOR_SENSOR, INPUT_PULLUP);

    /* Weight – analog, no pinMode needed for Wokwi pot on PA_9 */
    analogReadResolution(12); /* 12-bit on STM32 */

    /* Floor LEDs */
    for (uint8_t i = 0; i < NUM_FLOORS; i++) {
        pinMode(PIN_FLOOR_LED[i], OUTPUT);
        digitalWrite(PIN_FLOOR_LED[i], LOW);
    }
    /* Direction LEDs */
    pinMode(PIN_LED_UP,   OUTPUT);
    pinMode(PIN_LED_DOWN, OUTPUT);
    digitalWrite(PIN_LED_UP,   LOW);
    digitalWrite(PIN_LED_DOWN, LOW);

    /* Buzzer */
    pinMode(PIN_BUZZER, OUTPUT);
    digitalWrite(PIN_BUZZER, LOW);

    /* Start at floor 0 */
    gFloor    = 0;
    gDir      = 1;
    gState    = STATE_IDLE;
    gRequests = 0;

    updateLEDs();
}

/* ============================================================
 *  MAIN LOOP
 * ============================================================ */
void loop(void)
{
    uint32_t now = millis();

    /* ── 1. Read sensors ── */
    readWeight();

    /* ── 2. Read & dispatch buttons (only outside DOOR_OPEN/OVERLOAD
     *       we still accept new requests; we just ignore movement     ) */
    uint16_t fired = pollButtons();
    if (fired) dispatchButtonFired(fired);

    /* ── 3. Overload check – overrides all states ── */
    bool overloaded = (gWeightKg > OVERLOAD_KG);

    if (overloaded) {
        if (gState != STATE_OVERLOAD) {
            gPrevState = gState;  /* remember where we were */
            gState     = STATE_OVERLOAD;
        }
        digitalWrite(PIN_BUZZER, HIGH);
        updateLEDs();
        updateLCD();
        return; /* block all further processing this loop */
    }

    /* Leaving overload */
    if (gState == STATE_OVERLOAD) {
        digitalWrite(PIN_BUZZER, LOW);
        /* Resume from where we were; if we were mid-travel restart
         * the floor timer so we don't teleport                      */
        gState        = gPrevState;
        gFloorTimer   = now;  /* reset travel timer on resume */
        gDoorTimer    = now;
    }

    /* ── 4. FSM ── */
    switch (gState) {

        /* ────────────────────────────────────────── IDLE ── */
        case STATE_IDLE: {
            /* If current floor has a request, service it immediately */
            if (hasRequest(gFloor)) {
                clearRequest(gFloor);
                gDoorTimer = now;
                gState     = STATE_DOOR_OPEN;
                break;
            }
            if (!anyRequest()) break; /* nothing to do */

            /* Decide direction via SCAN */
            int8_t next = scanNextFloor();
            if (next == -1) break; /* defensive, covered above */

            if (next > gFloor) {
                gDir   = 1;
                gState = STATE_MOVING_UP;
            } else {
                gDir   = -1;
                gState = STATE_MOVING_DOWN;
            }
            gFloorTimer = now;
            break;
        }

        /* ───────────────────────────────────── MOVING_UP ── */
        case STATE_MOVING_UP: {
            if ((now - gFloorTimer) < FLOOR_TRAVEL_MS) break;
            gFloorTimer = now;

            gFloor++;
            if (gFloor >= NUM_FLOORS) gFloor = NUM_FLOORS - 1; /* clamp */
            updateLEDs();

            if (hasRequest(gFloor)) {
                clearRequest(gFloor);
                gDoorTimer = now;
                gState     = STATE_DOOR_OPEN;
                break;
            }

            /* Check if we should keep going up */
            bool moreUp = false;
            for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++) {
                if (hasRequest(f)) { moreUp = true; break; }
            }
            if (!moreUp) {
                /* Reverse or idle */
                bool moreDown = false;
                for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--) {
                    if (hasRequest(f)) { moreDown = true; break; }
                }
                if (moreDown) {
                    gDir   = -1;
                    gState = STATE_MOVING_DOWN;
                } else {
                    gState = STATE_IDLE;
                }
            }
            break;
        }

        /* ─────────────────────────────────── MOVING_DOWN ── */
        case STATE_MOVING_DOWN: {
            if ((now - gFloorTimer) < FLOOR_TRAVEL_MS) break;
            gFloorTimer = now;

            gFloor--;
            if (gFloor < 0) gFloor = 0; /* clamp */
            updateLEDs();

            if (hasRequest(gFloor)) {
                clearRequest(gFloor);
                gDoorTimer = now;
                gState     = STATE_DOOR_OPEN;
                break;
            }

            bool moreDown = false;
            for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--) {
                if (hasRequest(f)) { moreDown = true; break; }
            }
            if (!moreDown) {
                bool moreUp = false;
                for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++) {
                    if (hasRequest(f)) { moreUp = true; break; }
                }
                if (moreUp) {
                    gDir   = 1;
                    gState = STATE_MOVING_UP;
                } else {
                    gState = STATE_IDLE;
                }
            }
            break;
        }

        /* ─────────────────────────────────── DOOR_OPEN ── */
        case STATE_DOOR_OPEN: {
            /* New requests are queued but movement is ignored here.
             * The timer is NOT reset if this floor is pressed again
             * (the bit was already cleared on entry, so setRequest
             *  would re-arm it – handled correctly: we just wait out
             *  the timer then re-evaluate in IDLE/MOVING).           */
            if ((now - gDoorTimer) < DOOR_OPEN_MS) break;

            /* Door closes. Decide what to do next. */
            /* If current floor got re-requested while door was open,
               service it again next cycle via IDLE                   */
            if (!anyRequest()) {
                gState = STATE_IDLE;
                break;
            }

            /* SCAN: continue in same direction if possible, else reverse */
            bool ahead = false;
            if (gDir == 1) {
                for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++)
                    if (hasRequest(f)) { ahead = true; break; }
                if (ahead) { gState = STATE_MOVING_UP;   gFloorTimer = now; break; }
                for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--)
                    if (hasRequest(f)) { ahead = true; break; }
                if (ahead) { gDir = -1; gState = STATE_MOVING_DOWN; gFloorTimer = now; break; }
            } else {
                for (int8_t f = (int8_t)(gFloor - 1); f >= 0; f--)
                    if (hasRequest(f)) { ahead = true; break; }
                if (ahead) { gState = STATE_MOVING_DOWN; gFloorTimer = now; break; }
                for (int8_t f = (int8_t)(gFloor + 1); f < NUM_FLOORS; f++)
                    if (hasRequest(f)) { ahead = true; break; }
                if (ahead) { gDir = 1; gState = STATE_MOVING_UP; gFloorTimer = now; break; }
            }

            gState = STATE_IDLE;
            break;
        }

        case STATE_OVERLOAD:
        default:
            break;
    }

    /* ── 5. Outputs ── */
    updateLEDs();
    updateLCD();
}