#include "can_bus.h"

// https://docs.cirkitdesigner.com/component/a4fea22c-a62d-453b-8645-1c8efe324cbc/mcp2515
// https://learn.adafruit.com/adafruit-picowbell-can-bus-for-pico/overview
// Pico Lib: https://github.com/adamczykpiotr/pico-mcp2515
// Arduino Lib: https://github.com/autowp/arduino-mcp2515
// MCP2515 is the cheap aliexpress boards I have

// spi_inst_t* CHANNEL = spi0;
// uint32_t SPI_CLOCK = DEFAULT_SPI_CLOCK
//
//

// 10000000 //10Mhz
#define MCP_SPI_CLOCK 5000000 // 5Mhz
#define MCP_INSTRUCTION_RESET 0xC0
#define MCP_INSTRUCTION_WRITE 0x02

void mcp2515_init(mcp2515_config *config, uint8_t cs_pin, uint8_t tx_pin,
                  uint8_t rx_pin, uint8_t sck_pin, spi_inst_t *spi_inst) {
  config->CHANNEL = spi_inst;
  config->CS_PIN = cs_pin;

  spi_init(config->CHANNEL, MCP_SPI_CLOCK);
  gpio_set_function(rx_pin, GPIO_FUNC_SPI);
  gpio_set_function(sck_pin, GPIO_FUNC_SPI);
  gpio_set_function(tx_pin, GPIO_FUNC_SPI);
  spi_set_format(config->CHANNEL, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);

  // Chip select is active-low, so we'll initialise it to a driven-high state
  gpio_init(config->CS_PIN);
  gpio_set_dir(config->CS_PIN, GPIO_OUT);
  // gpio_put(cs_pin, 0);
}

static void mcp2515_start_spi(mcp2515_config *config) {
  asm volatile("nop \n nop \n nop");
  gpio_put(config->CS_PIN, 0);
  asm volatile("nop \n nop \n nop");
}

static void mcp2515_end_spi(mcp2515_config *config) {
  asm volatile("nop \n nop \n nop");
  gpio_put(config->CS_PIN, 1);
  asm volatile("nop \n nop \n nop");
}

static void mcp2515_set_registers(mcp2515_config *config, const uint8_t reg,
                                  const uint8_t values[], const uint8_t n) {
  mcp2515_start_spi(config);
  uint8_t data[2] = {MCP_INSTRUCTION_WRITE, reg};
  spi_write_blocking(config->CHANNEL, data, 2);
  spi_write_blocking(config->CHANNEL, values, n);
  mcp2515_end_spi(config);
}

enum MCP_ERROR mcp2515_reset(mcp2515_config *config) {
  mcp2515_start_spi(config);
  uint8_t instruction = MCP_INSTRUCTION_RESET;
  spi_write_blocking(config->CHANNEL, &instruction, 1);
  mcp2515_end_spi(config);
  sleep_ms(10);

  uint8_t zeros[14];
  memset(zeros, 0, sizeof(zeros));
  // setRegisters(MCP_TXB0CTRL, zeros, 14);
  // setRegisters(MCP_TXB1CTRL, zeros, 14);
  // setRegisters(MCP_TXB2CTRL, zeros, 14);

  // setRegister(MCP_RXB0CTRL, 0);
  // setRegister(MCP_RXB1CTRL, 0);

  // setRegister(MCP_CANINTE,
  //             CANINTF_RX0IF | CANINTF_RX1IF | CANINTF_ERRIF | CANINTF_MERRF);

  // receives all valid messages using either Standard or Extended Identifiers
  // that meet filter criteria. RXF0 is applied for RXB0, RXF1 is applied for
  // RXB1
  // modifyRegister(MCP_RXB0CTRL,
  //                RXBnCTRL_RXM_MASK | RXB0CTRL_BUKT | RXB0CTRL_FILHIT_MASK,
  //                RXBnCTRL_RXM_STDEXT | RXB0CTRL_BUKT | RXB0CTRL_FILHIT);
  // modifyRegister(MCP_RXB1CTRL, RXBnCTRL_RXM_MASK | RXB1CTRL_FILHIT_MASK,
  //                RXBnCTRL_RXM_STDEXT | RXB1CTRL_FILHIT);

  // clear filters and masks
  // do not filter any standard frames for RXF0 used by RXB0
  // do not filter any extended frames for RXF1 used by RXB1
  // RXF filters[] = {RXF0, RXF1, RXF2, RXF3, RXF4, RXF5};
  // for (int i = 0; i < 6; i++) {
  //   bool ext = (i == 1);
  //   ERROR result = setFilter(filters[i], ext, 0);
  //   if (result != ERROR_OK) {
  //     return result;
  //   }
  // }

  // MASK masks[] = {MASK0, MASK1};
  // for (int i = 0; i < 2; i++) {
  //   ERROR result = setFilterMask(masks[i], true, 0);
  //   if (result != ERROR_OK) {
  //     return result;
  //   }
  // }

  return ERROR_OK;
}

// mcp2515.setBitrate(CAN_125KBPS);
// mcp2515.setLoopbackMode();