// demo: CAN-BUS Shield, send data
#include <SPI.h>
#define CAN_2515
// #define CAN_2518FD
#include <stdio.h>  
#include <string.h>  
// Set SPI CS Pin according to your hardware

#if defined(SEEED_WIO_TERMINAL) && defined(CAN_2518FD)
// For Wio Terminal w/ MCP2518FD RPi Hat：
// Channel 0 SPI_CS Pin: BCM 8
// Channel 1 SPI_CS Pin: BCM 7
// Interupt Pin: BCM25
const int SPI_CS_PIN  = BCM8;
const int CAN_INT_PIN = BCM25;
#else

// For Arduino MCP2515 Hat:
// the cs pin of the version after v1.1 is default to D9
// v0.9b and v1.0 is default D10
const int SPI_CS_PIN = 9;
const int CAN_INT_PIN = 2;
#endif


#ifdef CAN_2518FD
#include "mcp2518fd_can.h"
mcp2518fd CAN(SPI_CS_PIN); // Set CS pin
#endif

#ifdef CAN_2515
#include "mcp2515_can.h"
mcp2515_can CAN(SPI_CS_PIN); // Set CS pin
#endif

int speed = 0;
int rpm = 0;
String a,b,c,d; //holding binary nibbles.......
char allHex[4];
unsigned char res[2];

unsigned char msg1[0] = {};
unsigned char msg2[8] = {0x08,0x1B,0x18,0x1B,0x01,0x8A,0x92,0x01};
unsigned char msg3[8] = {0x9C,0xCB,0x40,0x00,0x03,0x00,0x00,0xD7};
unsigned char speedMsg[8] = {0x04,0x44,0x98,0xC6,0xE2,0x00,0x00,0x00};  //mph

unsigned char toHex(String digits) {  
    String hexDigitToBinary[16] = {"0000", "0001", "0010", "0011", "0100", "0101", "0110", "0111", "1000", "1001", "1010", "1011", "1100", "1101", "1110", "1111"}; 
    unsigned char hexDigits[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8','9', 'A', 'B', 'C', 'D', 'E', 'F'};   
  
    int num = digits.toInt();
        for(int i=0; i<16; i++) {
            if(hexDigitToBinary[i].equals(digits)) {
                return hexDigits[i];  
            }  
        }  
    return 'z';
}  

void DecToBin(int dec){
  int nums[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
  //reseting a-d?
  a = "";
  b = "";
  c = "";
  d = "";

  int quo = dec;
  int rem = 0;
  String temp = "";

  //store backwards bin
  while(quo != 0){
    rem = quo%2;
    quo = quo/2;
    temp = temp+String(rem);
    //Serial.print(temp);
  }

  //reverse in array
  for(int i=0; i < temp.length();i++){
    nums[i] = temp.charAt(temp.length()-i-1) - '0';
  }

  for(int i=0;i<temp.length();i++){
    if(i<4){
      d=temp.charAt(i)+d;
    }else if(i<8){
      c=temp.charAt(i)+c;
    }else if(i<12){
      b=temp.charAt(i)+b;
    }else if(i<16){
      a=temp.charAt(i)+a;
    }
  }

  for(int i=temp.length()-1; i<15;i++){
    if(i<3){
      d='0'+d;
    }else if(i<7){
      c='0'+c;
    }else if(i<11){
      b='0'+b;
    }else if(i<15){
      a='0'+a;
    }
  }

}

unsigned char* doTheThing(int decVal){
  DecToBin(decVal);
  allHex[0]=toHex(a);
  allHex[1]=toHex(b);
  allHex[2]=toHex(c);
  allHex[3]=toHex(d);
  hex_decode(allHex,4,res);
  return res;
}

unsigned char* hex_decode(const char *in, size_t len,unsigned char *out)
{
        unsigned int i, t, hn, ln;

        for (t = 0,i = 0; i < len; i+=2,++t) {

                hn = in[i] > '9' ? in[i] - 'A' + 10 : in[i] - '0';
                ln = in[i+1] > '9' ? in[i+1] - 'A' + 10 : in[i+1] - '0';

                out[t] = (hn << 4 ) | ln;
        }

        return out;
}

void setup() {
  Serial.begin(115200);
    SERIAL_PORT_MONITOR.begin(115200);
    pinMode(A4, INPUT); //i don't think I actually need these
    pinMode(A1, INPUT);
    
    while (CAN_OK != CAN.begin(CAN_500KBPS)) {             // init can bus : baudrate = 500k
        SERIAL_PORT_MONITOR.println("CAN init fail, retry...");
        delay(100);
    }
    SERIAL_PORT_MONITOR.println("CAN init ok!");

}

void loop() {

  speed = analogRead(A1)*.2502;
  rpm = analogRead(A0)*7.8201;
  doTheThing(speed);
  speedMsg[6]=res[0];
  speedMsg[7]=res[1];
  
  if(speed>125){ //hoping this doesn't cause probelms... also hoping that middle byte doesn't matter......
    speedMsg[5]='E3';
  }else{
    speedMsg[5]='E2';
  }
  CAN.sendMsgBuf(0x202, 0, 8, speedMsg);
}