// 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.

package frc.robot;

import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.LEDAnimationKit.AnimationManager;
import frc.robot.LEDAnimationKit.LEDStrip;
import frc.robot.LEDAnimationKit.LogicalLEDStrip;
import frc.robot.LEDAnimationKit.PhysicalLEDStrip;
import frc.robot.LEDAnimationKit.animations.Breathing;
import frc.robot.LEDAnimationKit.animations.FluidRainbow;
import frc.robot.constants.LEDConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Pusher;
import frc.robot.subsystems.Shooter;

public class RobotContainer {
  private Drivetrain drivetrain;
  private Shooter shooter;
  private Pusher pusher;

  private CommandXboxController driver;

  private PhysicalLEDStrip strip;

  private LogicalLEDStrip l1;
  private LogicalLEDStrip l2;

  private AnimationManager manager;

  //private FluidRainbow rainbow;

  private Breathing b1;
  private Breathing b2;

  public RobotContainer() {
    drivetrain = new Drivetrain(true);
    shooter = new Shooter();
    pusher = new Pusher();

    driver = new CommandXboxController(OIConstants.kDriverUSB);

    strip = new PhysicalLEDStrip(
      LEDConstants.kLEDPWM, 
      LEDConstants.kNumLEDs
    );
    strip.start();

    //rainbow = new FluidRainbow(strip, LEDConstants.kRainbowUpdateRate);
    //rainbow.start();
    l1 = new LogicalLEDStrip(strip, 0, 31);
    l2 = new LogicalLEDStrip(strip, 31, 62);

    

    b1 = new Breathing(
      l1, 
      LEDConstants.kBreathingUpdateRate, 
      new Color8Bit(Color.kPurple), 
      LEDConstants.kBreathingMinPercentage, 
      LEDConstants.kBreathingMaxPercentage
    );
    b1.start();

    b2 = new Breathing(
      l2, 
      LEDConstants.kBreathingUpdateRate, 
      new Color8Bit(255, 64, 0), 
      LEDConstants.kBreathingMinPercentage, 
      LEDConstants.kBreathingMaxPercentage
    );
    b2.start();

    manager = new AnimationManager(new PhysicalLEDStrip[] {strip});
    //manager.registerAnimationForUpdate(rainbow);
    manager.registerAnimationForUpdate(b1);
    manager.registerAnimationForUpdate(b2);
    manager.start(LEDConstants.kAnimationManagerUpdateRate);

    configureBindings();
    configureShuffleboard();

    System.out.println("Setup completed");
  }

  private void configureBindings() {
    drivetrain.setDefaultCommand(
      drivetrain.arcadeDrive(
        driver::getLeftY, 
        driver::getLeftX
      )
    );

    driver.start().onTrue(drivetrain.toggleSlowMode());

    shooter.setDefaultCommand(
      shooter.setSpeed(
        driver::getRightTriggerAxis
      )
    );

    driver.a().onTrue(pusher.spin());
    driver.a().onFalse(pusher.stop());
  }

  private void configureShuffleboard() {
    ShuffleboardTab tab = Shuffleboard.getTab("DRIVE");

    tab.addBoolean("Slow Mode?", drivetrain::isSlowMode)
      .withPosition(0, 0)
      .withSize(2, 1)
      .withWidget(BuiltInWidgets.kBooleanBox);

    Shuffleboard.selectTab("DRIVE");
  }

  public Command getAutonomousCommand() {
    return Commands.print("No autonomous command configured");
  }
}