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

//pins
int trig_Pin_f = 3; //US sensor forward trigger pin
int echo_Pin_f = 2; //US sensor forward echo pin

// Hbridge controller
int enable = 9;
int direction = 6;

// LDR Light-beam sensors
int platform_sensor = A2;
int start_sensor = A0;
int tunnel_sensor = A1;

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

//
int LDR_start = 0;
int LDR_platform = 0;
int LDR_tunnel = 0;
int LDR_threshold = 0;
int LDR_background = 0;
const int LDR_step = -100 ;


/*
  ----------------------------------
  -------- 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(trig_Pin_f, OUTPUT); //US sensor forward trigger pin = output
   pinMode(echo_Pin_f, INPUT); //US sensor forward echo pin = input

   //Hbridge pin setup
   pinMode(enable, OUTPUT);
   pinMode(direction, OUTPUT);

   //LDR light-beam sensors
   pinMode(platform_sensor, INPUT);
   pinMode(start_sensor, INPUT);
   pinMode(tunnel_sensor, INPUT);
 
   // Find background light levels
   find_LDR_background();

   //**** 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(Fordistance);

  delay(100);

  //**** Send control signals to train
  // Set direction
  // Set PWM demand signal


  //**** 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()
{

  int slot_counter = 0;
  int speed_demand_1 = 50;

  do {
   //Set direction of the motor to forward...?
   digitalWrite(direction, LOW);
   //Map the power required to PWM value
   motor_PWM = map(speed_demand_1, 0, 100, 0, 255);
   analogWrite(enable, motor_PWM);

   //read LDR sensor
   LDR_start = analogRead(start_sensor);
   Serial.println(LDR_start);

   //increment counter when light is cut
   //if (LDR_start < LDR_threshold) {
    //Serial.print(slot_counter);
    //slot_counter = slot_counter + 1;
   //}

   } while(LDR_start > LDR_threshold);
} 

//float getDistance(int trigPIN, int echoPIN);
//{
  //Initialise LOCAL variables used in this function
  //float distance = 0;
  //unsigned long duration = 0;

  //**** Set the trigPin HIGH for 10 microseconds
  //digitalWrite(trigPIN, HIGH);//Set the pin high
  //delayMicroseconds(10);//delay for 10 microseconds
  //digitalWrite(trigPIN, LOW);//Set the pin low


  // Reads echoPin which returns the sound wave travel time in microseconds
  //duration = pulseIn( echoPIN, HIGH) //Use the pulseIn() function here

             // Calculate the distance
             //distance = duration;// calc the distance - see notes for equation

  //Return the value from the function
  //return distance; //
//


void find_LDR_background()
{
  int i =0;
  for(i=0 ; i<=500 ; i++){
  //LDR_previous = LDR_current;
  LDR_start = analogRead(start_sensor);
     if(LDR_start > LDR_background){
     LDR_background = LDR_start;
     //Serial.println(LDR_background);  
     }
  }
  LDR_threshold = LDR_background + LDR_step;
  Serial.print(LDR_threshold);
}