#include "kinetis_flexcan.h"
#include <FlexCAN.h> //https://github.com/pawelsky/FlexCAN_Library

// the setup function runs once when you press reset or power the
FlexCAN can_ptr = FlexCAN(125000, 0,0,0);
CAN_message_t enable_periodic_status_1;

uint32_t spark_max_device_type_bits = 0x02;
uint32_t spark_max_manufacturer_id_bits = 0x05;

uint8_t msg_byte = 0;
void setup() {
    can_ptr.begin(); //500kpbs

    enable_periodic_status_1.ext = 1;
    enable_periodic_status_1.id =
        spark_max_device_type_bits << 24 //[28:24] device type
        | spark_max_manufacturer_id_bits << 16 //[23:16] manufacturer id
        | (0x061 | 0) << 6// [15:6] CMD_API_STAT1
        | 2; //[5:0] device ID

    for (size_t i = 0; i < 8; i++)
    {
        enable_periodic_status_1.buf[i] = 0x00;
    }

    enable_periodic_status_1.buf[0] = 0x0F;
    enable_periodic_status_1.buf[1] = 0x0F;
    enable_periodic_status_1.len = 2;
    enable_periodic_status_1.timeout = 100; //100ms to send msg before timeout
    Serial.begin(9600);
    Serial.println("Starting CAN listener");
}


// the loop function runs over and over again until power down or reset
void loop() {

    if (Serial.available() > 0)
    {
        Serial.read();
        Serial.flush();
        Serial.println("Sending can msg");
        Serial.print("CAN msg ID: ");
        Serial.println(enable_periodic_status_1.id, HEX);

        int ret_val = can_ptr.write(enable_periodic_status_1);

        Serial.print("ret val: ");
        Serial.println(ret_val);
        delay(1000);

        uint32_t can_err = CAN0_ESR1; //
        if (can_err != 0) {
            Serial.print("CAN0_ESR1: 0x");
            Serial.println(can_err, HEX);
        }
    }

}