#include <low_power.h>
#include <STM32LowPower.h>
#include "stm32f4xx.h"
#include <STM32SD.h>
#include <EEPROM.h>
#define DEBUG 0

/* Symbolic names for bit rate of CAN message                                */
typedef enum {CAN_50KBPS, CAN_100KBPS, CAN_125KBPS, CAN_250KBPS, CAN_500KBPS, CAN_1000KBPS} BITRATE;

/* Symbolic names for formats of CAN message                                 */
typedef enum {STANDARD_FORMAT = 0, EXTENDED_FORMAT} CAN_FORMAT;

/* Symbolic names for type of CAN message                                    */
typedef enum {DATA_FRAME = 0, REMOTE_FRAME}         CAN_FRAME;

typedef struct
{
  uint32_t id;        /* 29 bit identifier                               */
  uint8_t  data[8];   /* Data field                                      */
  uint8_t  len;       /* Length of data field in bytes                   */
  uint8_t  ch;        /* Object channel(Not use)                         */
  uint8_t  format;    /* 0 - STANDARD, 1- EXTENDED IDENTIFIER            */
  uint8_t  type;      /* 0 - DATA FRAME, 1 - REMOTE FRAME                */
} CAN_msg_t;

typedef const struct
{
  uint8_t TS2;
  uint8_t TS1;
  uint8_t BRP;
} CAN_bit_timing_config_t;

CAN_bit_timing_config_t can_configs[6] = {{2, 12, 56}, {2, 12, 28}, {2, 13, 21}, {2, 11, 12}, {2, 11, 6}, {1, 5, 6}};

/**
 * Print registers.
*/
void printRegister(char * buf, uint32_t reg) {
  if (DEBUG == 0) return;
  SerialUSB.print(buf);
  SerialUSB.print(reg, HEX);
  SerialUSB.println();
}

/**
 * Initializes the CAN GPIO registers.
 *
 * @params: addr    - Specified GPIO register address.
 * @params: index   - Specified GPIO index.
 * @params: speed   - Specified OSPEEDR register value.(Optional)
 *
 */
void CANSetGpio(GPIO_TypeDef * addr, uint8_t index, uint8_t speed = 3) {
    uint8_t _index2 = index * 2;
    uint8_t _index4 = index * 4;
    uint8_t ofs = 0;
    uint8_t setting;

    if (index > 7) {
      _index4 = (index - 8) * 4;
      ofs = 1;
    }

    uint32_t mask;
    printRegister("GPIO_AFR(b)=", addr->AFR[1]);
    mask = 0xF << _index4;
    addr->AFR[ofs]  &= ~mask;         // Reset alternate function
    setting = 0x9;                    // AF9
    mask = setting << _index4;
    addr->AFR[ofs]  |= mask;          // Set alternate function
    printRegister("GPIO_AFR(a)=", addr->AFR[1]);

    printRegister("GPIO_MODER(b)=", addr->MODER);
    mask = 0x3 << _index2;
    addr->MODER   &= ~mask;           // Reset mode
    setting = 0x2;                    // Alternate function mode
    mask = setting << _index2;
    addr->MODER   |= mask;            // Set mode
    printRegister("GPIO_MODER(a)=", addr->MODER);

    printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR);
    mask = 0x3 << _index2;
    addr->OSPEEDR &= ~mask;           // Reset speed
    setting = speed;
    mask = setting << _index2;
    addr->OSPEEDR |= mask;            // Set speed
    printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR);

    printRegister("GPIO_OTYPER(b)=", addr->OTYPER);
    mask = 0x1 << index;
    addr->OTYPER  &= ~mask;           // Reset Output push-pull
    printRegister("GPIO_OTYPER(a)=", addr->OTYPER);

    printRegister("GPIO_PUPDR(b)=", addr->PUPDR);
    mask = 0x3 << _index2;
    addr->PUPDR   &= ~mask;           // Reset port pull-up/pull-down
    printRegister("GPIO_PUPDR(a)=", addr->PUPDR);
}



