.syntax unified
.cpu cortex-m3
.thumb

@ --- Hardware Addresses (Blue Pill F103) ---
.equ GPIOB_BSRR,     0x40010C10

.data
.align 2
@ sys_state layout: [8-15: Current Floor] [0-7: Request Bits]
@ Req Bits: bit0=F1, bit1=F2_Up, bit2=F2_Down, bit3=F2_Int, bit4=F3
sys_state: .word 0x00000100 

.text
.align 2

@ --- Export functions to C++ ---
.global asm_p0, asm_p1, asm_p2, asm_p3, asm_p4, asm_p5, asm_p6, asm_p7

@ Buttons (PA0 - PA4)
asm_p0: push {lr}; movs r3, #0x01; bl process_req; pop {pc} @ Floor 1
asm_p1: push {lr}; movs r3, #0x02; bl process_req; pop {pc} @ Floor 2 Up
asm_p2: push {lr}; movs r3, #0x04; bl process_req; pop {pc} @ Floor 2 Down
asm_p3: push {lr}; movs r3, #0x08; bl process_req; pop {pc} @ Floor 2 Int
asm_p4: push {lr}; movs r3, #0x10; bl process_req; pop {pc} @ Floor 3

@ Sensors (PA5 - PA7)
asm_p5: push {lr}; movs r2, #1; bl update_floor; pop {pc} @ Sensor F1
asm_p6: push {lr}; movs r2, #2; bl update_floor; pop {pc} @ Sensor F2
asm_p7: push {lr}; movs r2, #3; bl update_floor; pop {pc} @ Sensor F3

@ --- Logic Core ---
process_req:
    push {lr}
    ldr r0, =sys_state
    ldr r4, [r0]
    orrs r4, r3                 @ Add request bit
    bl elevator_brain
    str r4, [r0]
    pop {pc}

update_floor:
    push {lr}
    ldr r0, =sys_state
    ldr r4, [r0]
    @ Update Floor bits (8-15)
    ldr r1, =0xFFFF00FF
    ands r4, r1
    lsls r2, r2, #8
    orrs r4, r2
    
    @ Check if we need to stop
    lsrs r1, r4, #8
    ands r1, #0xFF              @ R1 = Current Floor
    
    @ Stop if current floor matches a request
    cmp r1, #1
    beq stop_f1
    cmp r1, #2
    beq stop_f2
    cmp r1, #3
    beq stop_f3
    b brain_re-eval

stop_f1: tst r4, #0x01; beq brain_re-eval; bic r4, #0x01; bl motor_stop; b brain_re-eval
stop_f2: tst r4, #0x0E; beq brain_re-eval; bic r4, #0x0E; bl motor_stop; b brain_re-eval
stop_f3: tst r4, #0x10; beq brain_re-eval; bic r4, #0x10; bl motor_stop; b brain_re-eval

brain_re-eval:
    bl elevator_brain
    str r4, [r0]
    pop {pc}

elevator_brain:
    push {lr}
    lsrs r1, r4, #8
    ands r1, #0xFF              @ Current Floor
    movs r2, #0x1F
    ands r2, r4                 @ Any Reqs?
    beq motor_stop
    
    @ F1 Logic
    cmp r1, #1
    bne 1f
    tst r4, #0x1E               @ Higher reqs?
    beq motor_stop
    movs r0, #1; movs r1, #0; bl update_motors @ Move UP
    b brain_done
1:  @ F3 Logic
    cmp r1, #3
    bne 2f
    tst r4, #0x0F               @ Lower reqs?
    beq motor_stop
    movs r0, #0; movs r1, #1; bl update_motors @ Move DOWN
    b brain_done
2:  @ F2 Logic
    tst r4, #0x01               @ Req F1?
    beq 3f
    movs r0, #0; movs r1, #1; bl update_motors @ DOWN
    b brain_done
3:  tst r4, #0x10               @ Req F3?
    beq motor_stop
    movs r0, #1; movs r1, #0; bl update_motors @ UP

brain_done:
    pop {pc}

motor_stop:
    push {lr}
    movs r0, #0; movs r1, #0; bl update_motors
    pop {pc}

update_motors:
    ldr r2, =GPIOB_BSRR
    @ PB10 (UP)
    cmp r0, #1
    beq u_on
    ldr r3, =(1 << 26); b 1f
u_on: ldr r3, =(1 << 10)
1:  str r3, [r2]
    @ PB11 (DOWN)
    cmp r1, #1
    beq d_on
    ldr r3, =(1 << 27); b 2f
d_on: ldr r3, =(1 << 11)
2:  str r3, [r2]
    bx lr