#include <Adafruit_DS3502.h>
#include <Adafruit_ADS1015.h>
#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 9
#define LED_BUTTOM 3
#define BUZZER 5
#define BUTTOM 7
#define LAMP 6
#define STATUS_LED 13
#define RST_RFID 11

Adafruit_DS3502 ds3502 = Adafruit_DS3502();
Adafruit_ADS1115 ads;    // Construct an ads1015 at the default address: 0x48

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

uint8_t pot_value = 63;
bool auto_mode = false;
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");
  Serial.println("Adafruit DS3502 Test");
  if (!ds3502.begin()) {
    Serial.println("Couldn't find DS3502 chip");
    while (1);
  }
  // ads.setGain(GAIN_TWOTHIRDS);  // 2/3x gain +/- 6.144V  1 bit = 3mV      0.1875mV (default)
  // ads.setGain(GAIN_ONE);        // 1x gain   +/- 4.096V  1 bit = 2mV      0.125mV
  // ads.setGain(GAIN_TWO);        // 2x gain   +/- 2.048V  1 bit = 1mV      0.0625mV
  // ads.setGain(GAIN_FOUR);       // 4x gain   +/- 1.024V  1 bit = 0.5mV    0.03125mV
  // ads.setGain(GAIN_EIGHT);      // 8x gain   +/- 0.512V  1 bit = 0.25mV   0.015625mV
  // ads.setGain(GAIN_SIXTEEN);    // 16x gain  +/- 0.256V  1 bit = 0.125mV  0.0078125mV
  ads.begin();
  Wire.setClock(400000);
  ds3502.setWiper(pot_value);
}


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;
}