/**
 * Initializes the CAN filter registers.
 *
 * @preconditions   - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register.
 * @params: index   - Specified filter index. index 27:14 are available in connectivity line devices only.
 * @params: scale   - Select filter scale.
 *                    0: Dual 16-bit scale configuration
 *                    1: Single 32-bit scale configuration
 * @params: mode    - Select filter mode.
 *                    0: Two 32-bit registers of filter bank x are in Identifier Mask mode
 *                    1: Two 32-bit registers of filter bank x are in Identifier List mode
 * @params: fifo    - Select filter assigned.
 *                    0: Filter assigned to FIFO 0
 *                    1: Filter assigned to FIFO 1
 * @params: bank1   - Filter bank register 1
 * @params: bank2   - Filter bank register 2
 *
 */
void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) {
  if (index > 27) return;

  CAN1->FA1R &= ~(0x1UL<<index);               // Deactivate filter

  if (scale == 0) {
    CAN1->FS1R &= ~(0x1UL<<index);             // Set filter to Dual 16-bit scale configuration
  } else {
    CAN1->FS1R |= (0x1UL<<index);              // Set filter to single 32 bit configuration
  }
    if (mode == 0) {
    CAN1->FM1R &= ~(0x1UL<<index);             // Set filter to Mask mode
  } else {
    CAN1->FM1R |= (0x1UL<<index);              // Set filter to List mode
  }

  if (fifo == 0) {
    CAN1->FFA1R &= ~(0x1UL<<index);            // Set filter assigned to FIFO 0
  } else {
    CAN1->FFA1R |= (0x1UL<<index);             // Set filter assigned to FIFO 1
  }

  CAN1->sFilterRegister[index].FR1 = bank1;    // Set filter bank registers1
  CAN1->sFilterRegister[index].FR2 = bank2;    // Set filter bank registers2

  CAN1->FA1R |= (0x1UL<<index);                // Activate filter

}

/**
 * Initializes the CAN controller with specified bit rate.
 *
 * @params: bitrate - Specified bitrate. If this value is not one of the defined constants, bit rate will be defaulted to 125KBS
 * @params: remap   - Select CAN port.
 *                    =0:CAN1_RX mapped to PA11, CAN1_TX mapped to PA12
 *                       CAN2_RX mapped to PB5 , CAN2_TX mapped to PB6
 *                    =1:Not used
 *                    =2:CAN1_RX mapped to PB8,  CAN1_TX mapped to PB9 (not available on 36-pin package)
 *                       CAN2_RX mapped to PB12, CAN2_TX mapped to PB13
 *                    =3:CAN1_RX mapped to PD0,  CAN1_TX mapped to PD1 (available on 100-pin and 144-pin package)
 *                       CAN2_RX mapped to PB12, CAN2_TX mapped to PB13
 *
 */
