// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "commands/Auto.h"

#include <frc/Timer.h>

Auto::Auto(Chassis &chassis, Shooter &shooter, Gatherer &gatherer) :
    m_chassis(chassis), m_shooter(shooter), m_gatherer(gatherer) {
  // Use addRequirements() here to declare subsystem dependencies.
  SetName("Auto");
  AddRequirements({&m_chassis, &m_shooter, &m_gatherer});
}

// Called when the command is initially scheduled.
void Auto::Initialize() {
  m_start_time = frc::Timer::GetFPGATimestamp();
}

// Called repeatedly when this Command is scheduled to run
void Auto::Execute() {
  auto cur_time = frc::Timer::GetFPGATimestamp();
  auto time = cur_time - m_start_time;
  m_shooter.set(0.95_rpm);
  if(time > 7_s) {
    m_chassis.drive_field_relative(0_mps, 0_mps, 0_rad_per_s);
    m_shooter.set(0_rpm);
    m_gatherer.stop();
  } else if(time > 4_s) {
    auto speeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
      1.5_mps,
      0_mps,
      0_rad_per_s,
      frc::Rotation2d(units::degree_t(0)));
    m_chassis.drive(speeds);
  } else if(time > 2_s) {
    m_gatherer.shoot(false);
  }
}

// Called once the command ends or is interrupted.
void Auto::End(bool interrupted) {
  m_gatherer.stop();
}

// Returns true when the command should end.
bool Auto::IsFinished() {
  return false;
}