 // Revision: v0.0.1
 // Licence: Apache License Version 2.0, January 2004 http://www.apache.org/licenses/
 
-#include "PIDController.h"
+/*
+ * Proprietary code includes
+ */
+#include "Controller.h"
+#include "EmergencyHandler.h"
 #include "ESC.h"
 #include "GPS.h"
-#include "Utils.h"
 #include "Gyroscope.h"
 #include "Moisture.h"
 #include "ProximitySensor.h"
 #include "RudderServo.h"
 #include "Temperature.h"
-#include "EmergencyHandler.h"
+#include "Trajectory.h"
+#include "Utils.h"
 #include "XbeeReceiver.h"
-#include "Trajectory.h"
-
-/*
- * Defines
+
+/*
+ * Arduino includes
+ */
+#include <Wire.h>
+
+/*
+ * Compilation switch
+ * Fixed station code -> 0
+ * Boat code -> 1
+ */
+#define __TARGET__ 1
+
+/*
+ * General defines
  */
 #define SECONDS 1000
-#define START_DELAY 10*SECONDS
-#define __BOAT__ 1
-#define MOTOR_SPEED 22
+#define START_DELAY 1*SECONDS
+#define STATIONNARY_DELAY 10*SECONDS
+#define RUN_TIME 180*SECONDS
+#define LAKE_MONTJOIE 0
+#define LAKE_DES_NATIONS 1
+
+/*
+ * ESC related defines
+ */
+#define ESC_MIN_VALUE 1500
+#define ESC_MAX_VALUE 2000
+#define ESC_PIN 12
+#define PROPELLER_SPEED 22
+
+/*
+ * Sensor related defines
+ */
+#define MOISTURE_SENSOR_PIN 5
+#define RUDDER_SERVO_PIN 2
+#define TEMPERATURE_SENSOR_PIN 52
+
+/*
+ * Emergency state sensor threshold defines
+ */
 #define TEMPERATURE_THRESHOLD 50
 #define SONAR_THRESHOLD 1
 #define PROXIMITY_THRESHOLD 1
-#define STATIONNARY_DELAY 10*SECONDS
-#define RUN_TIME 180*SECONDS
-/********************************************/
-
-
-/*
- * Object instanciation
- */
-#if __BOAT__>0
-  PIDController PIDController;
-  ESC esc(1500, 2000, 12);
+#define HUMIDITY_THRESHOLD "Dry"
+
+/*
+ * First occurence of compialtion switch
+ */
+#if __TARGET__ == 1
+
+/*
+ * Objects instanciation
+ */
+ 
+  /*
+   * Sensor objects
+   */
   Gyroscope gyroscope;
-  Moisture moistureSensor;
   ProximitySensor proximitySensorArray;
-  RudderServo rudderServo;
-  Temperature temperatureSensor;
+  ESC propeller(ESC_MIN_VALUE, ESC_MAX_VALUE, ESC_PIN);
+  Moisture moistureSensor(MOISTURE_SENSOR_PIN);
+  RudderServo rudderServo(RUDDER_SERVO_PIN);
+  Temperature temperatureSensor(TEMPERATURE_SENSOR_PIN);
+  
+  /*
+   * Logic and control classes
+   */
+  Controller controller;
   Trajectory trajectory;
-  EmergencyHandler emergencyHandler("Dry", TEMPERATURE_THRESHOLD, SONAR_THRESHOLD, PROXIMITY_THRESHOLD);
-#endif
-
+  EmergencyHandler emergencyHandler(HUMIDITY_THRESHOLD, TEMPERATURE_THRESHOLD, SONAR_THRESHOLD, PROXIMITY_THRESHOLD);
+  
+#endif /*__TARGET__ == 1*/
+
+  /*
+   * These two devices are used both on the boat and the fixed station 
+   * so they are not bound by the compilation switch.
+   */
   GPS gps;
   XbeeReceiver xbee;
 /********************************************/ 

/*
 * Variable definitions and initialization
 */
#if __BOAT__>0
  int currentEmergencyState = 0;
  int sonar = 0;
  ProximitySensorData proximitySensorData;
  NMEAData nextGpsData;
  NMEAData basecampGPSData;
  NMEAData gpsError;
  String moisture= "Unknown";
  uint8_t temperature = 0;
  double heading = 0.0;
  uint8_t count = 0;
  double theta = 0.0;
  double rudder = 0.0;
  int servoValue = 0;
  int escValue = 0;
  double gpsValueLat = 0.0;
  double gpsValueLon = 0;
  unsigned long timer1;
  unsigned long timer2;
  double distanceToTarget = 0.00;
  //double destinationPoint[1][2] = { 45.379184, 71.9239 }; //test campus
  double destinationPoint[1][2] = {45.39545,71.92775};

  int gpsListCounter = 2;
  double listPointsX[10] = {0};
  double listPointsY[10] = {0};
  
#endif /* __TARGET__ */

  /*
   * Xbee related variables
   */
  const char GPS = 'G';
  const char Manuel = 'M';
  char mode = 'E';

  /*
   * Holds the most recent GPS data
   */
  NMEAData currentGpsData;
/********************************************/

