FLASH_Status FLASHstatus = FLASH_COMPLETE;

  if((FLASH->SR & FLASH_FLAG_BSY) == FLASH_FLAG_BSY)

  {

    FLASHstatus = FLASH_BUSY;

  }

  else

  {

    if((FLASH->SR & (uint32_t)FLASH_FLAG_WRPERR)!= (uint32_t)0x00)

    {

      FLASHstatus = FLASH_ERROR_WRP;

    }

    else

    {

      if((FLASH->SR & (uint32_t)0x1E00) != (uint32_t)0x00)

      {

        FLASHstatus = FLASH_ERROR_PROGRAM;

      }

      else

      {

        FLASHstatus = FLASH_COMPLETE;

      }

    }

  }

  /* Return the FLASH Status */

  return FLASHstatus;

}

FLASH_Status Minimal_FLASH_WaitForLastOperation(uint32_t Timeout)

{

  __IO FLASH_Status status = FLASH_COMPLETE;

  /* Check for the FLASH Status */

  status = Minimal_FLASH_GetStatus();

  /* Wait for a FLASH operation to complete or a TIMEOUT to occur */

  while((status == FLASH_BUSY) && (Timeout != 0x00))

  {

    status = Minimal_FLASH_GetStatus();

    Timeout--;

  }

  if(Timeout == 0x00 )

  {

    status = FLASH_TIMEOUT;

  }

  /* Return the operation status */

  return status;

}

void Minimal_EEPROM_ProgramWord(uint32_t Address, uint32_t Data)

{

  // Wait for last operation to be completed 

  FLASH_Status status = FLASH_COMPLETE;

  status = Minimal_FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT);

  if(status == FLASH_COMPLETE)

  {

    *(__IO uint32_t *)Address = Data;

    // Wait for last operation to be completed 

    status = Minimal_FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT);

  }

  // Return the Write Status 

  return status;

}

void save_settings()

{

   Minimal_EEPROM_Unlock();

    // Try programming a word at an address divisible by 4

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BOOTTOBREW, boottobrew);

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_WINDUP_GUARD, windup_guard);

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_P, k_p);

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_I, k_i);

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_D, k_d);

    Minimal_EEPROM_Lock();

}

void save_setpoints()

{

    Minimal_EEPROM_Unlock();

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BREWTEMP, setpoint_brew);

    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_STEAMTEMP, setpoint_steam); 

    Minimal_EEPROM_Lock();

}

// TODO: Save/restore temperature setpoint settings

void restore_settings()

{

    Minimal_EEPROM_Unlock();

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);

    Minimal_EEPROM_Lock();

}

void user_input(uint16_t* to_modify)

{

    if(CHANGE_ELAPSED) {

        // TODO: Make function that takes reference to a var and increase/decreases it based on buttonpress

        if(!GPIO_ReadInputDataBit(SW_UP) ) {

            CHANGE_RESET;

            (*to_modify)++;

        }

        else if(!GPIO_ReadInputDataBit(SW_DOWN) && (*to_modify) > 0) {

            CHANGE_RESET;

            (*to_modify)--;

        }

    }

}

void machine()

{

    uint8_t last_state = state;

    uint8_t sw_btn = !GPIO_ReadInputDataBit(SW_BTN);

    uint8_t sw_up = !GPIO_ReadInputDataBit(SW_UP);

    uint8_t sw_down = !GPIO_ReadInputDataBit(SW_DOWN);

    uint8_t sw_left = !GPIO_ReadInputDataBit(SW_LEFT);

    uint8_t sw_right = !GPIO_ReadInputDataBit(SW_RIGHT);

    switch(state)

    {

        // Idle state

        case STATE_IDLE:

        {

            // Write text to OLED

            // [ therm :: idle ]

            ssd1306_DrawString("therm :: idle ", 0, 40);

            pid_enabled = 0;

            char tempstr[6];

            itoa_fp(temp, temp_frac, tempstr);

            ssd1306_DrawString("Temp: ", 3, 40);

            ssd1306_DrawString("    ", 3, 72);

            ssd1306_DrawString(tempstr, 3, 72);

            ssd1306_drawlogo();

            switch(goto_mode) {

                case 2:

                {

                    ssd1306_DrawString("-> brew     ", 1, 40);

                } break;

                case 1:

                {

                    ssd1306_DrawString("-> setup    ", 1, 40);

                } break;

                case 0:

                {

                    ssd1306_DrawString("-> reset    ", 1, 40);

                } break;

            }

            // Button handler

            if(SW_BTN_PRESSED) {

                switch(goto_mode) {

                    case 2:

                        state = STATE_PREHEAT_BREW;

                        break;

                    case 1:

                        state = STATE_SETP;

                        break;

                    case 0:

                        state = STATE_IDLE;

                        break;

                    default:

                        state = STATE_PREHEAT_BREW;

                }

            }

            else if(SW_UP_PRESSED && goto_mode < 2) {

                goto_mode++;

            }

            else if(SW_DOWN_PRESSED && k_p > 0 && goto_mode > 0) {

                goto_mode--;

            }

            // Event Handler

            // N/A

        } break;