#include "Servo.h"
#include "FSM.h"

enum State { IDLE, MOVE_TO_MAX, CLOSE_TIME, MOVE_TO_MIN, OPEN_TIME };

//        MIN   MAX    Name  SPEED  OPEN     CLOSE    AUTOMATIC  FORWARD  VERBOSE
FSM fsm0(   0,   90,   "00"  ,10UL  ,1UL,5000UL  ,10000UL  ,true      ,true    ,false);
//FSM fsm1( 180,   90,   "01"  ,100UL ,5000UL  ,4000UL  ,true      ,false   ,false);
FSM fsm2( 180,    0,   "02"  ,10UL  ,10UL ,1UL  ,1UL ,false ,false   ,false);
FSM fsm3(   0,   180,   "03"  ,10UL ,10UL ,1UL  ,1UL  ,true ,true    ,false);

Servo s0, s1, s2, s3;

#define ledPin 13

boolean pivot = false;

void setup() {
  Serial.begin(9600);
  pinMode(ledPin, OUTPUT);
  s0.attach( 3); fsm0.init(State::OPEN_TIME);
//s1.attach( 9); fsm1.init(State::OPEN_TIME);
  s2.attach(10); fsm2.init(State::OPEN_TIME);
  s3.attach(11); fsm3.init(State::OPEN_TIME);  
}

void loop() {
  static unsigned long previousMillis = millis();


  if (fsm0.currentState() == "MOVE_TO_MAX") {
    digitalWrite(ledPin, HIGH);
  } 
  //fsm2.setState(State::MOVE_TO_MAX);
  
  
  if (fsm0.currentState() == "MOVE_TO_MIN") {
    digitalWrite(ledPin, LOW);
    if (fsm0.update() == fsm0.getMinPos()) {
      previousMillis = millis();
      pivot = !pivot;
    }
  }
   if (millis() - previousMillis >= 2000UL) {
    previousMillis = millis();
    
    if (pivot){fsm2.setState(State::MOVE_TO_MAX);}
    if (!pivot){fsm2.setState(State::MOVE_TO_MIN);}
   }

  // fsm0.setOpenTime(10UL);
  // fsm0.setCloseTime(10UL);  
 
  
  
    s0.write(fsm0.update());
 //   s1.write(fsm1.update());
    s2.write(fsm2.update());
    s3.write(fsm3.update());

 };