bool CANInit(BITRATE bitrate, int remap)
{
  // Reference manual
  // https://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf

  RCC->APB1ENR |= 0x2000000UL;           // Enable CAN1 clock
  RCC->APB1ENR |= 0x4000000UL;           // Enable CAN2 clock

  if (remap == 0) {
    // CAN1
    RCC->AHB1ENR |= 0x1;                 // Enable GPIOA clock
    CANSetGpio(GPIOA, 11);               // Set PA11
    CANSetGpio(GPIOA, 12);               // Set PA12

    // CAN2
    RCC->AHB1ENR |= 0x2;                 // Enable GPIOB clock
    CANSetGpio(GPIOB, 5);                // Set PB5
    CANSetGpio(GPIOB, 6);                // Set PB6
  }

  if (remap == 2) {
    // CAN1
    RCC->AHB1ENR |= 0x2;                 // Enable GPIOB clock
    CANSetGpio(GPIOB, 8);                // Set PB8
    CANSetGpio(GPIOB, 9);                // Set PB9

    // CAN2
    RCC->AHB1ENR |= 0x2;                 // Enable GPIOB clock
    CANSetGpio(GPIOB, 12);               // Set PB12
    CANSetGpio(GPIOB, 13);               // Set PB13
  }

  if (remap == 3) {
    // CAN1
    RCC->AHB1ENR |= 0x8;                 // Enable GPIOD clock
    CANSetGpio(GPIOD, 0);                // Set PD0
    CANSetGpio(GPIOD, 1);                // Set PD1

    // CAN2
    RCC->AHB1ENR |= 0x2;                 // Enable GPIOB clock
    CANSetGpio(GPIOB, 12);               // Set PB12
    CANSetGpio(GPIOB, 13);               // Set PB13
  }

  CAN1->MCR |= 0x1UL;                    // Require CAN1 to Initialization mode
  while (!(CAN1->MSR & 0x1UL));          // Wait for Initialization mode
  printRegister("CAN1->MCR=", CAN1->MCR);

  CAN2->MCR |= 0x1UL;                    // Require CAN2 to Initialization mode
  while (!(CAN2->MSR & 0x1UL));          // Wait for Initialization mode
  printRegister("CAN2->MCR=", CAN2->MCR);

  //CAN1->MCR = 0x51UL;                  // Hardware initialization(No automatic retransmission)
  CAN1->MCR = 0x41UL;                    // Hardware initialization(With automatic retransmission)

  //CAN2->MCR = 0x51UL;                  // Hardware initialization(No automatic retransmission)
  CAN2->MCR = 0x41UL;                    // Hardware initialization(With automatic retransmission)


  // Set bit rates
  CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF));
  CAN1->BTR |=  (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF);
  printRegister("CAN1->BTR=", CAN1->BTR);

  CAN2->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF));
  CAN2->BTR |=  (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF);
  printRegister("CAN2->BTR=", CAN2->BTR);

  // Configure Filters to default values
  CAN1->FMR  |=   0x1UL;                 // Set to filter initialization mode
  CAN1->FMR  &= 0xFFFFC0FF;              // Clear CAN2 start bank
  printRegister("CAN1->FMR=", CAN1->FMR);

  // bxCAN has 28 filters.
  // These filters are used for both CAN1 and CAN2.
  // STM32F405 has CAN1 and CAN2, so CAN2 filters are offset by 14
  CAN1->FMR  |= 0xE00;                   // Start bank for the CAN2 interface

  // Set fileter 0
  // Single 32-bit scale configuration
  // Two 32-bit registers of filter bank x are in Identifier Mask mode
  // Filter assigned to FIFO 0
  // Filter bank register to all 0
  CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL);

  // Set fileter 14
  // Single 32-bit scale configuration
  // Two 32-bit registers of filter bank x are in Identifier Mask mode
  // Filter assigned to FIFO 0
  // Filter bank register to all 0
  CANSetFilter(14, 1, 0, 0, 0x0UL, 0x0UL);

  CAN1->FMR   &= ~(0x1UL);               // Deactivate initialization mode

  uint16_t TimeoutMilliseconds = 1000;
  bool can2 = false;
  CAN2->MCR   &= ~(0x1UL);               // Require CAN2 to normal mode

  // Wait for normal mode
  // If the connection is not correct, it will not return to normal mode.
  for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) {
    if ((CAN2->MSR & 0x1UL) == 0) {
      can2 = true;
      break;
    }
    delayMicroseconds(1000);
  }
  //SerialUSB.print("can2=");
  //SerialUSB.println(can2);
  if (can2) {
    SerialUSB.println("CAN2 initialize ok");
  } else {
    SerialUSB.println("CAN2 initialize fail!!");
  }

  bool can1 = false;
  CAN1->MCR   &= ~(0x1UL);               // Require CAN1 to normal mode

  // Wait for normal mode
  // If the connection is not correct, it will not return to normal mode.
  for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) {
    if ((CAN1->MSR & 0x1UL) == 0) {
      can1 = true;
      break;
    }
    delayMicroseconds(1000);
  }
  //SerialUSB.print("can1=");
  //SerialUSB.println(can1);
  if (can1) {
    SerialUSB.println("CAN1 initialize ok");
  } else {
    SerialUSB.println("CAN1 initialize fail!!");
    //return false;
  }
  return true;
}


#define STM32_CAN_TIR_TXRQ              (1U << 0U)  // Bit 0: Transmit Mailbox Request
#define STM32_CAN_RIR_RTR               (1U << 1U)  // Bit 1: Remote Transmission Request
#define STM32_CAN_RIR_IDE               (1U << 2U)  // Bit 2: Identifier Extension
#define STM32_CAN_TIR_RTR               (1U << 1U)  // Bit 1: Remote Transmission Request
#define STM32_CAN_TIR_IDE               (1U << 2U)  // Bit 2: Identifier Extension

