#include <LiquidCrystal.h>
LiquidCrystal lcd(43, 45, 47, 49, 51, 53);

volatile unsigned long vrijeme1;
volatile unsigned long vrijeme2;
volatile unsigned long vrijeme3;

long ts; // [us]
long tl; // [us]

float r; //udaljenost H2 - izvor zvuka [m]
//unsigned float d = 0.5; //udaljenost hidrofona [m]
float c = 0.0015; //brzina zvuka pod morem [m/us]
float b = 333; // b = d / c [us]
float fi;
float afi;

void setup()
{
  lcd.begin(16, 2);
  attachInterrupt(0, spremi1, RISING);
  attachInterrupt(1, spremi2, RISING);
  attachInterrupt(4, spremi3, RISING);
  Serial.begin(115200);
}

void loop()
{
  if (vrijeme2 > 0 && vrijeme1 > 0 && vrijeme3 > 0)
  {
    ts = vrijeme2 - vrijeme1;
    tl = vrijeme3 - vrijeme2;
  
  r = (c*(2*b*b-tl*tl-ts*ts))/(2*(tl-ts));  
  lcd.setCursor(0,0);
  lcd.print(r);
  lcd.setCursor(8,0);
  lcd.print("meters");   
  
  fi = ((tl+ts)/(2*b))*(1+((tl-ts)*(tl-ts))/(2*b*b-tl*tl-ts*ts));
  afi = asin(fi);
  afi = afi*(57.296);
  lcd.setCursor(0,1);
  lcd.print(afi);
  lcd.setCursor(8,1);
  lcd.print("degrees");

  Serial.print("r = ");
  Serial.println(r);
  Serial.print("fi = ");
  Serial.println(afi);
  
  ts=0;
  tl=0;
  vrijeme1=0;
  vrijeme2=0;
  vrijeme3=0;
  }
}

void spremi1()
{
  vrijeme1 = micros(); 
}

void spremi2()
{
  vrijeme2 = micros();
}

void spremi3()
{
  vrijeme3 = micros();
}
