#include <Arduino.h>

/*
==========================================================
 SMART CONVEYOR BELT
 STM32F103C8T6 BLUE PILL - WOKWI

 ADC      -> PA0
 TIMER    -> TIM4_CH1 / PB6
 SPI      -> SPI1 / PA5 PA6 PA7
 RFID CS  -> PB1
 RFID RST -> PB10
 E-STOP   -> PA8 / EXTI
 OBJECT   -> PB0
 FAULT LED -> PB14
 BUZZER   -> PB13
 UART     -> PA9 / PA10

==========================================================
*/


// ========================================================
// PIN DEFINITIONS
// ========================================================

#define CURRENT_SENSOR   PA0
#define OBJECT_SENSOR    PB0
#define EMERGENCY_STOP   PA8

#define MOTOR_PWM        PB6
#define FAULT_LED        PB14
#define BUZZER           PB13

// RFID
#define RFID_CS          PB1
#define RFID_RST         PB10


// ========================================================
// CURRENT LIMITS
// ========================================================

#define ADC_MAX_VALUE    4095.0f
#define MAX_CURRENT      12.0f

#define WARNING_CURRENT  8.0f
#define TRIP_CURRENT     10.0f


// ========================================================
// GLOBAL VARIABLES
// ========================================================

volatile bool emergencyTriggered = false;

bool overloadTriggered = false;

bool previousObjectState = HIGH;

unsigned long objectCount = 0;

unsigned long lastControlTime = 0;
unsigned long lastReportTime = 0;
unsigned long lastSPITime = 0;


// ========================================================
// EMERGENCY INTERRUPT
// ========================================================

void emergencyStopISR()
{
    emergencyTriggered = true;

    // Immediately stop motor
    TIM4->CCR1 = 0;

    // Red LED ON
    GPIOB->BSRR = GPIO_BSRR_BS14;

    // Buzzer ON
    GPIOB->BSRR = GPIO_BSRR_BS13;
}


// ========================================================
// GPIO INITIALIZATION
// ========================================================

