#include <SPI.h>
#include "mcp_can.h"
#include <MFRC522.h>

#define DEBUG true

#define RST_CAN A3
#define CS_CAN A4
#define IRQ_CAN 8
#define CS_RFID 12
#define LED_BUTTOM 3
#define BUZZER 5
#define BUTTOM 7
#define LAMP 6
#define STATUS_LED 13
#define RST_RFID 11

MCP_CAN CAN(CS_CAN); // Set CS pin
MFRC522 mfrc522(CS_RFID, RST_RFID);   // Crear instancia del MFRC522

unsigned char flagRecv = 0;

void setup() {
    // initialize the digital pin as an output.
  pinMode(LED_BUTTOM, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(LAMP, OUTPUT);
  pinMode(STATUS_LED, OUTPUT);
  pinMode(BUTTOM, INPUT);
  pinMode(RST_CAN, OUTPUT);
  digitalWrite(BUTTOM, HIGH);
  digitalWrite(LED_BUTTOM, HIGH);
  digitalWrite(BUZZER, LOW);
  digitalWrite(LAMP, LOW);
  digitalWrite(STATUS_LED, LOW);
  digitalWrite(RST_CAN, HIGH);
  Serial.begin(115200);
  delay(4000);
  while (CAN_OK != CAN.begin(CAN_500KBPS))              // init can bus : baudrate = 500k
    {
        Serial.println("CAN init failed, retry");
        delay(100);
    }
  attachInterrupt(IRQ_CAN, MCP2515_ISR, FALLING); // start interrupt
  Serial.println("CAN init ok");
  mfrc522.PCD_Init();     //Función  que inicializa RFID
  Serial.println("RFID init ok");
//  Wire.setClock(400000);
}


unsigned char len = 0;
unsigned char buf[8];
    
void loop() {
  read_buttom();
//  if (!auto_mode) pot();
  if (mfrc522.PICC_IsNewCardPresent())
   {
      if (mfrc522.PICC_ReadCardSerial())
      {
         Serial.print(F("Card UID:"));
         printArray(mfrc522.uid.uidByte, mfrc522.uid.size);
         Serial.println();
 
         // Finalizar lectura actual
         mfrc522.PICC_HaltA();
      }
   }
  if (flagRecv)
    {
      if(CAN_MSGAVAIL == CAN.checkReceive())            // check if data coming
    {
        CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

        unsigned long canId = CAN.getCanId();
        
        Serial.println("-----------------------------");
        Serial.print("Get data from ID: 0x");
        Serial.println(canId, HEX);

        for(int i = 0; i<len; i++)    // print the data
        {
            Serial.print(buf[i], HEX);
            Serial.print("\t");
        }
        Serial.println();
    }
    }
}

#define VCC 5000
#define POT_OUT 10000
//#define POT_IN 10000

void pot() {
//  float pot_in = ads.readADC_SingleEnded(1)*0.1875;
//  float sup_in = ads.readADC_SingleEnded(3)*0.1875;
//  float sup_out = ads.readADC_SingleEnded(2)*0.1875;
//  //float sup_out = VCC;
//  float pot_in_correct = (sup_out/sup_in)*pot_in;
//  #if DEBUG
//    Serial.print("Voltage input: ");
//    Serial.print(pot_in);
//    Serial.print(" mV, ");
//    Serial.print("Supply input: ");
//    Serial.print(sup_in);
//    Serial.print(" mV, ");
//    Serial.print("Voltage input corrected: ");
//    Serial.print(pot_in_correct);
//    Serial.print(" mV, ");
//  #endif
//  voltage(pot_in_correct);
}

void voltage(float volt)
  {  
//    float sup_out = ads.readADC_SingleEnded(2)*0.1875;
//    #if DEBUG
//      Serial.print("Supply out: ");
//      Serial.print(sup_out);
//      Serial.print(" mV, "); 
//    #endif
//    float resistor = (volt*POT_OUT)/(sup_out);
//    uint8_t value = uint8_t ((resistor*128)/POT_OUT);
//    ds3502.setWiper(value);
//    float pot_out = ads.readADC_SingleEnded(0)*0.1875;
//    #if DEBUG
//      Serial.print("Teorical Voltage output: ");
//      Serial.print(value*sup_out/127);
//      Serial.print(" mV ");
//      Serial.print("Voltage output: ");
//      Serial.print(pot_out);
//      Serial.println(" mV");
//    #endif
  }

void printArray(byte *buffer, byte bufferSize) {
   for (byte i = 0; i < bufferSize; i++) {
      Serial.print(buffer[i] < 0x10 ? " 0" : " ");
      Serial.print(buffer[i], HEX);
   }
}

unsigned char stmp[8] = {1, 0, 0, 0, 0, 0, 0, 0};

void read_buttom()
  {
    if (!digitalRead(BUTTOM))
      {
//        Serial.println("ON");
        digitalWrite(LED_BUTTOM, LOW);
        CAN.sendMsgBuf(0x00, 0, 8, stmp);
      }
    else
      {
//        Serial.println("OFF");
        digitalWrite(LED_BUTTOM, HIGH);
      }
  }

void MCP2515_ISR()
{
    flagRecv = 1;
}