#include <SPI.h>

#include <Canbus.h>
#include <defaults.h>
#include <global.h>
#include <mcp2515.h>
#include <mcp2515_defs.h>

#include <SD.h>

#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x3f, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

File logFile;

// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;
const int chipSelect = 4;

uint32_t ticks = 0;

tCAN     canBuffer[256]; // Must be 256 to match the uint8_t indexes for head and tail
uint8_t  canHead, canTail;
uint32_t canOverflow, canErrors;
bool     canRunning;

void startTimer(Tc *tc, uint32_t channel, IRQn_Type irq, uint32_t frequency) {
        pmc_set_writeprotect(false);
        pmc_enable_periph_clk((uint32_t)irq);
        TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK4);
        uint32_t rc = VARIANT_MCK/128/frequency; //128 because we selected TIMER_CLOCK4 above
        TC_SetRA(tc, channel, rc/2); //50% high, 50% low
        TC_SetRC(tc, channel, rc);
        TC_Start(tc, channel);
        tc->TC_CHANNEL[channel].TC_IER=TC_IER_CPCS;
        tc->TC_CHANNEL[channel].TC_IDR=~TC_IER_CPCS;
        NVIC_EnableIRQ(irq);
}

void TC3_Handler()
{
    ticks++;
    TC_GetStatus(TC1, 0);
}

/*
 * canHandler
 * 
 * Interrupt handler for CAN receive interrupt.
 * There is no need to perform the mcp2515_check_message() function since this only
 * checks the status of the interrupt line.
 * 
 * If mcp2515_get_message() returns 0 (no message available), the canError is incremented.
 * If a buffer overrun is detected (canHead bites canTail), the canOverflow counter is incremented.
 * 
 * Note: the canBuffer is 256 items and matches the uint8_t type of canHead and canTail.
 * This makes the head and tail to wrap automaticaly from 255 to 0.
 */
void canHandler()
{
    uint8_t newHead = canHead;

    digitalWrite(6,1);
    if(canRunning)
    {
        newHead++;
    
        /*
         * If head bites tail, increment overflow counter and overwrite last item in list
         */
        if(newHead == canTail)
        {
            canOverflow++;
        } else
        {
            canHead = newHead;
        }
    
        /*
         * Read message into head of canBuffer and check for errors 
         */
        canBuffer[canHead].timestamp = ticks;
        if(!mcp2515_get_message(&canBuffer[canHead]))
        {
            canErrors++;
        }
    } else
    {
        // CAN not running, read message and drop.
        mcp2515_get_message(&canBuffer[0]);
    }
    digitalWrite(6,0);
    return;
}

/*
 * Read a CAN message into the given buffer.
 * Returns false if no message available.
 */
bool canRead(tCAN *message)
{
    if(canHead== canTail)
    {
        return false;
    }
    memcpy(message, &canBuffer[canTail], sizeof(tCAN));
    canTail++;
    return true;
}

void setup() {
    Serial.begin(115200);
    Serial.println("Starting demo");

    /*
     * Initialize CAN interface
     * start the CAN interrupt but 
     */
    if(Canbus.init(CANSPEED_250))  //Initialise MCP2515 CAN controller at the specified speed
        Serial.println("CAN Init ok");
    else
        Serial.println("Can't init CAN");

    canHead    = 0;
    canTail    = 0;
    canRunning = false;

    attachInterrupt(digitalPinToInterrupt(MCP2515_INT), canHandler, FALLING); // attach canHandler interrupt

    Serial.print("\nInitializing SD card...");

    // Initialize SD Card
  
    if (!SD.begin(1000000, chipSelect))
    {
        Serial.println("initialization failed!");
        while (1);
    }
    logFile = SD.open("logfile.txt", FILE_WRITE);
    if(logFile)
    {
        logFile.println("Opening logfile");
        Serial.println("Logfile.txt opened");
    }

    // Setup LCD
    
    lcd.begin(20, 4);
    lcd.clear();
    lcd.print("hello!");

    pinMode(6, OUTPUT);
    pinMode(7, OUTPUT);
    digitalWrite(6,0);
    digitalWrite(7,0);
    startTimer(TC1, 0, TC3_IRQn, 1000);
    canRunning = true;

    
}

void loop() {

    tCAN message;
    char str[100];
    
    if (canRead(&message)) 
    {
        digitalWrite(7,1);

        sprintf(str,"%5d, %3x[%d], %02x %02x %02x %02x %02x %02x %02x %02x", message.timestamp, message.id, message.header.length, 
                message.data[0], message.data[1], message.data[2], message.data[3], message.data[4], message.data[5], message.data[6], message.data[7]); 
        logFile.println(str);
        digitalWrite(7,0);

    }
    
    if(ticks > 5000)
    {
        canRunning = false;
        logFile.close();
        Serial.println("Ready");
        Serial.print("canErrors = "); Serial.println(canErrors);
        Serial.print("canOverflow = "); Serial.println(canOverflow);
        while(1);
    }
}