int trig_outside = 10;
int trig_inside = 13;
int echo_outside = 11;
int echo_inside = 12;

#define hight 300
#define lowt 60

#define decrease 2
#define increase 3


float distance_o;
float distance_i;
float distance_o_prev;
float distance_i_prev;
int output_sensor, input_sensor, outk=1, ink=1, generalk = 1;
int firstthree = 1, counter = 0, differ = 0;
int nofpeople = 0;
unsigned int state, state_prev = 1, total_state = 0;


void trigger(int trig) {
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
}

float schmidt(float a){
  
  float distance;
  if(a > hight) distance = 400;
  else if(a < lowt) distance = 0;
  return distance;
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(trig_outside, OUTPUT);
  pinMode(trig_inside, OUTPUT);
  pinMode(echo_outside, INPUT);
  pinMode(echo_inside, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  trigger(trig_outside);
  float distance_out = pulseIn(echo_outside, HIGH) / 58.0;
  trigger(trig_inside);
  float distance_in = pulseIn(echo_inside, HIGH) / 58.0;

  float distance_o = schmidt(distance_out);
  float distance_i = schmidt(distance_in);


/*  if(!firstthree){
  if(distance_o_prev == 400 && distance_o == 0) {
    output_sensor += outk*decrease;
    outk *= 10;
    }
  else if(distance_o_prev == 0 && distance_o == 400) {
    output_sensor += outk*increase;
    outk *= 10;
    }
  if(distance_i_prev == 400 && distance_i == 0) {
    input_sensor += ink*decrease;
    ink *= 10;
    }
  else if(distance_i_prev == 0 && distance_i == 400) {
    input_sensor += ink*increase;
    ink *= 10;
    }
  } */

////distance_o_prev distance_o distance_i_prev distance_i




    if(distance_o > distance_i) state = 3;
    else if(distance_o < distance_i) state = 2;
    else if(distance_o == 400) state = 1;
    else state = 0;

    if(state_prev != state) {
      total_state *= 10;
      total_state += state;
    }

  


  Serial.print("distance_out_schmidt: ");
  Serial.print(distance_o);
  Serial.print(" distance_in_scmidt: ");
  Serial.print(distance_i);
  Serial.print("   State: ");
  Serial.println(total_state);
/*  Serial.print(" -50 450------");
  Serial.print(output_sensor);
  Serial.print(" ");
  Serial.println(input_sensor);
*/

/*
  if(input_sensor == 32 && output_sensor == 32){
    Serial.println("Someone went out");
    nofpeople--;
    input_sensor = 0;
    output_sensor = 0;
    outk = 1;
    ink = 1;
  }

  else if(input_sensor == 23 && output_sensor == 23){
    Serial.println("Someone came in");
    nofpeople++;
    input_sensor = 0;
    output_sensor = 0;
    outk = 1;
    ink = 1;
  }
*/

if(total_state == 3021 || total_state == 3121){
    Serial.println("Someone went out");
    nofpeople--;
    total_state = 0;
    generalk = 1;
    differ = 1;
  }

  else if(total_state == 2031 || total_state == 2131){
    Serial.println("Someone came in");
    nofpeople++;
    total_state = 0;
    generalk = 1;
    differ = 1;
  }

  if(total_state > 10000){
    total_state %= 10000;
  /*  total_state /= 10;
    total_state *= 10; */
    generalk = 10;
  }
  if(differ){
    Serial.print("Number of people inside: ");
    Serial.println(nofpeople); 
    differ = 0;
  }

  distance_o_prev = distance_o;
  distance_i_prev = distance_i;
  state_prev = state;


  if(firstthree == 1 ) {
    counter++;
    if(counter == 3) firstthree = 0;
  }
  delay(100);
}