#include "misc.h"
#include "stm32f4xx_can.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "SysTick.h"
#include "CAN_interface.h"
#include "CAN_messageTypes.h"
#include "USART_dump.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MAX_NUMBER_OF_DOORS 15

__attribute__((naked)) __attribute__((section (".start_section")) )
void startup ( void )
{
__asm__ volatile(" LDR R0,=0x2001C000\n");		/* set stack */
__asm__ volatile(" MOV SP,R0\n");
__asm__ volatile(" BL main\n");					/* call main */
__asm__ volatile(".L1: B .L1\n");				/* never return */
}

uint16_t auxId;

//is true when the central unit has sent a can message with init info
bool isInited = false;

// queue struct
typedef struct{
    int mainTop;
    int helperTop;
    char * mainQueue[MAX_NUMBER_OF_DOORS];
    char * helperQueue[MAX_NUMBER_OF_DOORS];
} Queue;


uint16_t alarmDelayms;
uint8_t numberOfDoors;

bool activeDoors[MAX_NUMBER_OF_DOORS];
bool alarmsCalled[MAX_NUMBER_OF_DOORS];

Queue alarms;
// the two functions below are defined to create a queue to see who started each timer
void enqueue(char data){
    while(alarms.mainTop > -1){
        alarms.helperTop++;
        alarms.helperQueue[alarms.helperTop] = alarms.mainQueue[alarms.mainTop];
        alarms.mainTop--;
    }
    alarms.mainTop++;
    alarms.mainQueue[alarms.mainTop];
    while(alarms.helperTop > -1){
        alarms.mainTop++;
        alarms.mainQueue[alarms.helperTop] = alarms.helperQueue[alarms.mainTop];
        alarms.helperTop--;
    }
    
}

char dequeue(){
    char data = alarms.mainQueue[alarms.mainTop];
    alarms.mainTop--;
    return data;
}


//sets the door pins to input and pullup
void doorAlarmInit(){
    
    GPIO_InitTypeDef GPIOx_init;
    
    int pinNumber = 1;
    for(int i = 0; i < MAX_NUMBER_OF_DOORS; i++){
        GPIOx_init.GPIO_Pin |= pinNumber;
        pinNumber = 1<<pinNumber;
    }
    GPIOx_init.GPIO_Mode = GPIO_Mode_IN;
    GPIOx_init.GPIO_PuPd = GPIO_PuPd_UP;
    
    GPIO_Init(GPIOD, &GPIOx_init);
}
//sets the lamps pins to output
void lampsInit(){
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
    
    GPIO_InitTypeDef GPIOx_init;
    
    GPIOx_init.GPIO_Pin = GPIO_Pin_All;
    GPIOx_init.GPIO_Mode = GPIO_Mode_OUT;
    GPIOx_init.GPIO_OType = GPIO_OType_PP;
    GPIOx_init.GPIO_Speed = GPIO_Low_Speed;
    GPIOx_init.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOE, &GPIOx_init);
}
//all of the functions below are handlers for diffrent types of can messages
void aux_config_handler(Message message){
    numberOfDoors = message.data[0];
    alarmDelayms = (message.data[1]<<8) || message.data[2];
}

void aux_init_handler(Message message){
    auxId = (message.data[1] << 8) || message.data[2];
    numberOfDoors = message.data[0];
    alarmDelayms = (message.data[3]<<8) || message.data[4];
    isInited = true;
}

void aux_activate_handler(Message message){
    if(message.data[0] == 0xff){
        for(int i = 0; i < numberOfDoors; i++){
            activeDoors[i] = 1;
        }   
    }else{
        activeDoors[message.data[0]] = 1;
    }
}

void aux_deact_handler(Message message){
    if(message.data == 0xff){
        for(int i = 0; i < numberOfDoors; i++){
            activeDoors[i] = 0;
        }   
    }else{
        activeDoors[message.data[0]] = 0;
    }
}

void aux_info_handler(Message message){
    uint8_t* data;
    data[0] = alarmDelayms && 0xff;
    data[1] = alarmDelayms && 0xff00;
    data[2] = numberOfDoors;
    for(int i = 0; i < numberOfDoors; i++){
        if(i < 8){
            data[3] |= activeDoors[i] && (1<<i);
        }else{
            data[4] |= activeDoors[i] && (1<<(i-8));
        }
    }
}
// inits can, gpio and timers and sets necisary local variables, and sends a request to the central unit asking to be inited
void init(){
    SysTick_initTimers();
    lampsInit();
    doorAlarmInit(GPIOD);
    CAN_init(false);
    
    alarms.helperTop = -1;
    alarms.mainTop = -1;
    
     for(int i = 0; i < MAX_NUMBER_OF_DOORS; i++){
        alarmsCalled[i] = 0;
    }
    
    CAN_registerMessageHandler(AUXILIARY_INIT, aux_init_handler);
    CAN_registerMessageHandler(AUX_ACTIVATE, aux_activate_handler);
    CAN_registerMessageHandler(AUX_DEACTIVATE, aux_deact_handler);
    CAN_registerMessageHandler(AUX_CONFIG, aux_config_handler);
    CAN_registerMessageHandler(AUX_INFO, aux_info_handler);
    
    CAN_auxRequestInit(0);
}
// sends an alarm message if a door is still open after a timer runs out
void callAlarm(){
    DUMP("callAlarm");
    int alarmCaller = dequeue(); 
    alarmsCalled[alarmCaller] = 0;
    if(!GPIO_ReadInputDataBit(GPIOD, (alarmCaller<<1)) && activeDoors[alarmCaller]){
        CAN_send(auxId, ALARM, 0,0);
        DUMP("timer finished alarm message sent");
    }else{
        GPIO_WriteBit(GPIOE, GPIO_Pin_15, Bit_RESET);
    }
}

// checks if a door that is open is active and if so calls an alarm by calling a timer and turing on the alarm light
void checkDoors(){
    for(int i = 0; i < numberOfDoors; i++){
        if(activeDoors[i]){
            if(GPIO_ReadInputDataBit(GPIOD, (1<<i)) && !alarmsCalled[i]){
                DUMP("alarm went off, light should be on");
                GPIO_WriteBit(GPIOE, GPIO_Pin_15, Bit_SET);
                enqueue(i);
                SysTick_startTimer(alarmDelayms, callAlarm);
                alarmsCalled[i] = 1;
            }
        }
    }
}
// checks if a door is unlocked and if so turns on the green light
void checkLamps(){
    for(int i = 0; i < numberOfDoors; i++){
        if(activeDoors[i]){
            GPIO_WriteBit(GPIOE, (1<<i), Bit_RESET);
        }else{
            GPIO_WriteBit(GPIOE, (1<<i), Bit_SET);
        }
    }
        
}






void main(void)
{
    // inits the local parts and asks the cental unit to be inited, then it waits for the central unit to send 
    // an init message then starts checking the doors and lights/turns of the lamps accordingly
    init();
    while(!isInited);
    while(1){
        checkDoors();
        checkLamps();
    }
}