#include <Arduino.h>

int  i;
unsigned int x, y;
float accum, fout, fval = 0.001;    
long threshold = 0;

void setup() {
  Serial.begin(9600);
  DDRB = 0b00100101;
  PORTB = 0b00000000;

  
  for (short i = 0; i < 10; i++) {
    long max;
    y = 0;
    x = 0;
    for (short i = 0; i < 4; i++) {
      
      PORTB |= 0b00000001; 
      while ((PINB & 0b00000010) != 0b00000010 ) { 
        x++;
      }
      delay(1);

      
      PORTB &= 0b11111110;      
      while((PINB & 0b00000010) != 0b00000000 ) { 
        y++;  
      }
      delay(1);
    }
    max = (x > y ? x : y);
    threshold = (threshold > max ? threshold : max);
  }

  Serial.print("Threshold: ");
  Serial.println(threshold);
  Serial.print('\n');
}

void loop() {
  y = 0;
  x = 0;

  for (short i = 0; i < 4; i++) {
    
    PORTB |= 0b00000001; 
    while ((PINB & 0b00000010) != 0b00000010 ) { 
      x++;
    }
    delay(1);
    
    
    PORTB &= 0b11111110;       
    while((PINB & 0b00000010) != 0b00000000 ) { 
      y++;  
    }
    delay(1);
  
  fout =  (fval * (float)x) + ((1-fval) * accum);  
  }

  
  if (x > threshold && y > threshold) {
    PORTB |= 0b00100000;    
    Serial.println("Touch");    
  } else {
    PORTB &= 0b11011111;
    Serial.println("Doesn't Touch");    
  } 
  Serial.print("\nCarica: ");
  Serial.print(x);
  Serial.print(" / Sarica: ");
  Serial.print(y);
  Serial.println();
  delay(100);
}