#define CAN_EXT_ID_MASK                 0x1FFFFFFFU
#define CAN_STD_ID_MASK                 0x000007FFU

/**
 * Decodes CAN messages from the data registers and populates a
 * CAN message struct with the data fields.
 *
 * @preconditions     - A valid CAN message is received
 * @params CAN_rx_msg - CAN message structure for reception
 *
 */
void CANReceive(uint8_t ch, CAN_msg_t* CAN_rx_msg)
{
  if(ch == 1) {
    uint32_t id = CAN1->sFIFOMailBox[0].RIR;
    if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format
        CAN_rx_msg->format = STANDARD_FORMAT;;
        CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U));
    }
    else {                               // Extended frame format
        CAN_rx_msg->format = EXTENDED_FORMAT;;
        CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U));
    }

    if ((id & STM32_CAN_RIR_RTR) == 0) {  // Data frame
        CAN_rx_msg->type = DATA_FRAME;
    }
    else {                                // Remote frame
        CAN_rx_msg->type = REMOTE_FRAME;
    }


    CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL;

    CAN_rx_msg->data[0] = 0xFFUL &  CAN1->sFIFOMailBox[0].RDLR;
    CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8);
    CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16);
    CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24);
    CAN_rx_msg->data[4] = 0xFFUL &  CAN1->sFIFOMailBox[0].RDHR;
    CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8);
    CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16);
    CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24);

    CAN1->RF0R |= 0x20UL;
  } // end CAN1

  if(ch == 2) {
    uint32_t id = CAN2->sFIFOMailBox[0].RIR;
    if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format
        CAN_rx_msg->format = STANDARD_FORMAT;;
        CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U));
    }
    else {                               // Extended frame format
        CAN_rx_msg->format = EXTENDED_FORMAT;;
        CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U));
    }

    if ((id & STM32_CAN_RIR_RTR) == 0) {  // Data frame
        CAN_rx_msg->type = DATA_FRAME;
    }
    else {                                // Remote frame
        CAN_rx_msg->type = REMOTE_FRAME;
    }


    CAN_rx_msg->len = (CAN2->sFIFOMailBox[0].RDTR) & 0xFUL;

    CAN_rx_msg->data[0] = 0xFFUL &  CAN2->sFIFOMailBox[0].RDLR;
    CAN_rx_msg->data[1] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 8);
    CAN_rx_msg->data[2] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 16);
    CAN_rx_msg->data[3] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 24);
    CAN_rx_msg->data[4] = 0xFFUL &  CAN2->sFIFOMailBox[0].RDHR;
    CAN_rx_msg->data[5] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 8);
    CAN_rx_msg->data[6] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 16);
    CAN_rx_msg->data[7] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 24);

    CAN2->RF0R |= 0x20UL;
  } // END CAN2

}

/**
 * Encodes CAN messages using the CAN message struct and populates the
 * data registers with the sent.
 *
 * @params CAN_tx_msg - CAN message structure for transmission
 *
 */
