int trigPin1=13;
int echoPin1=12;
int trigPin2=8;
int echoPin2=7;
float R1=0.03,R2=0.08;
unsigned long t1,t2;
float d,yk,yk_1,pk,pk_1,k,yk_cap,yk_cap2,pk_cap1,pk_cap2,time1,time2,distance1,distance_1,distance2,distance_2;

void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
Serial.println("Start");
t1 = millis();                // start time
pinMode(trigPin1,OUTPUT);
pinMode(echoPin1,INPUT);

pinMode(trigPin2,OUTPUT);
pinMode(echoPin2,INPUT);

//sensor 1 initial distance
digitalWrite(trigPin1,LOW);
delayMicroseconds(2);
digitalWrite(trigPin1,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1,LOW);
time1=pulseIn(echoPin1,HIGH,2000);
distance1=(time1/2)*0.03435;

if(distance1>15 && distance1<65){
  yk_1=0.0175*time1 + 0.4703;
}
else if(distance1>=65 && distance1<105){
  yk_1=0.0173*time1-0.289;
}
else if(distance1>=105 && distance1<160){
  yk_1=0.0172*time1 + 0.1809;
}
else{
  yk_1=90;      //initial value outside of valid range
}

pk_1=R1;
}



double cali_1(double distance_1,double time1){
  if(distance_1>15 && distance_1<65){
    d=0.0175*time1 + 0.4703;
  }
  else if(distance_1>=65 && distance_1<105){
    d=0.0173*time1-0.289;
  }
  else if(distance_1>=105 && distance_1<155){
    d=0.0172*time1 + 0.1809;
  }
  return(d);
}

double cali_2(double distance_2,double time2){
  if(distance_2>15 && distance_2<65){
    d=0.0175*time2 + 0.4703;
  }
  else if(distance_2>=65 && distance_2<105){
    d=0.0173*time2-0.289;
  }
  else if(distance_2>=105 && distance_2<155){
    d=0.0172*time2 + 0.1809;
  }
  return(d);
}
  

void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(trigPin1,LOW);
delayMicroseconds(2);
digitalWrite(trigPin1,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1,LOW);
time1=pulseIn(echoPin1,HIGH,2000);
distance1=(time1/2)*0.03435;

digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
time2 = pulseIn(echoPin2, HIGH, 2000);
distance2=(time2/2)*0.03435;



//filter(Kalman)

//prediction
yk=yk_1;
pk=pk_1;


//correction 1
k=pk/(pk+R1);
yk_cap=yk+k*(cali_1(distance1,time1)-yk);
pk_cap1=(1-k)*pk;

//correction 2
k=pk_1/(pk_1+R2);
yk_cap2=yk_cap+k*(cali_2(distance2,time2)-yk_cap);
pk_cap2=(1-k)*pk_cap1;

//update
yk_1=yk_cap2;
pk_1=pk_cap2;

// Serial.print(yk_1);
// Serial.print(", ");
// Serial.print(pk_1);
// Serial.println(", ");

t2 = millis();
//if(pk_1<0.1){
//  Serial.print(t2-t1);
//  exit(0);
//}
delay(100);

}