#include <Arduino.h>
#include "driver/twai.h"
#include "MeyCan.h"

// https://github.com/espressif/arduino-esp32/blob/master/libraries/ESP32/examples/TWAI/TWAIreceive/TWAIreceive.ino

#define RX_PIN 2
#define TX_PIN 3

void DebugBlink(int d) {
  pinMode(20, OUTPUT);
  while (true) {

    digitalWrite(20, HIGH);
    delay(d);
    digitalWrite(20, LOW);
    delay(d);
  }
}

bool driver_installed = false;
void setup() {
  Serial.begin(9600);
  SPI.begin();

  // Explicit GND for LED and Input
  pinMode(21, OUTPUT);
  digitalWrite(21, LOW);

  // Initialize configuration structures using macro initializers
  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TX_PIN, (gpio_num_t)RX_PIN, TWAI_MODE_NORMAL);
  twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();  //Look in the api-reference for other speed sets.
  twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();


  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
    Serial.println("Driver installed");
  } else {
    DebugBlink(100);
    return;
  }



  esp_err_t e = twai_start();
  // Start TWAI driver
  if (e == ESP_OK) {
    driver_installed = true;
  } else {
    DebugBlink(500);
    return;
  }


  SetDevicedId(0x05, 0x1F);

  SetMeyPin(1, 5);
  SetMeyPin(2, 6);
  SetMeyPin(3, 7);
  SetMeyPin(4, 8);
  SetMeyPin(5, 9);
  SetMeyPin(6, 10);
  SetMeyPin(7, 20);

  SetupMeyCan(8, 1, 3);
}

twai_message_t frame;
void loop() {

  if (!driver_installed) {
    // Driver not installed
    DebugBlink(2000);
    return;
  }

  CheckMeyPinsTriggered();
  if (twai_receive(&frame, 0) == ESP_OK) {

    HandleFrame(&frame);
  }
}