 
// Mod by JDN  (feb 24)
#include <FlexCAN.h>

#ifndef __MK66FX1M0__
#error "Teensy 3.6 with dual CAN bus is required to run this example"
#endif

// CAN0 1 Mbit/sec
//ID 0  (pin 29,30)
// txAlternative  if 1 use std pin 29 else pin 3
// rxAlternative  if 1 usestd pin30, else pin 4
// -----------------------------------------------------
// CAN1 1 Mbit/sec
// ID 1  (pin 34,35)
// txAlternative  pin 33 - no alternative bq pins sre not broken out on teensy 3.6
// rxAlternative  pin 34 -  no alternative bq pins sre not broken out on teensy 3.6

FlexCAN Can0(100000, 0, 1, 1);

FlexCAN Can1(100000, 1, 1, 1);

static CAN_message_t msg0, msg1;



int initCAN() {
  CAN_filter_t mask;
  //Setting pin 35 / 28 low enables high speed mode on the two transceivers
  pinMode(28, OUTPUT);
  pinMode(35, OUTPUT);

  digitalWrite(28, LOW);
  digitalWrite(35, LOW);
  delay(10);
  mask.ext = 0;
  mask.rtr=0;
  mask.id = 0;
  Can0.begin(mask);
  Can1.begin(mask);

  return 0;  // for future use
}




uint32_t tCan0 = 0, tCan0Per = 10;

uint32_t tFlt = 0, tFltPer = 2000;

void setup(void) {
  
  delay(100);
  Serial.begin(57600);

  while (!Serial)
    ;  // wait
  Serial.println("cantest2");

  pinMode(13, OUTPUT);  // for blinking


  msg0.id = 0;
  msg0.ext = 1;

  msg0.buf[0] = 1;

  msg1.id = 0;
  msg1.ext = 1;

  msg1.buf[0] = 1;

  initCAN();



  Serial.println("bef start- wait two seconds");
  Serial.println(millis());
  delay(2000);

  // lert me throw the first packet for the ping-pong
  tCan0 = tFlt = millis();
}

void setFilter(int iface, int onoff, int id) {
  CAN_filter_t mask;
  mask.ext = 0;  // 11 bit
  mask.rtr = 0;
  if (onoff) {
    mask.id = id;
  } else {
    mask.id = 0;
  }

  if (iface == 0) {
    Can0.end();
    Can0.begin(mask);
  } else {
    Can1.end();
    Can1.begin(mask);
  }
}

int can0Cnt = 0, can1Cnt = 0;

int ret0 = 0, ret1 = 0;

void loop(void) {
  static int fl = 0;
  if (tCan0 <= millis()) {
    tCan0 += tCan0Per;
    for (int i = 1; i < 8; i++)  // to avoid bit stuffing (for oscilloscope)
      msg0.buf[i] = 0xaa;
    msg0.buf[0]++;
    msg0.len = 8;
    Serial.print("Can0 ");
    Serial.println(msg0.buf[0]);
    Can0.write(msg0);  // MISSING returns 1 if msg is delivered to CAN controller. 0 if not !!!
  }


  if (Can1.available()) {
    msg1.timeout = 1;
    ret1 = Can1.read(msg1);  // did you receive anything ?returns 1 (ok) and 0 if no sucess
    if (ret1 == 1) {         // got a pkg

      Serial.print("can1 ");
      Serial.println(msg1.buf[0]);
    } else {
      Serial.println("Can1 : available but no data ?");
    }
  }
  if (tFlt <= millis()) {
    tFlt += tFltPer;
    if (fl) {
      fl = 0;
      setFilter(1, 0, 0); //filter iface 1 (Can1) , 0/1 off/on, filter id
    } else {
      fl = 1;
      setFilter(1, 1, 10);
    }
  }
}
