#include <Easy.h>

//*****************************************************************
// Parameter I2C Motor
#define MOTOR_TOGGLE_NUMBER 2
#define MOTOR_TIMED_NUMBER 1
// Parameter MainSwitch
#define MAIN_SWITCH_PIN 39
// Parameter variable Input
#define VARIABLE_INPUT_PIN A0
// ToggleSwitch
#define DIRECTION_PIN1 43
#define DIRECTION_PIN2 41

// Parameters Monoflop end switch
#define MONOFLOP_LEFT_PIN 43
#define MONOFLOP_LEFT_HIGH_DELAY 5000
#define MONOFLOP_LEFT_LOW_DELAY 200
#define MONOFLOP_RIGHT_PIN 41
#define MONOFLOP_RIGHT_HIGH_DELAY 5000
#define MONOFLOP_RIGHT_LOW_DELAY 200

void setup() {
  //((*** Initialize: Configure your sketch here....
  //** Define Actuators:
  Actuator* motorToogles = new MotorI2C(MOTOR_TOGGLE_NUMBER);    // Create toggeling motor
  Actuator* motorTimed = new MotorI2C(MOTOR_TIMED_NUMBER);       // Create timed motor

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

  // Create the speed inputs
  Input* motorSpeed = new VariableInput(VARIABLE_INPUT_PIN);
  Input* inverter = new Inverter(motorSpeed);
  Input* stopSpeed = FixValue::Off();

  // Define input monoflop to the limit switch, they turn the move of toggeling motor
  Input* monoFlopLeft = new MonoFlop(MONOFLOP_LEFT_PIN, MONOFLOP_LEFT_HIGH_DELAY, MONOFLOP_LEFT_LOW_DELAY, true, smPullUpExternal);
  Input* monoFlopRight = new MonoFlop(MONOFLOP_RIGHT_PIN, MONOFLOP_RIGHT_HIGH_DELAY, MONOFLOP_RIGHT_LOW_DELAY, false, smPullUpExternal);

  //** Define logic with condition and relation
  // ActuatorCollections are used because more than one actuator depends on the same condition.
  // Because each ActuatorCollectionItems has its own input, the Relation has no input (NULL).

  // Relation 1: Toggle motor reach limit switch left. Timed motor start for a defined time (backward).
  Condition* monoFlopLeftHigh = new LogicCondition(mainSwitch, OpEQ, Switch2Position::On, LgAND, monoFlopLeft, OpEQ, MonoFlop::StateHigh);
  Actuator* toggleMotorStopDelayMotorStartBackward = new ActuatorCollection(motorToogles, stopSpeed, motorTimed, inverter);
  Relation* toggleMotorReachLimitSwitchLeft = new Relation1to1(monoFlopLeftHigh, toggleMotorStopDelayMotorStartBackward, NULL);

  // Relation 2: Timer duration ended, stop timed motor and start toggle motor in direction to right
  Condition* monoFlopLeftHighTimerEnd = new LogicCondition(mainSwitch, OpEQ, Switch2Position::On, LgAND, monoFlopLeft, OpEQ, MonoFlop::StateHighTimerEnd);
  Actuator* toggleMotorStopDelayMotorStartToRight = new ActuatorCollection(motorToogles, motorSpeed, motorTimed, stopSpeed);
  Relation* timedMotorStopToggleMotorStartToRight = new Relation1to1(monoFlopLeftHighTimerEnd, toggleMotorStopDelayMotorStartToRight, NULL);

  // Relation 3: Toggle motor reach limit switch right. Timed motor start for a defined time (forward).
  Condition* monoFlopRightHigh = new LogicCondition(mainSwitch, OpEQ, Switch2Position::On, LgAND, monoFlopRight, OpEQ, MonoFlop::StateHigh);
  Actuator* toggleMotorStopDelayMotorStartForward = new ActuatorCollection(motorToogles, stopSpeed, motorTimed, motorSpeed);
  Relation* motorTimedStartRight = new Relation1to1(monoFlopRightHigh, toggleMotorStopDelayMotorStartForward, NULL);

  // Relation 4: Timer duration ended, stop timed motor and start toggle motor in direction to left
  Condition* monoFlopRightHighTimerEnd = new LogicCondition(mainSwitch, OpEQ, 1, LgAND, monoFlopRight, OpEQ, MonoFlop::StateHighTimerEnd);
  Actuator* toggleMotorStopDelayMotorStartToLeft = new ActuatorCollection(motorToogles, inverter, motorTimed, stopSpeed);
  Relation* timedMotorStopToggleMotorStartToLeft = new Relation1to1(monoFlopRightHighTimerEnd, toggleMotorStopDelayMotorStartToLeft, NULL);

  // Relation 5: Imidiately stop when main switch goes to low.
  Condition* motorStopCondition = new CompareCondition(mainSwitch, OpEQ, Switch2Position::Off);
  // A different notation is used here. Up to 4 items are possible per constructor argument (as above),
  // this kind can contain any number of ActuatorCollectionItems.
  // A define is used for readability. It's not a const because they uses memory.
  #define ACTUATOR_COLLLECTION_ITEM_SIZE 2
  CollectionItem** stopAllMotors = new ActuatorCollectionItem*[ACTUATOR_COLLLECTION_ITEM_SIZE];
  stopAllMotors[0] = new ActuatorCollectionItem(motorToogles, stopSpeed);
  stopAllMotors[1] = new ActuatorCollectionItem(motorTimed, stopSpeed);
  Relation* relationStopAllMotors = new Relation1to1(motorStopCondition, stopAllMotors, ACTUATOR_COLLLECTION_ITEM_SIZE);
  // ***))

  // 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);
}