void GPIO_Init_RegisterLevel()
{
    // Enable GPIOA, GPIOB and AFIO
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
    RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;


    // ----------------------------------------------------
    // PA0 = ADC INPUT
    // ----------------------------------------------------

    GPIOA->CRL &= ~(GPIO_CRL_MODE0 | GPIO_CRL_CNF0);


    // ----------------------------------------------------
    // PA5 = SPI SCK
    // Alternate Function Push-Pull
    // ----------------------------------------------------

    GPIOA->CRL &= ~(GPIO_CRL_MODE5 | GPIO_CRL_CNF5);

    GPIOA->CRL |= GPIO_CRL_MODE5_0 |
                  GPIO_CRL_MODE5_1 |
                  GPIO_CRL_CNF5_1;


    // ----------------------------------------------------
    // PA6 = SPI MISO
    // Input floating
    // ----------------------------------------------------

    GPIOA->CRL &= ~(GPIO_CRL_MODE6 | GPIO_CRL_CNF6);

    GPIOA->CRL |= GPIO_CRL_CNF6_0;


    // ----------------------------------------------------
    // PA7 = SPI MOSI
    // Alternate Function Push-Pull
    // ----------------------------------------------------

    GPIOA->CRL &= ~(GPIO_CRL_MODE7 | GPIO_CRL_CNF7);

    GPIOA->CRL |= GPIO_CRL_MODE7_0 |
                  GPIO_CRL_MODE7_1 |
                  GPIO_CRL_CNF7_1;


    // ----------------------------------------------------
    // PA8 = Emergency Stop
    // Input Pull-Up
    // ----------------------------------------------------

    GPIOA->CRH &= ~(GPIO_CRH_MODE8 | GPIO_CRH_CNF8);

    GPIOA->CRH |= GPIO_CRH_CNF8_1;

    GPIOA->ODR |= GPIO_ODR_ODR8;


    // ----------------------------------------------------
    // PA9 = USART TX
    // ----------------------------------------------------

    GPIOA->CRH &= ~(GPIO_CRH_MODE9 | GPIO_CRH_CNF9);

    GPIOA->CRH |= GPIO_CRH_MODE9_0 |
                  GPIO_CRH_MODE9_1 |
                  GPIO_CRH_CNF9_1;


    // ----------------------------------------------------
    // PA10 = USART RX
    // ----------------------------------------------------

    GPIOA->CRH &= ~(GPIO_CRH_MODE10 | GPIO_CRH_CNF10);

    GPIOA->CRH |= GPIO_CRH_CNF10_0;


    // ----------------------------------------------------
    // PB0 = OBJECT SENSOR
    // Input Pull-Up
    // ----------------------------------------------------

    GPIOB->CRL &= ~(GPIO_CRL_MODE0 | GPIO_CRL_CNF0);

    GPIOB->CRL |= GPIO_CRL_CNF0_1;

    GPIOB->ODR |= GPIO_ODR_ODR0;


    // ----------------------------------------------------
    // PB1 = RFID CS
    // Output Push-Pull
    // ----------------------------------------------------

    GPIOB->CRL &= ~(GPIO_CRL_MODE1 | GPIO_CRL_CNF1);

    GPIOB->CRL |= GPIO_CRL_MODE1_0 |
                  GPIO_CRL_MODE1_1;


    // CS HIGH = inactive
    GPIOB->BSRR = GPIO_BSRR_BS1;


    // ----------------------------------------------------
    // PB6 = TIM4_CH1 PWM
    // Alternate Function Push-Pull
    // ----------------------------------------------------

    GPIOB->CRL &= ~(GPIO_CRL_MODE6 | GPIO_CRL_CNF6);

    GPIOB->CRL |= GPIO_CRL_MODE6_0 |
                  GPIO_CRL_MODE6_1 |
                  GPIO_CRL_CNF6_1;


    // ----------------------------------------------------
    // PB10 = RFID RESET
    // Output Push-Pull
    // ----------------------------------------------------

    GPIOB->CRH &= ~(GPIO_CRH_MODE10 | GPIO_CRH_CNF10);

    GPIOB->CRH |= GPIO_CRH_MODE10_0 |
                  GPIO_CRH_MODE10_1;


    // RFID reset HIGH
    GPIOB->BSRR = GPIO_BSRR_BS10;


    // ----------------------------------------------------
    // PB13 = BUZZER
    // Output Push-Pull
    // ----------------------------------------------------

    GPIOB->CRH &= ~(GPIO_CRH_MODE13 | GPIO_CRH_CNF13);

    GPIOB->CRH |= GPIO_CRH_MODE13_0 |
                  GPIO_CRH_MODE13_1;


    // ----------------------------------------------------
    // PB14 = FAULT LED
    // Output Push-Pull
    // ----------------------------------------------------

    GPIOB->CRH &= ~(GPIO_CRH_MODE14 | GPIO_CRH_CNF14);

    GPIOB->CRH |= GPIO_CRH_MODE14_0 |
                  GPIO_CRH_MODE14_1;


    // Outputs OFF
    GPIOB->BRR =
        GPIO_BRR_BR13 |
        GPIO_BRR_BR14;
}


// ========================================================
// ADC1 INITIALIZATION
// ========================================================

void ADC1_Init_RegisterLevel()
{
    // Enable ADC1
    RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;


    // ADC clock = 72 MHz / 6 = 12 MHz
    RCC->CFGR &= ~RCC_CFGR_ADCPRE;

    RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6;


    // One conversion
    ADC1->SQR1 = 0;

    ADC1->SQR2 = 0;

    ADC1->SQR3 = 0;


    // Channel 0 sample time
    ADC1->SMPR2 |= ADC_SMPR2_SMP0;


    // Enable ADC
    ADC1->CR2 |= ADC_CR2_ADON;


    // Reset calibration
    ADC1->CR2 |= ADC_CR2_RSTCAL;

    while (ADC1->CR2 & ADC_CR2_RSTCAL)
    {
    }


    // Calibration
    ADC1->CR2 |= ADC_CR2_CAL;

    while (ADC1->CR2 & ADC_CR2_CAL)
    {
    }
}


// ========================================================
// ADC READ
// ========================================================

uint16_t ADC1_Read()
{
    ADC1->CR2 |= ADC_CR2_ADON;

    ADC1->CR2 |= ADC_CR2_SWSTART;


    while (!(ADC1->SR & ADC_SR_EOC))
    {
    }


    return ADC1->DR;
}


// ========================================================
// CURRENT CALCULATION
// ========================================================

float readMotorCurrent()
{
    uint16_t raw = ADC1_Read();

    float current =
        ((float)raw / ADC_MAX_VALUE)
        * MAX_CURRENT;

    return current;
}


// ========================================================
// TIM4 PWM INITIALIZATION
// PB6 = TIM4_CH1
// ========================================================

