byte D_byte[2] = {0,0}; //Byte for Digital Read

void setup()
{
  Serial.begin(115200);
/* 
  // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
  if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK) Serial.println("MCP2515 Initialized Successfully!");
  else Serial.println("Error Initializing MCP2515...");

  CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
 */

  //PIN Init
  pinMode(A0, INPUT);
  //pinMode(1, INPUT); //this crate some problems 
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(6, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);

  // --- fino a qui ok --

  //pinMode(9, INPUT);
  //pinMode(10, INPUT);
  //pinMode(11, INPUT);
  //pinMode(12, INPUT);
  //pinMode(13, INPUT);

  // Problema non posso leggere questi pin Digitali
}

//byte data[8] = {0x00, 0x02, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
//int data2 = 0x02;


// Function to converto from a Bool array to a byte format 
void Bool_to_Byte(bool Dig[], int Dig_dim)
{
  
  int i = Dig_dim-1;
  int j = 0;
  if (Dig_dim%8 == 0)
  {
      j = (Dig_dim/8) -1;
  }
  else
  {
    j = Dig_dim/8;
  }
  //Serial.print("\n il numero di bit necessari ->");
  //Serial.print(j+1);
  //Serial.println();


  int k = 0;

  while(j>-1)
  {
    if(Dig[i])
    {
      //Serial.print(k);
      //Serial.print(".");
      //Serial.print("[");
      D_byte[j] |= (1 << k);
      //Serial.print(D_byte[j],BIN);
      //Serial.print("]");
    }
    
    if(i==7 || i <= 0)
    {
      //Serial.print("|-|");
      j--;
      k = 0;
    }
    else 
    {
      k++;
    }
    i--;
  }

    /* Serial.println();
    Serial.print(D_byte[0],BIN);
    Serial.print("-->");
    Serial.println(D_byte[0],DEC);
    Serial.println("-----------------");
    Serial.print(D_byte[1],BIN);
    Serial.print("-->");
    Serial.println(D_byte[1],DEC);
 */
}
/* 
bool DigRead(bool* Dig, int N_pins)
{
  // the max number that N_pins can be is 13
  if(N_pins>13)
  {
    Serial.print("\n\n -------------ERROR NUMBER OF PIN TO READ IS > 13");
    return 0; 
  }
  for (int i = 0; i<N_pins; i++)
  {
    Dig[i] = digitalRead(i);
    Serial.print("[");
    Serial.print(Dig[i],BIN);
    Serial.print("]");
  }
  return 1;
} */

void loop()
{

  bool Dig[7] = {0,0,1,0,1,0,0};
  // Dig[0] = PIN_2
  //...
  //Dig[7] = PIN_8

  for (int i = 0; i<(sizeof(Dig)); i++)
  {
    Dig[i] = digitalRead(i+2);
    Serial.print("[");
    Serial.print(Dig[i],BIN);
    Serial.print("]");
  }
  
  //are the values of the Digital PINS -> {PIN 13, PIN 12, PIN 11, PIN 10, PIN 9, PIN 8, PIN 7, PIN 6, PIN 5, PIN 4, PIN 3}
  //the last 2 pins are not used because arduino don't work properly with that 

  Serial.println("\n\nil numero da convertire : ");
  for (int i = 0; i<sizeof(Dig); i++)
  {
    Serial.print("[");
    Serial.print(Dig[i]);
    Serial.print("]");
  }
  //Serial.println("\n---------------\n");
  //Serial.print("lunghezza vettore: ");
  //Serial.println(sizeof(Dig));

  Bool_to_Byte(Dig, sizeof(Dig));  // Convertion from Bool to Byte type

  int Pot;
  Pot = analogRead(A0);
  D_byte[1] = highByte(Pot);
  D_byte[2] = lowByte(Pot); 
  Serial.print("\n analog read \n");
  Serial.print(D_byte[1],BIN);
  Serial.print("||");
  Serial.print(D_byte[2],BIN);
  Serial.print(" ---> ");
  Serial.print(Pot,DEC);

  delay(2000);
  
/* 
  //-----------SEND DATA------------------|
  // send data:  ID = 0x302, Standard CAN Frame, Data length = 2 bytes, 'data' = array of data bytes to send
  byte sndStat = CAN0.sendMsgBuf(0x302, 0, 2, D_byte);
  if(sndStat == CAN_OK){
    Serial.println("Message Sent Successfully!");
  } else {
    Serial.println("Error Sending Message...");
  }
  delay(100);   // send data per 100ms
  */


Serial.print("\n\nMessage to be send ->");
for(int i = 0; i<3;  i++)
{
  Serial.print("[");
  Serial.print(D_byte[i],BIN);
  Serial.print("]");
}

// this are needed in order to reset the byte once we have trnsmitted the message
Serial.print("\n");

D_byte[0] = 0;
D_byte[1] = 0;
D_byte[2] = 0; 
}