/**
 * @file main.cpp
 * @author sami dhiab ([email protected])
 * @brief 
 * @version 0.1
 * @date 2022-02-11
 * 
 * @copyright Copyright (c) 2022
 * 
 */
//#define COM_V1 
#ifdef COM_V1
#include "Communication_v1.hpp"
#else
#include "com_v2.hpp"
#endif


//#define DECLARE_POINTERS
#ifdef DECLARE_POINTERS
// Create pointers objects 
   // Setup Motors Objects: Note: PIN must be defined in config file
  Nema8* M_X= new Nema8(0, XMs1_pin, XMs2_pin, XMs3_pin, XDir_pin, XEnable_pin, XStep_pin, down);
  Nema8* M_Y= new Nema8(0, YMs1_pin, YMs2_pin, YMs3_pin, YDir_pin, YEnable_pin, YStep_pin, down);
  Nema8* M_Z= new Nema8(0, ZMs1_pin, ZMs2_pin, ZMs3_pin, ZDir_pin, ZEnable_pin, ZStep_pin, down);
  // Setup EndStops Objects
  endSwitch* ES_x= new endSwitch(E_X, LOW);
  endSwitch* ES_y= new endSwitch(E_Y, LOW);
  endSwitch* ES_z= new endSwitch(E_Z, LOW);
  endSwitch* ES_Emergency = new endSwitch(E_Emergency, LOW);
  // Setup Distance Sensors Objects
  DistanceSensor* DS_x= new DistanceSensor(D_X, DT);
  DistanceSensor* DS_y= new DistanceSensor(D_Y, DT);
  DistanceSensor* DS_z= new DistanceSensor(D_Z, DT);
  // Setup Light barrier Objects
  lightBarrier* LB_x= new lightBarrier(LB_X, LOW);
  lightBarrier* LB_y= new lightBarrier(LB_Y, LOW);
  lightBarrier* LB_z= new lightBarrier(LB_Z, LOW);
  // Setup Alignment Object
  Alignment* AlignObj = new Alignment(M_X, M_Y, M_Z, DS_x, DS_y, DS_z, ES_x, ES_y, ES_z,ES_Emergency, LB_x, LB_y, LB_z);
  #ifdef COM_V1
  // Setup communication v1
  AlignmentCommunication* AlignComObj = new AlignmentCommunication(AlignObj);
  #else
  // Setup com v2
  AlignCom*  AlignComObj = new AlignCom(&AlignObj);
  #endif

void setup() {
  AlignObj->setTargetDistanceToML(4); //4 mm between pritnhead and ML
  AlignComObj->initCom(9600);
}
void loop() { 
    
    AlignComObj->processCom();

}
#else
// Create Objects 
   // Setup Motors Objects: Note: PIN must be defined in config file
  Nema8 M_X(0, XMs1_pin, XMs2_pin, XMs3_pin, XDir_pin, XEnable_pin, XStep_pin, down);
  Nema8 M_Y(0, YMs1_pin, YMs2_pin, YMs3_pin, YDir_pin, YEnable_pin, YStep_pin, down);
  Nema8 M_Z(0, ZMs1_pin, ZMs2_pin, ZMs3_pin, ZDir_pin, ZEnable_pin, ZStep_pin, down);
  // Setup EndStops Objects
  endSwitch ES_x(E_X, LOW);
  endSwitch ES_y(E_Y, LOW);
  endSwitch ES_z(E_Z, LOW);
  endSwitch ES_Emergency(E_Emergency, LOW);
  // Setup Distance Sensors Objects: hc_sr04
  DistanceSensor DS_x(D_X, DT);
  DistanceSensor DS_y(D_Y, DT);
  DistanceSensor DS_z(D_Z, DT);
  // Setup Light barrier Objects
  lightBarrier LB_x(LB_X, LOW);
  lightBarrier LB_y(LB_Y, LOW);
  lightBarrier LB_z(LB_Z, LOW);
  // Setup Alignment Object
  Alignment AlignObj(&M_X, &M_Y, &M_Z, &DS_x, &DS_y, &DS_z, &ES_x, &ES_y, &ES_z,&ES_Emergency, &LB_x, &LB_y, &LB_z);
  #ifdef COM_V1
  // setup com v1
  AlignmentCommunication AlignComObj(&AlignObj);
  #else 
  //setup com v2
  AlignCom AlignComObj(&AlignObj);
  #endif

void setup() {
  // if to test distance sensor to ML
  AlignObj.setTargetDistanceSensorToML(140); //minimum target
  // if to test distance ph to ML
  AlignObj.setTargetDistancePHToML(5); // 
  AlignComObj.initCom(9600);
}
void loop() { 

    AlignComObj.processCom();

}
#endif