#include "can.h"
#include <xc.h> //XC32 prekladac
#include <sys/kmem.h> //obsahuje makra pro praci s adresami

unsigned int bufferCan[8]; //TX FIFO (FIFO 0), 1 cela msg = 1 * 4 => 4 + 4 = 8
                           //RX FIFO (FIFO 1), 1 cela msg = 1 * 4

                           //cela msg = 4
                           //data msg = 2 (pouze RX)

unsigned int* msg; //ukazatel na msg

unsigned int idCan; //prijata adresa
unsigned char dataCan[8]; //prijata data

void initCan(void) {
    CAN_RX_TRIS = 1; //vstup
    C1RXR = 4;  //CAN 1 RX (C1RX) pin na RPF0
    RPF1R = 12; //CAN 1 TX (C1TX) pin na RPF1
    
    //init CAN
    //C1CONbits.ON = 0; //vypnout
    //while(C1CONbits.CANBUSY != 0);//dokud se nevypne
    
    C1CONbits.ON = 1; //zapnout
            
    C1CONbits.REQOP = 4; //configuration mode
    while(C1CONbits.OPMOD != 4); //dokud se nenastavi configuration mode
    
    C1FIFOCON0bits.FSIZE = 0; //1 msg FIFO
    C1FIFOCON0bits.TXEN = 1; //TX msg
    C1FIFOBA = KVA_TO_PA(bufferCan); //fyzicka adresa zacatku bufferu
    
    C1FIFOCON1bits.FSIZE = 0; //1 msg FIFO
    C1FIFOCON1bits.TXEN = 0; //RX msg
    C1FIFOCON1bits.DONLY = 0; //cela msg
    
    C1CFGbits.SEG2PH = 2; //3 * TQ - nastaveni prenosove rychlosti
    C1CFGbits.SEG2PHTS = 1; //nastaveno rucne
    C1CFGbits.SEG1PH = 2; //3 * TQ
    C1CFGbits.PRSEG = 2; //3 * TQ
    C1CFGbits.BRP = 3; //TQ = 1 us -> 10 * TQ = 100 kHz
    
    C1FLTCON0bits.FSEL0 = 1; //ukladat prijata data do FIFO 1 - nastaveni filtru
    C1FLTCON0bits.MSEL0 = 0; //maska 0
    C1RXF0bits.SID = 0x555; //SID 0x555
    C1RXF0bits.EXID = 0; //porovnavat jen standard data frame msg
    C1RXM0bits.SID = 0x7FF; //maskovani filtru, 0x000 - 0x7FF (0 - ignorovat)
    C1RXM0bits.MIDE = 1; //porovnavat jen standard data frame msg
    C1FLTCON0bits.FLTEN0 = 1; //povolit filter 0
    
    C1CONbits.REQOP = 0; //normal operation mode
    while(C1CONbits.OPMOD != 0); //dokud se nenastavi normal operation mode
}

void writeCan(unsigned int id, unsigned char n, unsigned char* data) {
    unsigned char i;
    
    if(id > 2047) { //pouze 11 bitu
        return;
    }
    
    if(n > 8) { //pouze 8 bajtu dat
        return;
    }
    
    msg = PA_TO_KVA1(C1FIFOUA0); //adresa volneho mista ve FIFO
    *(msg + 0) = id; //adresa (poslednich 11 bitu)
    *(msg + 1) = (unsigned int) n; //pocet dat (0 - 8 bajtu)
    
    *(msg + 2) = 0; //nulovani bufferu
    *(msg + 3) = 0;
    
    for(i = 0; i < n; i++) { //zapis do bufferu
        if (i < 4) {
            *(msg + 2) |= ((unsigned int)*(data + i)) << (8 * i);
        }
        
        else {
            *(msg + 3) |= ((unsigned int)*(data + i)) << (8 * (i - 4));
        }
    }
    
    C1FIFOCON0bits.UINC = 1; //odeslat
    C1FIFOCON0bits.TXREQ = 1;
    while(C1FIFOCON0bits.TXREQ == 1); //dokud se neodesle
}

unsigned char readCan(void) {
    unsigned char n, i;
    
    while(C1FIFOINT1bits.RXNEMPTYIF == 0); //pockat na prijem msg
    
    msg = PA_TO_KVA1(C1FIFOUA1); //adresa volneho mista ve FIFO
    
    idCan = *msg & 0x000007FF; //adresa (poslednich 11 bitu)
    n = (unsigned char)(*(msg + 1) & 0x0000000F); //pocet dat (0 - 8 bajtu)
    
    if(n > 8) { //maximalne 8 bajtu dat
        n = 8;
    }
    
    for(i = 0; i < n; i++) { //cteni z bufferu a zapis do pole uzivatele
        if(i < 4) {
            *(dataCan + i) = (unsigned char)((*(msg + 2) >> (8 * i)) & 0x000000FF);
        }
        
        else {
            *(dataCan + i) = (unsigned char)((*(msg + 3) >> (8 * (i - 4))) & 0x000000FF);
        }
    }
    
    C1FIFOCON1bits.UINC = 1; //precteno
    
    return n;
}