void TIM4_Init_RegisterLevel()
{
    // Enable TIM4
    RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;


    // Timer frequency:
    // 72 MHz / 72 = 1 MHz

    TIM4->PSC = 71;


    // 1 MHz / 1000 = 1 kHz PWM

    TIM4->ARR = 999;


    // Start at 0%
    TIM4->CCR1 = 0;


    // PWM mode 1
    TIM4->CCMR1 &= ~TIM_CCMR1_OC1M;

    TIM4->CCMR1 |=
        TIM_CCMR1_OC1M_1 |
        TIM_CCMR1_OC1M_2;


    // Preload
    TIM4->CCMR1 |= TIM_CCMR1_OC1PE;


    // Enable channel
    TIM4->CCER |= TIM_CCER_CC1E;


    // Auto reload preload
    TIM4->CR1 |= TIM_CR1_ARPE;


    // Start timer
    TIM4->CR1 |= TIM_CR1_CEN;
}


// ========================================================
// MOTOR SPEED
// ========================================================

void setMotorSpeed(int percentage)
{
    if (percentage < 0)
        percentage = 0;

    if (percentage > 100)
        percentage = 100;


    TIM4->CCR1 =
        (percentage * 1000) / 100;
}


// ========================================================
// STOP MOTOR
// ========================================================

void stopMotor()
{
    TIM4->CCR1 = 0;
}


// ========================================================
// USART1 INITIALIZATION
// ========================================================

void USART1_Init_RegisterLevel()
{
    // Enable USART1
    RCC->APB2ENR |= RCC_APB2ENR_USART1EN;


    // 115200 baud at 72 MHz
    USART1->BRR = 0x0271;


    // Enable transmitter,
    // receiver and USART

    USART1->CR1 |=
        USART_CR1_TE |
        USART_CR1_RE |
        USART_CR1_UE;
}


// ========================================================
// UART CHARACTER
// ========================================================

void UART_SendChar(char c)
{
    while (!(USART1->SR & USART_SR_TXE))
    {
    }

    USART1->DR = c;
}


// ========================================================
// UART STRING
// ========================================================

void UART_SendString(const char *text)
{
    while (*text)
    {
        UART_SendChar(*text);

        text++;
    }
}


// ========================================================
// UART NUMBER
// ========================================================

void UART_SendNumber(unsigned long number)
{
    char buffer[12];

    int i = 0;


    if (number == 0)
    {
        UART_SendChar('0');

        return;
    }


    while (number > 0)
    {
        buffer[i++] =
            '0' + (number % 10);

        number /= 10;
    }


    while (i > 0)
    {
        UART_SendChar(buffer[--i]);
    }
}


// ========================================================
// UART CURRENT
// ========================================================

void UART_SendCurrent(float current)
{
    int whole =
        (int)current;


    int decimal =
        (int)((current - whole) * 100);


    UART_SendNumber(whole);

    UART_SendChar('.');


    if (decimal < 10)
        UART_SendChar('0');


    UART_SendNumber(decimal);

    UART_SendChar('A');
}


// ========================================================
// SPI1 INITIALIZATION
// ========================================================

void SPI1_Init_RegisterLevel()
{
    // Enable SPI1
    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;


    // Clear configuration
    SPI1->CR1 = 0;


    // Master mode
    SPI1->CR1 |= SPI_CR1_MSTR;


    // Software NSS
    SPI1->CR1 |= SPI_CR1_SSM;

    SPI1->CR1 |= SPI_CR1_SSI;


    // SPI clock = 72MHz / 16
    SPI1->CR1 |= SPI_CR1_BR_1;


    // Mode 0
    // CPOL = 0
    // CPHA = 0


    // Enable SPI
    SPI1->CR1 |= SPI_CR1_SPE;
}


// ========================================================
// SPI TRANSFER
// ========================================================

uint8_t SPI1_Transfer(uint8_t data)
{
    while (!(SPI1->SR & SPI_SR_TXE))
    {
    }


    SPI1->DR = data;


    while (!(SPI1->SR & SPI_SR_RXNE))
    {
    }


    return SPI1->DR;
}


// ========================================================
// RFID SELECT
// ========================================================

void RFID_Select()
{
    GPIOB->BRR =
        GPIO_BRR_BR1;
}


// ========================================================
// RFID DESELECT
// ========================================================

void RFID_Deselect()
{
    GPIOB->BSRR =
        GPIO_BSRR_BS1;
}


// ========================================================
// RFID WRITE REGISTER
// ========================================================

void RFID_WriteRegister(
    uint8_t address,
    uint8_t value
)
{
    RFID_Select();


    SPI1_Transfer(
        (address << 1) & 0x7E
    );


    SPI1_Transfer(value);


    RFID_Deselect();
}