void CANSend(uint8_t ch, CAN_msg_t* CAN_tx_msg)
{
  volatile int count = 0;

  uint32_t out = 0;
  if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format
      out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE;
  }
  else {                                       // Standard frame format
      out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U);
  }

  // Remote frame
  if (CAN_tx_msg->type == REMOTE_FRAME) {
      out |= STM32_CAN_TIR_RTR;
  }

  if (ch == 1) {
    CAN1->sTxMailBox[0].TDTR &= ~(0xF);
    CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL;

    CAN1->sTxMailBox[0].TDLR  = (((uint32_t) CAN_tx_msg->data[3] << 24) |
                                 ((uint32_t) CAN_tx_msg->data[2] << 16) |
                                 ((uint32_t) CAN_tx_msg->data[1] <<  8) |
                                 ((uint32_t) CAN_tx_msg->data[0]      ));
    CAN1->sTxMailBox[0].TDHR  = (((uint32_t) CAN_tx_msg->data[7] << 24) |
                                 ((uint32_t) CAN_tx_msg->data[6] << 16) |
                                 ((uint32_t) CAN_tx_msg->data[5] <<  8) |
                                 ((uint32_t) CAN_tx_msg->data[4]      ));

    // Send Go
    CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ;

    // Wait until the mailbox is empty
    while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000);

    // The mailbox don't becomes empty while loop
    if (CAN1->sTxMailBox[0].TIR & 0x1UL) {
      SerialUSB.println("Send Fail");
      SerialUSB.println(CAN1->ESR);
      SerialUSB.println(CAN1->MSR);
      SerialUSB.println(CAN1->TSR);
    }
  } // end CAN1

  if (ch == 2) {
    CAN2->sTxMailBox[0].TDTR &= ~(0xF);
    CAN2->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL;

    CAN2->sTxMailBox[0].TDLR  = (((uint32_t) CAN_tx_msg->data[3] << 24) |
                                 ((uint32_t) CAN_tx_msg->data[2] << 16) |
                                 ((uint32_t) CAN_tx_msg->data[1] <<  8) |
                                 ((uint32_t) CAN_tx_msg->data[0]      ));
    CAN2->sTxMailBox[0].TDHR  = (((uint32_t) CAN_tx_msg->data[7] << 24) |
                                 ((uint32_t) CAN_tx_msg->data[6] << 16) |
                                 ((uint32_t) CAN_tx_msg->data[5] <<  8) |
                                 ((uint32_t) CAN_tx_msg->data[4]      ));

    // Send Go
    CAN2->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ;

    // Wait until the mailbox is empty
    while(CAN2->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000);

    // The mailbox don't becomes empty while loop
    if (CAN2->sTxMailBox[0].TIR & 0x1UL) {
      SerialUSB.println("Send Fail");
      SerialUSB.println(CAN1->ESR);
      SerialUSB.println(CAN1->MSR);
      SerialUSB.println(CAN1->TSR);
    }
  } // end CAN2

}

 /**
 * Returns whether there are CAN messages available.
 *
 * @returns If pending CAN messages are in the CAN controller
 *
 */
uint8_t CANMsgAvail(uint8_t ch)
{
  if (ch == 1) {
    // Check for pending FIFO 0 messages
    return CAN1->RF0R & 0x3UL;
  } // end CAN1

  if (ch == 2) {
    // Check for pending FIFO 0 messages
    return CAN2->RF0R & 0x3UL;
  } // end CAN2

}

uint8_t insertStr(uint8_t index, char* b1, char* b2)
{
  uint8_t i;
  for(i = index; i < index + strlen(b1); i++)
  {
    b2[i] = b1[i - index];
  }
  return i;
}

File dataFile;

uint8_t counter = 0;
uint8_t frameLength = 0;
unsigned long previousMillis = 0;     // stores last time output was updated
bool haveBeenConnected = false;
unsigned long oldFileSize;
uint16_t fileVolume = 0;
char fileIndex;
char filename[20];
char buf[30];
char bigBuf[64];

void setup() {
  SerialUSB.begin(115200);               // start up the uart module and initialize
  while(!SerialUSB.available()) ;
  while(SerialUSB.available()) SerialUSB.read();
  SerialUSB.println("Startup");
  delay(1000);                        // give the CAN wires time to connect
  LowPower.begin();                   // start up the low power stuff
  bool ret = CANInit(CAN_250KBPS, 2);  // CAN_RX mapped to PB12, CAN_TX mapped to PB13
  while(!ret) ret = CANInit(CAN_250KBPS, 2); // init CAN while we haven't
  pinMode(PA5, OUTPUT); //Green LED
  pinMode(PA6, OUTPUT); //Yellow LED
  pinMode(PA7, OUTPUT); //Red LED
  pinMode(PA3, INPUT);  //Plugged in detection
  SerialUSB.print("Initializing SD card...");
  // see if the card is present and can be initialized:
  uint8_t attempts = 0;
  while (!SD.begin(SD_DETECT_NONE))
  {
    delay(100);
    attempts++;
    if(attempts > 20)
    {
      SerialUSB.println("Initialization failed.");
      digitalWrite(PA7, HIGH);
      while(true) ;
    }
  }
  delay(100);
  SerialUSB.println("card initialized.");

  // Create incrementing filename
  if(EEPROM.read(1) != 0) { //This might be the first time this program has ever run
    EEPROM.write(0, 0x00);  //so initialize the filename incrementing index at 0
    EEPROM.write(1, 0x00);  //and store something that says it's been initialized
  }

  fileIndex = (char) EEPROM.read(0);
  sprintf(filename, "%s_%d%d.csv", "log", fileIndex, fileVolume);
  // open the file
  dataFile = SD.open(filename, FILE_WRITE);
  // if the file is available, seek to last position
  if (dataFile) {
    EEPROM.write(0, (byte) (fileIndex + 1));
    dataFile.seek(dataFile.size());
    oldFileSize = dataFile.size();
    SerialUSB.println("Opened file for writing");
  }
  // if the file isn't open, pop up an error, light the red LED and halt
  else {
    SerialUSB.println("error opening file for writing");
    digitalWrite(PA7, HIGH);
    while(true) ;
  }
}

