#include "Autonomous.h"
#include "Commands/CommandGroup.h"
#include "../../DentRobot.h"
#include "../Elevator/Raise.h"
#include "../Elevator/Lower.h"
#include "../BinElevator/BinRaise.h"
#include "../BinElevator/BinLower.h"
#include "AutoDrive.h"
#include "Turn.h"
#include "../Collector/RollIn.h"
#include "CollectTote.h"
#include "ReleaseTote.h"
Autonomous::Autonomous(int seq) {
  Wait(SmartDashboard::GetNumber("Auto Wait Time"));
  switch(seq) {
    case 0:
      // Just for testing
      // Turn testing
      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
      break;
    case 1:
      // Drive to Auto Zone (TM)
      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8, 0.01));
      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
      break;
    case 2:
      // Collect a tote with BinElevator, turn, drive to Auto Zone (TM)
      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
      AddSequential(new BinRaise(1.2));
      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
      AddSequential(new BinLower(1.0));
      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
      break;
    case 3:
      // Collect one, two, or three totes, drive to Auto Zone (TM), release totes
      AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
      if(SmartDashboard::GetBoolean("Two totes")) {
        AddParallel(new Turn(0.81));
        AddSequential(new Raise(3.5));
        AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
        AddSequential(new CollectTote());
        AddSequential(new Lower(3.0));
        AddSequential(new Raise(3.5));
        if(SmartDashboard::GetBoolean("Three totes")) {
          AddSequential(new Turn(3.8));
          AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75));
          AddSequential(new CollectTote());
          AddSequential(new Lower(3.0));
          AddSequential(new Raise(3.5));
        }
      }
      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.75));
      AddSequential(new ReleaseTote());
      break;
    case 4:
      //Use rear elevator to move tote
      AddSequential(new Turn(1.8));
      AddSequential(new AutoDrive(2.3, 0.0, -0.75));
      AddSequential(new Turn(1.8));
      break;
    case 5:
      //Use rear elevator to move bin
      AddSequential(new BinLower(0.1));
      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.75));
      AddSequential(new Turn(2.1));
      break;
    case 6:
      //Drive forward and collect a single gray tote
      AddSequential(new Lower(3.0));
      AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
      AddSequential(new Lower(3.0));
      AddSequential(new Raise(3.5));
      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
    default:
      printf("Invalid seq: %d\n", seq);
      break;
  }
}
// vim: ts=2:sw=2:et