/*
  ----------------------------------------------------------------------------------
 PINBALL_CAN
 ----------------------------------------------------------------------------------
 Developped to work with CAN Library by IGOR REAL (16 - 05 - 2011)
 This method can be used to control any number of CAN devices based in Arduino Board
 
 Created 10 ago 2012
 By Alex Posada / M.A. de Heras
 http://www.mediainteractivedesign.com
 ----------------------------------------------------------------------------------
 
 ID = 0  ->  MASTER
 ID > 0  ->  SLAVE
 
 ----------------------------------------------------------------------------------
 
 */


#include "CAN_.h"

#define    led_tx         9
#define    res            1
#define    analogPullup   1

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 6

int sensor[MESSAGE_SIZE + 1] = {
  0,0,0,0,0,255}; // id + 4 datos + BYTE sincro

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]) || (s[1] != s_old[1]) || (s[2] != s_old[2]) || (s[3] != s_old[3]) )
  {
    return(true);
  }
  else return(false);
}

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

  average = total / lecturas;  
  if(average < 1000) // limitacion a 1000 en vez de 1024, para respetar bytes de sinc y posibles comandos
    return(average);
  else
    return(1000);

}

void sensorUpdate()
{
  for(int i=0 ; i<4; i++) s[i] = (byte)(average(sPin[i])/4);  // Lectura de valores
  //for(int i=0 ; i<1; i++) s[i] = (byte)(average(sPin[i])/4);  // Lectura de valores - Limitacion a un sensor
  if (check_change()) // Chequeo de un cambio en un sensor
  { 
    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);
  //pinMode(10,OUTPUT);
  pinMode(led_tx,OUTPUT);

  CAN.begin(250);

  for(int i=0 ; i<4; i++)
  {
    pinMode(id_pin[i],INPUT); // Pull-Up entradas digitales switch ID
    digitalWrite(id_pin[i],HIGH);
  }

  if(analogPullup)
  {
    for(int i=0 ; i<4; i++)  // Pull-UP entradas analogicas de sensores
    {
      pinMode(sPin[i],INPUT);
      digitalWrite(sPin[i], HIGH);
    }
  }

  check_id();

  if (test)
  { 
    Serial.print("ID = ");
    Serial.println(id_can);
  }
  digitalWrite(led_tx,HIGH);
}


void loop()
{

  check_id();  // chequeo del switch id del dispositivo

  if (id_can == 0) canUpdateRx(); // recibo datos si soy el id=0 -> Master

  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) // verifico si hay un cambio en los sensores de la propia placa (Master)
  { 
    can_tx = false;

    digitalWrite(led_tx,LOW);

    sensor[0] = id_can;
    sensor[1] = s[0];
    sensor[2] = s[1];
    sensor[3] = s[2];
    sensor[4] = s[3];

    if (!test)
    {
      for(int b=0 ; b < MESSAGE_SIZE; b++)  
        Serial.write((unsigned char)sensor[b]); 
    }
    else 
    {
      Serial.print("Id: ");
      Serial.print(sensor[0]);
      for(int b=1 ; b < MESSAGE_SIZE-1; b++)  
      {
        Serial.print(" ");
        Serial.print((unsigned char)sensor[b]);
      }

    }

    digitalWrite(led_tx,HIGH);

  }

  if (CAN.CheckNew()) // verifico si hay un cambio en los sensores de uno de los esclavos
  {

    digitalWrite(led_tx,LOW);

    CAN.ReadFromDevice(&CAN_RxMsg);

    sensor[0] = CAN_RxMsg.id;
    sensor[1] = CAN_RxMsg.data[0];
    sensor[2] = CAN_RxMsg.data[1];
    sensor[3] = CAN_RxMsg.data[2];
    sensor[4] = CAN_RxMsg.data[3];

    if (!test)
    {
      for(int b=0 ; b < MESSAGE_SIZE; b++)  
        Serial.write((unsigned char)sensor[b]); 
    }
    else 
    {
      Serial.print("Id: ");
      Serial.print(sensor[0]);
      for(int b=1 ; b < MESSAGE_SIZE-1; b++)  
      {
        Serial.print(" ");
        Serial.print((unsigned char)sensor[b]);
      }
    }

    digitalWrite(led_tx,HIGH);

  }
}