#include "can.h"


u8 can_init() {
	// Enable CAN1 peripheral
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);
    // Enable AFIORCC_APB2Periph_AFIO | GPIOA
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA, ENABLE);

    // Set up GPIO on port A
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    // CAN1_RX - PA11
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // CAN1_TX - PA12
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // Disable filters so we can actually receive things
    CAN_FilterInitTypeDef CAN_FilterInitStruct;
    CAN_FilterInitStruct.CAN_FilterNumber = 0;
    CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStruct.CAN_FilterIdHigh = 0x0000;
    CAN_FilterInitStruct.CAN_FilterIdLow = 0x0000;
    CAN_FilterInitStruct.CAN_FilterMaskIdHigh = 0x0000;
    CAN_FilterInitStruct.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStruct.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStruct.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStruct);

    // Init the can controller, set baud rate to 1Mbps
    // Formula for baud:
    //   SysClk = 72MHz
    //   AHB = 72MHz
    //   APB1 = AHB / 2 = 36MHz
    //   Bitrate = APB1 / ((BS1 + BS2 + 1) * Prescaler)
    //   1Mbaud = 36MHz / ((14 + 3 + 1) * 2)
    //
    // Other values that work, BS1=5, BS2=3, Div=4
    // For now I use values that are as close to CTRE's candle light code
    CAN_InitTypeDef CAN_InitStructure;
    CAN_StructInit(&CAN_InitStructure);
    CAN_InitStructure.CAN_Prescaler = 4;
    CAN_InitStructure.CAN_SJW = CAN_SJW_1tq;
    CAN_InitStructure.CAN_BS1 = CAN_BS1_5tq;
    CAN_InitStructure.CAN_BS2 = CAN_BS2_3tq;

    return CAN_Init(CAN1, &CAN_InitStructure);
}

u8 can_recv(CanRxMsg *RxMessage) {
	u8 got_msg = 0;

	if(CAN_MessagePending(CAN1, CAN_FIFO0)) {
		CAN_Receive(CAN1, CAN_FIFO0, RxMessage);
		got_msg = 1;
	} else if(CAN_MessagePending(CAN1, CAN_FIFO1)) {
		CAN_Receive(CAN1, CAN_FIFO1, RxMessage);
		got_msg = 1;
	}

	return got_msg;
}