// ========================================================
// RFID READ REGISTER
// ========================================================

uint8_t RFID_ReadRegister(
    uint8_t address
)
{
    uint8_t value;


    RFID_Select();


    SPI1_Transfer(
        ((address << 1) & 0x7E) | 0x80
    );


    value =
        SPI1_Transfer(0x00);


    RFID_Deselect();


    return value;
}


// ========================================================
// RFID INITIALIZATION
// ========================================================

void RFID_Init()
{
    UART_SendString(
        "Initializing MFRC522...\r\n"
    );


    // Hardware reset LOW
    GPIOB->BRR =
        GPIO_BRR_BR10;

    delay(10);


    // Hardware reset HIGH
    GPIOB->BSRR =
        GPIO_BSRR_BS10;

    delay(50);


    // Soft reset
    RFID_WriteRegister(
        0x01,
        0x0F
    );

    delay(50);


    // Read Version Register
    uint8_t version =
        RFID_ReadRegister(0x37);


    UART_SendString(
        "MFRC522 Version = 0x"
    );


    const char hex[] =
        "0123456789ABCDEF";


    UART_SendChar(
        hex[(version >> 4) & 0x0F]
    );


    UART_SendChar(
        hex[version & 0x0F]
    );


    UART_SendString("\r\n");


    if (version != 0x00 &&
        version != 0xFF)
    {
        UART_SendString(
            "SPI DEVICE DETECTED\r\n"
        );
    }
    else
    {
        UART_SendString(
            "SPI DEVICE NOT DETECTED\r\n"
        );
    }
}


// ========================================================
// FAULT ALARM
// ========================================================

void faultAlarm()
{
    GPIOB->BSRR =
        GPIO_BSRR_BS14;

    GPIOB->BSRR =
        GPIO_BSRR_BS13;
}


// ========================================================
// CLEAR FAULT
// ========================================================

void clearFault()
{
    GPIOB->BRR =
        GPIO_BRR_BR14;

    GPIOB->BRR =
        GPIO_BRR_BR13;
}


// ========================================================
// OBJECT DETECTION
// ========================================================

void checkObjectSensor()
{
    bool currentState =
        (GPIOB->IDR &
         GPIO_IDR_IDR0);


    if (previousObjectState == HIGH &&
        currentState == LOW)
    {
        objectCount++;


        UART_SendString(
            "OBJECT DETECTED | Count = "
        );


        UART_SendNumber(
            objectCount
        );


        UART_SendString(
            "\r\n"
        );
    }


    previousObjectState =
        currentState;
}


// ========================================================
// NORMAL OPERATION
// ========================================================

void normalOperation(
    float current
)
{
    clearFault();

    setMotorSpeed(70);


    UART_SendString(
        "NORMAL | Current = "
    );


    UART_SendCurrent(
        current
    );


    UART_SendString(
        " | Speed = 70%\r\n"
    );
}


// ========================================================
// WARNING
// ========================================================

void warningOperation(
    float current
)
{
    // Red LED ON
    GPIOB->BSRR =
        GPIO_BSRR_BS14;


    // Buzzer OFF
    GPIOB->BRR =
        GPIO_BRR_BR13;


    // Reduce conveyor speed
    setMotorSpeed(50);


    UART_SendString(
        "WARNING | Current = "
    );


    UART_SendCurrent(
        current
    );


    UART_SendString(
        " | Speed = 50%\r\n"
    );
}


// ========================================================
// OVERLOAD
// ========================================================

void overloadFault(
    float current
)
{
    stopMotor();

    faultAlarm();

    overloadTriggered = true;


    UART_SendString(
        "\r\n!!! OVERLOAD FAULT !!!\r\n"
    );


    UART_SendString(
        "Current = "
    );


    UART_SendCurrent(
        current
    );


    UART_SendString(
        "\r\nCONVEYOR MOTOR STOPPED\r\n"
    );
}


// ========================================================
// EMERGENCY FAULT
// ========================================================

void emergencyFault()
{
    stopMotor();

    faultAlarm();


    UART_SendString(
        "\r\n!!! EMERGENCY STOP !!!\r\n"
    );


    UART_SendString(
        "CONVEYOR MOTOR STOPPED\r\n"
    );
}


// ========================================================
// SETUP
// ========================================================