void loop() {
  CAN_msg_t CAN_TX_msg;
  CAN_msg_t CAN_RX_msg;

  bool connectedToCAN = true;//digitalRead(PA3) == HIGH;
  if(SerialUSB.available()) connectedToCAN = false;
  digitalWrite(PA5, connectedToCAN); //green LED is CAN connection status
  if(connectedToCAN) {
    uint8_t recv_ch = 2;
    digitalWrite(PA6, LOW);
    if(CANMsgAvail(recv_ch)) {
      haveBeenConnected = true; //light the yellow LED cause we've got a message
      digitalWrite(PA6, HIGH);
      CANReceive(recv_ch, &CAN_RX_msg);

      if(dataFile) { //if the file's still open
        //digitalWrite(PA7, HIGH); //indicate that we're writing to the SD card
        //Add timestamp,messageID,byte,byte,byte,...
        uint8_t index = 0;
        dataFile.seek(dataFile.size());
        sprintf(buf, "%ld,\0", millis() - previousMillis);
        previousMillis = millis();
        index = insertStr(index, buf, bigBuf);
        //dataFile.print(buf);
        sprintf(buf, "%X,\0", CAN_RX_msg.id);
        index = insertStr(index, buf, bigBuf);
        //dataFile.print(buf);
        for(uint8_t i=0; i<CAN_RX_msg.len; i++) {
          sprintf(buf, "%02X\0", CAN_RX_msg.data[i]);
          index = insertStr(index, buf, bigBuf);
          //dataFile.print(buf);
        }
        index = insertStr(index, "\n\0", bigBuf);
        SerialUSB.print(dataFile.write(bigBuf, index));
        //SerialUSB.write(bigBuf, index);
        SerialUSB.print("   ");
        SerialUSB.print(dataFile.size());
        SerialUSB.print(" ");
        SerialUSB.println(oldFileSize);

        if(dataFile.size() - oldFileSize > 2000) { //Write to SD card before unwritten data becomes too large
          digitalWrite(PA7, HIGH); //indicate that we're writing to the SD card
          dataFile.flush();
          digitalWrite(PA7, LOW);
          oldFileSize = dataFile.size();
        }

        if(dataFile.size() > 16777216) { //Limit individual file size to 16MB
          dataFile.close();
          fileVolume += 1;
          sprintf(filename, "%s_%d%d.csv", "log", fileIndex, fileVolume);
          dataFile = SD.open(filename, FILE_WRITE);
          oldFileSize = dataFile.size();
        }
      }
    }
  }
  else if(haveBeenConnected) {
    SerialUSB.println("Powering Down");
    dataFile.close(); //Neatly close down the file before we lose power (same as flush but also closes file)
    delay(1000);
    Serial.println("\nFiles found on the card (name, date and size in bytes): ");
    File root = SD.openRoot();
    // list all files in the card with date and size
    root.ls(LS_R | LS_DATE | LS_SIZE);
    root.close();
    SerialUSB.println();
    SerialUSB.println(filename);
    delay(1000);
    File reader = SD.open(filename);

    // if the file is available, write to it:
    if (reader) {
      while (reader.available()) {
        SerialUSB.write(reader.read());
      }
      reader.close();
    }

    digitalWrite(PA5, LOW); //Shut off all the lights
    digitalWrite(PA6, LOW);
    digitalWrite(PA7, LOW);

    while(true) ;

    //LowPower.deepSleep(); //Go into deep sleep to allow the SD card as much time to finish writing as possible
  }
}