/* 
--------------------------------------------  
-------- Begin Initialise Variables --------
--------------------------------------------
*/

//**** STAGE 1 ********

//DEFINE VARIABLES HERE....
//pins
int trig_Pin_f = 3; //US sensor forward trigger pin
int echo_Pin_f = 2; //US sensor forward echo pin

float distance = 0.0; // variable for the forwards distance measurement
int motor_PWM = 0;// define PWM output

int enA = 9;
int direction = 6;


int platform_sensor = A2;
int test_start_sensor = A0;
int tunnel_sensor = A1;
int pwmOut;
/* 
----------------------------------  
-------- Begin Setup Loop --------
----------------------------------
*/

//**** STAGE 2 *******
void setup() {

  //begin serial transmission
  Serial.begin(9600); // Start Serial comms with a baud rate of 9600

  //**** SETUP YOUR PINS HERE
  pinMode(enA, OUTPUT);
  pinMode(direction, OUTPUT);
  pinMode(trig_Pin_f, OUTPUT); //US sensor forward trigger pin = output
  pinMode(echo_Pin_f, INPUT); //US sensor forward echo pin = input
  pinMode(platform_sensor,OUTPUT);
  pinMode(test_start_sensor,OUTPUT);
  pinMode(tunnel_sensor,OUTPUT);
  //**** Call your initialise function
  initialise_train();	//you can write this function if you like- found at the end of the code!
  
  //**** initialise your US trigger (set to LOW)
  digitalWrite(trig_Pin_f, LOW);
  Serial.println("DEBUG: System Initialised");

}


/* 
----------------------------------  
-------- Begin Main Loop ---------
----------------------------------
*/ 
void loop() {

  //**** STAGE 3 *******
  
  //Get the distance to the wall
  //**** WRITE THIS FUNCTION ****
  distance = getDistance(trig_Pin_f,echo_Pin_f); //measure the distance from the US sensor
  
  Serial.println(distance);

  delay(100);
 
  //**** Send control signals to train

  
  //**** Write variables to Serial with comma to seperate - for plotting)

  //**** distance, control signal - you could also add time [optional]

}

/* 
----------------------------------------------------------------------------- 
--------- Declare User Defined Functions to be called by Main Loop ----------
-----------------------------------------------------------------------------
*/ 

void initialise_train()
{
// Set direction
// Set PWM demand signal  
int speed_demand = 50; //0-100 value
//Set Direction CW
digitalWrite(direction, LOW);
//Set Speed via PWM
pwmOut = map(speed_demand, 0, 100, 0 , 255);
analogWrite(enA, pwmOut);

}

float getDistance(int trig_Pin_f, int echo_Pin_f)
{
    //Initialise LOCAL variables used in this function
	float distance = 0.0;
	unsigned long duration = 0;
	
	//**** Set the trigPin HIGH for 10 microseconds
	digitalWrite(trig_Pin_f, HIGH);//Set the pin high
  delayMicroseconds(10);//delay for 10 microseconds
  digitalWrite(trig_Pin_f, LOW);//Set the pin low
  
  
    // Reads echoPin which returns the sound wave travel time in microseconds
  duration = pulseIn(echo_Pin_f,HIGH); //Use the pulseIn() function here

    // Calculate the distance
  distance = duration*(0.000343/2);// calc the distance - see notes for equation
	
	//Return the value from the function
Serial.println(distance); //

}
float getSpeed (int trig_Pin_f, int echo_Pin_f)
{
  float speed = 0.0;
  unsigned long duration_speed = 0;
  unsigned long duration_speed_2 = 0;
	
  digitalWrite(trig_Pin_f, HIGH);//Set the pin high
  delayMicroseconds(10);//delay for 10 microseconds
  digitalWrite(trig_Pin_f, LOW);//Set the pin low
  duration_speed = pulseIn(echo_Pin_f,HIGH);
  delay(1000);
  digitalWrite(trig_Pin_f, HIGH);//Set the pin high
  delayMicroseconds(10);//delay for 10 microseconds
  digitalWrite(trig_Pin_f, LOW);//Set the pin low
  duration_speed_2 = pulseIn(echo_Pin_f,HIGH);

  speed = distance/((duration_speed_2-duration_speed)/1000000);
  Serial.println(speed);
}