void setup()
{
    GPIO_Init_RegisterLevel();

    ADC1_Init_RegisterLevel();

    TIM4_Init_RegisterLevel();

    USART1_Init_RegisterLevel();

    SPI1_Init_RegisterLevel();


    // ----------------------------------------------------
    // Emergency interrupt
    // ----------------------------------------------------

    attachInterrupt(
        digitalPinToInterrupt(
            EMERGENCY_STOP
        ),
        emergencyStopISR,
        FALLING
    );


    // Safe initial state
    stopMotor();

    clearFault();


    // ----------------------------------------------------
    // Startup message
    // ----------------------------------------------------

    UART_SendString(
        "\r\n"
        "================================\r\n"
        " SMART CONVEYOR BELT SYSTEM\r\n"
        " STM32F103C8T6 BLUE PILL\r\n"
        "================================\r\n"
    );


    UART_SendString(
        "ADC   : PA0 / ADC1\r\n"
    );


    UART_SendString(
        "PWM   : PB6 / TIM4_CH1\r\n"
    );


    UART_SendString(
        "SPI   : SPI1\r\n"
    );


    UART_SendString(
        "SCK   : PA5\r\n"
    );


    UART_SendString(
        "MISO  : PA6\r\n"
    );


    UART_SendString(
        "MOSI  : PA7\r\n"
    );


    UART_SendString(
        "CS    : PB1\r\n"
    );


    UART_SendString(
        "RST   : PB10\r\n"
    );


    UART_SendString(
        "E-STOP: PA8\r\n"
    );


    UART_SendString(
        "UART  : 115200 baud\r\n"
    );


    UART_SendString(
        "SYSTEM READY\r\n\r\n"
    );


    // Initialize RFID
    RFID_Init();
}


// ========================================================
// MAIN LOOP
// ========================================================

void loop()
{
    // ----------------------------------------------------
    // Object detection
    // ----------------------------------------------------

    checkObjectSensor();


    // ----------------------------------------------------
    // Emergency stop
    // ----------------------------------------------------

    if (emergencyTriggered)
    {
        emergencyFault();


        // Wait for button release
        while (!(GPIOA->IDR &
                 GPIO_IDR_IDR8))
        {
            delay(20);
        }


        delay(100);


        emergencyTriggered =
            false;


        clearFault();


        UART_SendString(
            "Emergency stop released.\r\n"
        );


        UART_SendString(
            "Restarting conveyor...\r\n"
        );


        delay(500);


        return;
    }


    // ----------------------------------------------------
    // Overload latch
    // ----------------------------------------------------

    if (overloadTriggered)
    {
        stopMotor();

        faultAlarm();

        return;
    }


    // ----------------------------------------------------
    // CONTROL EVERY 100 ms
    // ----------------------------------------------------

    if (millis() -
        lastControlTime >= 100)
    {
        lastControlTime =
            millis();


        float current =
            readMotorCurrent();


        // Overload
        if (current >= TRIP_CURRENT)
        {
            overloadFault(
                current
            );

            return;
        }


        // Warning
        else if (
            current >= WARNING_CURRENT
        )
        {
            warningOperation(
                current
            );
        }


        // Normal
        else
        {
            normalOperation(
                current
            );
        }
    }


    // ----------------------------------------------------
    // SPI TEST EVERY 2 SECONDS
    // ----------------------------------------------------

    if (millis() -
        lastSPITime >= 2000)
    {
        lastSPITime =
            millis();


        uint8_t version =
            RFID_ReadRegister(
                0x37
            );


        const char hex[] =
            "0123456789ABCDEF";


        UART_SendString(
            "SPI DATA | MFRC522 Version = 0x"
        );


        UART_SendChar(
            hex[(version >> 4) & 0x0F]
        );


        UART_SendChar(
            hex[version & 0x0F]
        );


        UART_SendString(
            "\r\n"
        );
    }


    // ----------------------------------------------------
    // STATUS EVERY 1 SECOND
    // ----------------------------------------------------

    if (millis() -
        lastReportTime >= 1000)
    {
        lastReportTime =
            millis();


        float current =
            readMotorCurrent();


        UART_SendString(
            "\r\n---------- STATUS ----------\r\n"
        );


        UART_SendString(
            "Motor Current : "
        );


        UART_SendCurrent(
            current
        );


        UART_SendString(
            "\r\n"
        );


        UART_SendString(
            "Objects       : "
        );


        UART_SendNumber(
            objectCount
        );


        UART_SendString(
            "\r\n"
        );


        UART_SendString(
            "Emergency     : "
        );


        if (emergencyTriggered)
        {
            UART_SendString(
                "ACTIVE\r\n"
            );
        }
        else
        {
            UART_SendString(
                "OK\r\n"
            );
        }


        UART_SendString(
            "-----------------------------\r\n"
        );
    }
}