Measuring Time of Flight with Arduino Uno - time

Question

I am trying to implement a master/slave setup to determine the time of flight between two Arduino Uno boards and ultimately use that as a measure of distance between the two. Using the standard 16MHz crystal and the APC220 from DFRobot communicating between the two is easy but getting a time of flight reading is where I get stuck.
I use the following code on the master side to send the first signal and receive the echo from the slave:
// set pins:
const int switchPin = 3; // pin number of the switch
// variables:
int switchState = 0; // variable for reading switch status
int iDisplay = 1;
unsigned long start, finished, elapsed;
// initialize
void setup()
{
// initialize switch pin as input:
pinMode(switchPin, INPUT);
// initialize serial wireless communication:
Serial.begin(9600);
// read initial state of switch
switchState = digitalRead(switchPin);
}
void displayResult()
{
float h,m,s,ms;
unsigned long over;
elapsed=finished-start;
h=int(elapsed/3600000);
over=elapsed%3600000;
m=int(over/60000);
over=over%60000;
s=int(over/1000);
ms=over%1000;
Serial.print("Raw elapsed time: ");
Serial.println(elapsed);
Serial.print("Elapsed time: ");
Serial.print(h,0);
Serial.print("h ");
Serial.print(m,0);
Serial.print("m ");
Serial.print(s,0);
Serial.print("s ");
Serial.print(ms,0);
Serial.println("ms");
Serial.println();
}
// program loop
void loop()
{
if (Serial.available()>0 && iDisplay == 1)
{
finished=millis();
displayResult();
iDisplay = 0;//Only once
}
// read switch state and print line if state has changed
switch (digitalRead(switchPin)) { // read pin status
case HIGH:
if (switchState == LOW)
{ // check if message has to be sent
start=millis();
delay(200); // for debounce
iDisplay = 1;//Only once
Serial.println(100); // send message about switch
switchState = HIGH; // message has been sent
}
break;
case LOW:
if (switchState == HIGH)
{ // check if message has to be sent
start=millis();
delay(200); // for debounce
iDisplay = 1;//Only once
Serial.println(100); // send message about switch
switchState = LOW; // message has been sent
}
break;
}
}
And the following code for the slave:
// variables:
int intTime = 0;
// initialize
void setup()
{
// initialize serial wireless communication:
Serial.begin(9600);
}
// program loop
void loop()
{
if (Serial.available()>0)
{
intTime = Serial.parseInt();
if (intTime > 1)
{
Serial.println(intTime);
}
intTime = 0;
}
}
Yet this only returns the 200 Milliseconds from the debounce delay, can this be done with the Arduino? Am I getting the math or the code wrong?