#include <CapacitiveSensor.h> //add library for capacitive sensing
#include <SoftwareSerial.h> //add library for serial monitor
int rxPin = 10; //rx is pin is made up 
int txPin = 4; //tx is pin PA4
int receive_pin = 0; //receive pin for sensor CS_top01 (sensor pin)
int send_pin = 1; //send pin for sensor CS_top01
int receive_pin2 = 2; //receive pin for sensor CS_bottom23 (sensor pin)
int send_pin2 = 3; //send pin for sensor CS_bottom23

int delay_time =100; //set delay time

const int LED_1 = 8; // LED on PB2
const int LED_2 = 5; // LED on PA5
const int LED_3 = 6; // LED on PA6

SoftwareSerial serial(rxPin, txPin); //define SoftwareSerial 

CapacitiveSensor   CS_top01 = CapacitiveSensor(send_pin,receive_pin); //define CapacitiveSensor för top sensor
CapacitiveSensor   CS_bottom23 = CapacitiveSensor(send_pin2,receive_pin2); //define CapacitiveSensor for bottom sensor

void setup(){
  pinMode(rxPin, INPUT); //set pin mode för rx to input
  pinMode(txPin, OUTPUT); //set pin mode för tx to output
  serial.begin(9600); //initiate serial

  pinMode(receive_pin, INPUT); //set pin mode for the receive pin to input 
  pinMode(send_pin, OUTPUT); //set pin mode for the send pin to output 

  pinMode(receive_pin2, INPUT); //set pin mode for the receive pin to input 
  pinMode(send_pin2, OUTPUT); //set pin mode for the send pin to output 

  CS_top01.set_CS_AutocaL_Millis(0xFFFFFFFF); //turn off autocalibrate on top sensor
  CS_bottom23.set_CS_AutocaL_Millis(0xFFFFFFFF); // turn off autocalibrate on bottom sensor

   }

void loop(){
  long start1 = millis(); //millis = returns the number of ms since program started 
  long start2 = millis(); //millis = returns the number of ms since program started 
  long total1 =  CS_top01.capacitiveSensor(30); //number of times the sensor measure
  long total2 =  CS_bottom23.capacitiveSensor(30);//number of times the sensor measure

  if(total1 < 8800 && total2 < 6400){ //if both are under

        //turn all LEDs off (making all OUTPUT and LOW)
        //turn off LED 1 
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time);

        //turn off LED L2
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time);
          
        //turn off LED L3 
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time);
        
        //turn off LED L4
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, LOW); 
          
        delay(delay_time);
        
        //turn off LED L5
        pinMode(LED_1, OUTPUT); 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); 
        digitalWrite(LED_3, LOW); 

        delay(delay_time);
        
        //turn off LED L6 
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time); 
        }       
        
 else{
        //blink all LEDs
        //turn on LED 1 
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, HIGH); 
        pinMode(LED_3, INPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time); 

        //turn on LED L2
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, HIGH); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, INPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time); 
          
        //turn on LED L3 
        pinMode(LED_1, INPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, HIGH); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, LOW); 
        
        delay(delay_time);
        
        //turn on LED L4
        pinMode(LED_1, INPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, OUTPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row 3 
        digitalWrite(LED_3, HIGH); 
          
        delay(delay_time);
        
        //turn on LED L5
        pinMode(LED_1, OUTPUT); 
        digitalWrite(LED_1, HIGH); 
        pinMode(LED_2, INPUT); 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); 
        digitalWrite(LED_3, LOW); 

        delay(delay_time);
        
        //turn on LED L6 
        pinMode(LED_1, OUTPUT); //row 1 
        digitalWrite(LED_1, LOW); 
        pinMode(LED_2, INPUT); //row 2 
        digitalWrite(LED_2, LOW); 
        pinMode(LED_3, OUTPUT); //row3 
        digitalWrite(LED_3, HIGH); 
        
        delay(delay_time); 
        }    
        
        
serial.print("Top sensor:");
serial.print(millis() - start1); //check on performance in ms
serial.print("\t"); //tab character for debug window spacing
serial.print(total1); //print output from top sensor
serial.println("\t");

serial.print("Bot sensor:");
serial.print(millis() - start2); //check on performance in ms
serial.print("\t"); //tab character for debug window spacing
serial.print(total2); //print output from bottom sensor
serial.println("\t");
    
delay(100); //delay to limit data to serial port 
}