#include <Wire.h>

int i_min = 0;
int i_max = 4;
int j_min = 0;
int j_max = 4;
int nloop = 100;
int count, i, j;
int up[4][4],down[4][4],value[4][4];
int x;
int tempo_gain = 0;
double gain[4][4];
double eps = 0.1;
double filt[4][4];
String str_filt ="";
volatile int16_t capacitive_values[16];

void setup() {
  for (i = i_min; i<i_max; i++){
    for (j = j_min; j<j_max; j++){
      gain[i][j] = 1.000;
      filt[i][j] = 0.000;
    }
  }
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  Wire.begin(1); //Replace this number by the number of the board you're uploadng
  Wire.onRequest(requestEvent);
  Serial.begin(9600);
}

void loop() {
  tempo_gain ++;
  if (tempo_gain == 80) {
    for (i = i_min; i<i_max; i++){
      for (j = j_min; j<j_max; j++){
        gain[i][j] = 7500/filt[i][j];
      }
    }
  }
  for (i = i_min; i<i_max; i++){
    for (j = j_min; j<j_max; j++){
      up[i][j] = 0;
      down[i][j] = 0;
      for (count = 0; count < nloop; ++count){
        delayMicroseconds(0.1);
        digitalWrite(j+3, HIGH);
        x = analogRead(i);
        up[i][j] += x;
        delayMicroseconds(0.1);
        digitalWrite(j+3, LOW);
        x = analogRead(i);
        down[i][j] += x;
        }
      value[i][j] = up[i][j] - down[i][j];
      filt[i][j] = (1-eps)*filt[i][j] + eps*value[i][j];
    }
  }
  str_filt="";
  for (j = j_min; j<j_max; j++){
    for (i = i_min; i<i_max; i++){
      str_filt = str_filt + String(filt[i][j]) + ",";
    }
  }
  for (j = j_min; j<j_max; j++){
    for (i = i_min; i<i_max; i++){
      capacitive_values[4*j+i]=(int)filt[i][j];
    }
  }
  for (int i = 0; i<16; i++){
    Serial.print(capacitive_values[i]);
    Serial.print(";");
  }
  Serial.print("\n");
}

void requestEvent(){
  byte sent_list[16][2];
  for (int i = 0; i<16; i++){
    sent_list[i][0] = (capacitive_values[i] >> 8) & 0xFF;
    sent_list[i][1] = capacitive_values[i] & 0xFF;
    Wire.write(sent_list[i], 2);
  }
}