#include "CAN_interface.h"
#include "CAN_messageTypes.h"
#include "GPIO.h"
#include "USART_dump.h"
#include "string.h"
void startup(void) __attribute__((naked)) __attribute__((section (".start_section")) );

void startup ( void )
{
__asm volatile(
	" LDR R0,=0x2001C000\n"		/* set stack */
	" MOV SP,R0\n"
	" BL main\n"				/* call main */
	"_exit: B .\n"				/* never return */
	) ;
}



uint16_t nextAuxUnit;

static void ping(Message* msg) {
    DUMP("Ping!");
}

static void request_init(Message* msg) {
    DUMP("Request init!");
    uint32_t tempId; 
    memcpy(&tempId, (msg -> data) + 1, 4);
    CAN_centralSetInit(nextAuxUnit, tempId);
    DUMP("reqInit");
    nextAuxUnit++;
}

static void auxiliary_init(Message* msg) {
    DUMP("Auxiliary init!");
}

unsigned int counter;
bool isCentral;
#define GPIO_E (*((volatile PGPIO) 0x40021000))
//test program
void main(void) {
    nextAuxUnit = 0;
    isCentral = false;
    debug = true;
	CAN_init(isCentral);
    CAN_registerMessageHandler(PING, ping);
    CAN_registerMessageHandler(REQUEST_INIT, request_init);
    CAN_registerMessageHandler(AUXILIARY_INIT, auxiliary_init);
	if(!isCentral) 	CAN_auxRequestInit(0);
    
    
    
    counter = 0;

    while(1){
        for(int i = 0; i < 3000000; i++) {
            counter++;
        }
        counter = 0;
    }
    /*
	//GPIO_D.moder = 0x5555;
    //GPIO_D.odrL = 0;
	GPIO_E.moder = 0x5500;
    GPIO_E.pupdr = 0xAA;
    GPIO_E.odrL = 0x10;
    GPIO_E.otyper = 0;
    
    char data [] = {0xff, 0x0b, 0x31, 0x48};
    
    char low;
    char last = 0xff;
    uint16_t testAuxId = 0;
    while(1){
        //For use with keypad. If button "1" is pressed, a message is sent. 
        low = GPIO_E.idrL & 0b1;
        if(low && last != low){
            last = low;
            //DUMP("on");
            CAN_send(testAuxId, PING, data, sizeof(data));
            counter = 0;
            for(int i = 0; i <= 3000000; i++){
                counter++;
            }
        }
        else if (last != low){
            last = low;
            //DUMP("off");
        }
    }*/
}