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

//STAGE #1
const int solenoid_pin = 2;
const int HE_pin = A0;

//Define pendulum length
const int pendulum_length = 0.23;
const int ping_time = 200;
const int peak_threshold = 520;

//Sensor readings
int HE_reading = 0;

/*--------------------------------------------  
-------- End Initialise Variables ----------
--------------------------------------------
*/


/* 
----------------------------------  
-------- Begin Setup Loop --------
----------------------------------
*/

void setup() 
{

  // initialize serial communication at 115200 bits per second.
  Serial.begin(9600);

  //STAGE 2 - define pin modes
  pinMode(solenoid_pin, OUTPUT);
  pinMode(HE_pin, INPUT);  
  
  //Control solenoid
  digitalWrite(solenoid_pin, LOW);
  digitalWrite(solenoid_pin, HIGH);
  delay(ping_time); //WAIT FOR 200ms
  digitalWrite(solenoid_pin, LOW);
}

/* 
-------------------------------
------- End Setup Loop --------
-------------------------------
*/

/* 
----------------------------------  
-------- Begin Main Loop ---------
----------------------------------
*/ 

  //STAGE 3 - the main loop
void loop() 
{
  // define local variables for timing
  unsigned long startTime = 0;
  unsigned long currentTime = 0;
  unsigned long elapsedTime = 0;
  int HE_reading_old = 0;
  int peak_count = 0;
  float gravity = 0;
  float T_period = 0;

  //get start time
  startTime = millis();

  do  {
    
     //read Hall effects sensor
     HE_reading = analogRead(HE_pin);

     //get current time
     currentTime = millis();

     //calc elapsed time in milliseconds
     elapsedTime = currentTime - startTime;

     //DEtect if peak has occured
     if( (HE_reading>=peak_threshold) && (HE_reading_old<peak_threshold) )
     {
        peak_count = peak_count + 1; //register that a peak has occured
     }

     HE_reading_old = HE_reading;
	
	  //Print data back to the computer
	  Serial.print(elapsedTime);
    Serial.print(',');
    Serial.print(HE_reading);
    Serial.print(',');
    Serial.print(peak_count);
    Serial.print('\n');
	
  } while (elapsedTime<=10000);

  
  //STAGE 5 - calculate Gravity...
  //average_period = something here;

  //evaluate gravity
  //g = your calculation here: use the sq() and PI

  //STAGE 5b - print your answer...
  Serial.print("gravity = ");
  
  delay(10000); //wait for 10 seconds - LEAVE THIS FOR RELOAD TO WORK PROPERLY!
}

/* 
----------------------------------  
--------- End Main Loop ----------
----------------------------------
*/