#define RCC_BASE       0x40021000
#define GPIOA_BASE     0x50000000

// Offsets for relevant registers
#define RCC_IOPENR     (*(volatile uint32_t *)(RCC_BASE + 0x34))
#define GPIOA_MODER    (*(volatile uint32_t *)(GPIOA_BASE + 0x00))
#define GPIOA_OTYPER   (*(volatile uint32_t *)(GPIOA_BASE + 0x04))
#define GPIOA_OSPEEDR  (*(volatile uint32_t *)(GPIOA_BASE + 0x08))
#define GPIOA_PUPDR    (*(volatile uint32_t *)(GPIOA_BASE + 0x0C))
#define GPIOA_ODR      (*(volatile uint32_t *)(GPIOA_BASE + 0x14))
#define GPIOA_IDR      (*(volatile uint32_t *)(GPIOA_BASE + 0x10))

#define PIN0           0 // START
#define PIN1           1 // RUN
#define PIN2           2 //STOP
#define PIN4           4 //EMERGENCY STOP

#define BPIN5           5 //START
#define BPIN6           6 //RUN
#define BPIN7           7 //STOP
#define BPIN8           8 //EMERGENCY STOP

void initGPIO();
void startMachine();
void stopMachine();
void runMachine();
void emergencyStop();
void delay1(volatile long i){
  while(i--);
}

int main(void) {
    uint8_t mode = 0;
    RCC_IOPENR |= (1 << 0); // Enable GPIOA clock
    GPIOA_MODER &= ~((3 << (PIN0 * 2))|(3 << (PIN1 * 2))|(3 << (PIN2 * 2))|(3 << (PIN4 * 2))); // Clear mode bits for PA5
    GPIOA_MODER |= (1 << (PIN0 * 2))|(1 << (PIN1 * 2))|(1 << (PIN2 * 2))|(1 << (PIN4 * 2));
    GPIOA_MODER &= ~((3 << (BPIN5 * 2))|(3 << (BPIN6 * 2))|(3 << (BPIN7 * 2))|(3 << (BPIN8 * 2)));

    GPIOA_PUPDR &= ~((3 << (BPIN5 * 2))|(3 << (BPIN6 * 2))|(3 << (BPIN7 * 2))|(3 << (BPIN8 * 2)));
    GPIOA_PUPDR |= (1 << (BPIN5 * 2))|(1 << (BPIN6 * 2))|(1 << (BPIN7 * 2))|(1 << (BPIN8 * 2));
    while (1) {
      if(!(GPIOA_IDR & (1<<BPIN5))) {
            delay1(50); // Debounce delay
            if(!(GPIOA_IDR & (1<<BPIN5))) {
                mode = 1; // Start machine
            }
      } else if (!(GPIOA_IDR & (1<<BPIN6))) {
            delay1(50);
            if (!(GPIOA_IDR & (1<<BPIN6))) {
                mode = 2; // run machine
            }
      } else if (!(GPIOA_IDR & (1<<BPIN7))) {
            delay1(50);
            if (!(GPIOA_IDR & (1<<BPIN7))) {
                mode = 3; // stop mode
            }
      } else if (!(GPIOA_IDR & (1<<BPIN8))) {
            delay1(50);
            if (!(GPIOA_IDR & (1<<BPIN8))) {
                mode = 4; // Emergency stop
            }
      }

        // Switch-case to handle machine modes
      switch (mode) {
            case 1:
                startMachine();      // Start machine
                runMachineStop();
                stopMachine1();
                emergencyStop1();
                mode = 0;            // Reset mode
                break;
            case 2:
                stopMachine();       // Stop machine
                startMachineStop();
                runMachineStop();
                emergencyStop1();
                mode = 0;            // Reset mode
                break;
            case 3:
                runMachine();       // run mode
                stopMachine1();
                emergencyStop1();
                startMachineStop();
                mode = 0;            // Reset mode
                break;
            case 4:
                emergencyStop();     // Emergency stop
                stopMachine1();
                startMachineStop();
                runMachineStop();
                mode = 0;            // Reset mode
                break;
            default:
                // Do nothing if no valid mode
                break;
        }
         
    }

    return 0;
}
// Machine modes
void startMachine() {
    GPIOA_ODR |=  (1<<PIN0);
}
void runMachine() {
    GPIOA_ODR |=  (1<<PIN1);
}
void stopMachine() {
    GPIOA_ODR |=  (1<<PIN2);  
}
void emergencyStop() {
    GPIOA_ODR |=  (1<<PIN4); 
}

void startMachineStop() {
    GPIOA_ODR &=  ~(1<<PIN0); 
}
void runMachineStop() {
    GPIOA_ODR &=  ~(1<<PIN1);
}
void stopMachine1() {
    GPIOA_ODR &=  ~(1<<PIN2);
}
void emergencyStop1() {
    GPIOA_ODR &=  ~(1<<PIN4);
}