#include <Easy.h>

//*****************************************************************
// Parameter Master Motor
#define MOTOR_LEADER_NR 2
#define MOTOR_FOLLOWER_NR 1
// Parameter MainSwitch
#define MAIN_SWITCH_PIN 14
// Parameter variable Input
#define VARIABLE_INPUT_PIN A0
// ToggleSwitch
#define DIRECTION_PIN1 43
#define DIRECTION_PIN2 41

//***************************************************************************************************************
void setup() {
  //((*** Initialize: Configure your sketch here....

  //** Define Actuators:
  Actuator* motorLeader = new MotorI2C(MOTOR_LEADER_NR);
  Actuator* motorFollower = new MotorI2C(MOTOR_FOLLOWER_NR);

  //** Define Input
  // Main switch to start/stop all.
  Input* mainSwitch = new Switch2Position(MAIN_SWITCH_PIN);

  // Switch knows the value Pos1 and Pos2. The value changes depending on two signal.
  // This makes it possible to define motor direction (when value change).
  Input* toggleSwitch = new ToggleSwitch(DIRECTION_PIN1, DIRECTION_PIN2);

  // Create variable input, defines speed
  Input* motorSpeed = new VariableInput(VARIABLE_INPUT_PIN);
  // To turn backward an inverter changes the sign of the variable input value
  Input* inverter = new Inverter(motorSpeed);

  //** Define logic with condition and relation
  // Define direction when signal 1 (limit switch 1 pressed). An ActuatorCollection is used because more than one Actuator depends
  // on the same condition. Because the input is assigned into the ActuatorCollection, the value is NULL into the Relation.
  Condition* motorDirection1Condition = new LogicCondition(mainSwitch, OpEQ, Switch2Position::On, LgAND, toggleSwitch, OpEQ, ToggleSwitch::Pos2);
  Actuator* actuatorCollectionMotorDirection1 = new ActuatorCollection(motorLeader, motorSpeed, motorFollower, motorSpeed);
  Relation* relationMotorDirection1 = new Relation1to1(motorDirection1Condition, actuatorCollectionMotorDirection1, NULL);

  // Define direction when signal 2 (limit switch 2 pressed). An ActuatorCollection is used because more than one Actuator depends
  // on the same condition. Because the input is assigned into the ActuatorCollection, the value is NULL into the Relation.
  Condition* motorDirection2Condition = new LogicCondition(mainSwitch, OpEQ, Switch2Position::On, LgAND, toggleSwitch, OpEQ, ToggleSwitch::Pos1);
  Actuator* actuatorCollectionMotorDirection2 = new ActuatorCollection(motorLeader, inverter, motorFollower, inverter);
  Relation* relationMotorDirection2 = new Relation1to1(motorDirection2Condition, actuatorCollectionMotorDirection2, NULL);

  // Define stop all, when switch is off
  Condition* motorStopCondition = new CompareCondition(mainSwitch, OpEQ, Switch2Position::Off);
  Actuator* actuatorCollectionMotorStop = new ActuatorCollection(motorLeader, FixValue::Off(), motorFollower, FixValue::Off());
  Relation* relationMotorStop = new Relation1to1(motorStopCondition, actuatorCollectionMotorStop, NULL);
  // ***))

  // Initialize control
  ControlManagerFactory::GetControlManager()->Setup();
}

//*****************************************************************
void loop() {
  //*** Run: No additional code is required
  ControlManagerFactory::GetControlManager()->Loop();

  // Depending on Arduino it needs a short delay. Do not add any other delays!
  delay(5);
}