#include "CAN_.h"

#define led_tx 9
#define res 4

byte id_can;
byte master_id;
int id_pin[4] = {
  8,7,6,5};

// Analog inputs

byte sPin[4] = {
  A0,A1,A2,A3};
int s[4] = {
  0,0,0,0};

int s_old[5] = {
  0,0,0,0};

boolean can_tx = false;
long previousMillis = 0;

boolean test= false;

#define MESSAGE_SIZE 16

char sensor[MESSAGE_SIZE] = { // Message template
  '/', 'p', 'b', '/',   // SENSOR MESSAGE TEMPLATE
  's' , '1' , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};


void check_id()
{
  id_can = 0x00;
  for(byte i=0 ; i<4; i++)
  {
    if (!digitalRead(id_pin[i])) id_can = id_can + 0x01;
    if (i<3) id_can = id_can << 1;
  }
}

boolean check_change()
{
  if (((s[0]>=(s_old[0] + res))||((s[0] + res) <= s_old[0]))||((s[1]>=(s_old[1] + res))||((s[1] + res) <= s_old[1]))||((s[2]>=(s_old[2] + res))||((s[2] + res) <= s_old[2]))||((s[3]>=(s_old[3] + res))||((s[3] + res) <= s_old[3]))) 
  {
    return(true);
  }
  else return(false);
}

int average(int anaPin)
{
  long total = 0;
  long average = 0;
  int count = 0;
  int lecturas = 10;
  for(int i=0; i<lecturas; i++)
  {
    total = total + analogRead(anaPin);
  }

  average = total / lecturas;  
  return(average);

}

void sensorUpdate()
{
  //for(int i=0 ; i<1; i++) s[i] = (byte)(average(sPin[i])/4);  // limitacion a un sensor
  for(int i=0 ; i<1; i++) s[i] = (byte)(average(sPin[i])/4);  // limitacion a un sensor
  if (check_change())
    { 
      can_tx= true; 
      s_old[0]=s[0]; 
      s_old[1]=s[1]; 
      s_old[2]=s[2]; 
      s_old[3]=s[3]; 
      if (id_can>0) canUpdateTx();
    }
}


void setup()
{
  Serial.begin(115200);
  Serial.println("Starting...");
  //pinMode(10,OUTPUT);
  pinMode(led_tx,OUTPUT);

  CAN.begin(250);

  for(int i=0 ; i<4; i++)
  {
    pinMode(id_pin[i],INPUT);
    digitalWrite(id_pin[i],HIGH);
  }
  check_id();
  Serial.print("ID = ");
  Serial.println(id_can);
  digitalWrite(led_tx,HIGH);
}


void loop()
{
  check_id();
  if (id_can == 0) canUpdateRx();

  sensorUpdate();
}

// COMMUNICATION CAN FUNCTION

void canUpdateTx()
{
  digitalWrite(led_tx,LOW);
  CAN_TxMsg.id = id_can;    
  CAN_TxMsg.header.rtr=0;
  CAN_TxMsg.header.length = 4;
  CAN_TxMsg.data[0]=s[0];
  CAN_TxMsg.data[1]=s[1];
  CAN_TxMsg.data[2]=s[2];
  CAN_TxMsg.data[3]=s[3];
  CAN.send(&CAN_TxMsg);
  digitalWrite(led_tx,HIGH);
}

void canUpdateRx()
{
  if (can_tx)
  {
    can_tx = false;
    
    sensor[MESSAGE_SIZE-4] = s[0];
    sensor[MESSAGE_SIZE-3] = s[1];
    sensor[MESSAGE_SIZE-2] = s[2];
    sensor[MESSAGE_SIZE-1] = s[3];
    
    sensor[5] = '0';
    
    if (!test)
    {
      for(int b=0 ; b < MESSAGE_SIZE; b++)  
        Serial.write((unsigned char)sensor[b]); 
    }
    else 
    {
      for(int b=0 ; b < (MESSAGE_SIZE - 4); b++)  
        Serial.write((unsigned char)sensor[b]); 
      for(int b=(MESSAGE_SIZE - 4) ; b < MESSAGE_SIZE; b++)  
        {
          Serial.print(" ");
          Serial.print((unsigned char)sensor[b]);
        }
      Serial.println();  
    }
    delay(5); // delay entre mensajes!
      
  }
  if (CAN.CheckNew())
  {
    CAN.ReadFromDevice(&CAN_RxMsg);
    
    sensor[MESSAGE_SIZE-4] = CAN_RxMsg.data[0];
    sensor[MESSAGE_SIZE-3] = CAN_RxMsg.data[1];
    sensor[MESSAGE_SIZE-2] = CAN_RxMsg.data[2];
    sensor[MESSAGE_SIZE-1] = CAN_RxMsg.data[3];
    
    sensor[5] = CAN_RxMsg.id + '0';
    if (!test)
    {
      for(int b=0 ; b < MESSAGE_SIZE; b++)  
        Serial.write((unsigned char)sensor[b]); 
    }
    else 
    {
      for(int b=0 ; b < (MESSAGE_SIZE - 4); b++)  
        Serial.write((unsigned char)sensor[b]); 
      for(int b=(MESSAGE_SIZE - 4) ; b < MESSAGE_SIZE; b++)  
        {
          Serial.print(" ");
          Serial.print((unsigned char)sensor[b]);
        }
      Serial.println();  
    }
  }
}