#include "stm32f4xx.h"                  // Device header
#include "FreeRTOS.h"                   // ARM.FreeRTOS::RTOS:Core
#include "FreeRTOSConfig.h"             // ARM.FreeRTOS::RTOS:Config
#include "Task.h"
#include "queue.h"
#include "semphr.h"
//MAP 0x40000000, 0x400FFFFF EXEC READ WRITE

SemaphoreHandle_t A5_Semaphore = NULL;
TickType_t WakeTime1 = NULL, WakeTime2 = NULL;
TaskHandle_t Task1Handle = NULL, Task2Handle = NULL;

void vTask1(void* pvParams)
{
    TickType_t WakeTime1 = xTaskGetTickCount();
    while(1)
    {
        if( A5_Semaphore != NULL){
         //If the semaphore exists
            if( ( xSemaphoreTake( A5_Semaphore, 100 ) == pdTRUE )
                || ( xSemaphoreGetMutexHolder( A5_Semaphore ) == Task1Handle ) )
            { //If the semaphore was succesfully taken or Task1 already has it
                for( char i=0; i<4; i++ ){
                    GPIOA->ODR |= GPIO_ODR_ODR_5;
                    vTaskDelay(pdMS_TO_TICKS(500));
                    GPIOA->ODR &= ~GPIO_ODR_ODR_5;
                    vTaskDelay(pdMS_TO_TICKS(500));
                }
                xSemaphoreGive(A5_Semaphore);
                vTaskDelayUntil(&WakeTime1, pdMS_TO_TICKS(4000));
            }
            else taskYIELD();
        }
    }
}


void vTask2(void* pvParams){
    TickType_t WakeTime2 = xTaskGetTickCount();
    while(1){
        if( A5_Semaphore != NULL ){
             //If the semaphore exists
            if( ( xSemaphoreTake( A5_Semaphore, 100 ) == pdTRUE )
                || ( xSemaphoreGetMutexHolder( A5_Semaphore ) == Task2Handle ) )
            { //If the semaphore was succesfully taken or Task2 already has it
                for( char i=0; i<3; i++ ){
                    GPIOA->ODR |= GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(133));
                    GPIOA->ODR &= ~GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(133));
                }
                for( char i=0; i<3; i++ ){
                    GPIOA->ODR |= GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(334));
                    GPIOA->ODR &= ~GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(334));
                }
                for( char i=0; i<3; i++ ){
                    GPIOA->ODR |= GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(133));
                    GPIOA->ODR &= ~GPIO_ODR_ODR_6;
                    vTaskDelay(pdMS_TO_TICKS(133));
                }
                xSemaphoreGive(A5_Semaphore);
                vTaskDelayUntil(&WakeTime2, pdMS_TO_TICKS(4000));
            }
            else taskYIELD();
        }
    }
}


int main(void){
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
    //pin A5
    GPIOA->MODER |= GPIO_MODER_MODER5_0 | GPIO_MODER_MODER6_0;

    xTaskCreate(vTask2, "DiodeSOS", configMINIMAL_STACK_SIZE, NULL, 1, &Task2Handle);
    xTaskCreate(vTask1, "DiodeBlink", configMINIMAL_STACK_SIZE, NULL, 1, &Task1Handle);
    A5_Semaphore = xSemaphoreCreateMutex();
    vTaskStartScheduler();
    while(1){

    }
}