package org.usfirst.frc.team6595.robot;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Spark;

import org.opencv.core.Mat;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.hal.PDPJNI;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends SampleRobot {
    RobotDrive Terra = new RobotDrive(0, 1, 2, 3);
    Joystick XboxController1 = new Joystick(0);
    Spark hopperintake = new Spark(4);
    Spark liftmecha = new Spark(7);
    Spark geararm = new Spark(8);
    Joystick XboxController2 = new Joystick(1);
    final String BaselineAuto = "Baseline Auto";
    final String CntrPegAuto = "Center Peg Auto";
    SendableChooser<String> chooser = new SendableChooser<>();
    PowerDistributionPanel PDP;

    public Robot() {
        Terra.setExpiration(0.1);
    }

    @Override
    public void robotInit() {
        chooser.addDefault("Baseline Auto", BaselineAuto);
        chooser.addObject("Center Peg Auto", CntrPegAuto);
        SmartDashboard.putData("Auto modes", chooser);
        PDP = new PowerDistributionPanel();

        UsbCamera camera1 = CameraServer.getInstance().startAutomaticCapture();
        camera1.setResolution(640, 480);
        camera1.setFPS(30);

    }

    @Override
    public void autonomous() {
        String autoSelected = (String) chooser.getSelected();
        System.out.println("Autonomous Mode selected: " + autoSelected);

        switch (autoSelected) {
        case CntrPegAuto:
            Terra.setSafetyEnabled(false);
            Terra.drive(0.3, 0.0); // spin at half speed
            Timer.delay(2.3); // for 2 seconds
            Terra.drive(0.0, 0.0); // stop robot
            break;
        case BaselineAuto:
        default:
            Terra.setSafetyEnabled(false);
            Terra.drive(0.3, 0.0); // drive forwards half speed
            Timer.delay(2.5); // for 2 seconds
            Terra.drive(0.0, 0.0); // stop robot
            break;
        }
    }

    @Override
    public void operatorControl() {
        Terra.setSafetyEnabled(true);
        while (isOperatorControl() && isEnabled()) {

            this.updateDashboard();

            PDP.clearStickyFaults();
            Terra.tankDrive(-XboxController1.getRawAxis(1), -XboxController1.getRawAxis(5));

            if(XboxController2.getRawButton(3)) {
                hopperintake.set(-0.8);
            }
            else {
                hopperintake.stopMotor();
            }

            if(XboxController2.getRawButton(4)) {
                hopperintake.set(0.8);
            }
            else {
                hopperintake.stopMotor();
            }

            if(XboxController2.getRawButton(7)) {
                liftmecha.set(1);
            }
            else {
                liftmecha.stopMotor();
            }
            if(XboxController2.getRawButton(9)) {
                liftmecha.set(-1);
            }
            else {
                liftmecha.set(0);
            }

            if (XboxController2.getRawButton(5)) {
                geararm.set(0.4);
            }
            else {
                geararm.stopMotor();
            }

            if (XboxController2.getRawButton(6)) {
                geararm.set(-0.4);
            }
            else {
                geararm.stopMotor();
            }

        /*  Thread t = new Thread(() -> {

                boolean allowCam1 = false;

                UsbCamera camera1 = CameraServer.getInstance().startAutomaticCapture(0);
                camera1.setResolution(320, 240);
                camera1.setFPS(30);
                UsbCamera camera2 = CameraServer.getInstance().startAutomaticCapture(1);
                camera2.setResolution(320, 240);
                camera2.setFPS(30);

                CvSink cvSink1 = CameraServer.getInstance().getVideo(camera1);
                CvSink cvSink2 = CameraServer.getInstance().getVideo(camera2);
                CvSource outputStream = CameraServer.getInstance().putVideo("Switcher", 320, 240);

                Mat image = new Mat();

                while(!Thread.interrupted()) {

                    if(XboxController2.getRawButton(9)) {
                        allowCam1 = !allowCam1;
                    }

                    if(allowCam1){
                      cvSink2.setEnabled(false);
                      cvSink1.setEnabled(true);
                      cvSink1.grabFrame(image);
                    } else{
                      cvSink1.setEnabled(false);
                      cvSink2.setEnabled(true);
                      cvSink2.grabFrame(image);
                    }

                    outputStream.putFrame(image);
                }

            });
            t.start();*/


            Timer.delay(0.005);
        }

    }
    @Override
    public void test() {
    }

    public void updateDashboard() {

//      double angle = gyro.getAngle();
        double pdpvoltage =  PDP.getVoltage();

//      SmartDashboard.putNumber("Current Angle: ", angle);
        SmartDashboard.putNumber("Voltage of PDP: ", pdpvoltage);
    }
}


/*  public void straightTankDrive() {
    Joystick joystick = new Joystick(0);
    GyroBase gyro = new ADXRS450_Gyro();
    double reducedAngle = gyro.getAngle() * 0.03;
    int button = 0;
    int axis1 = 1;
    int axis5 = 5;
    int count = 0;
    if (joystick.getRawButton(button)) {
        if (count < 1) {
            gyro.reset();
            count++;
        }
        drive.drive((joystick.getRawAxis(axis1) + joystick.getRawAxis(axis5)) / 2, reducedAngle);
    } else {
        count = 0;
        drive.tankDrive(joystick.getRawAxis(axis1), joystick.getRawAxis(axis5));
    }

}*/

/*public void robotInit() {
UsbCamera cam0 = new UsbCamera("cam0", 0);
UsbCamera cam1 = new UsbCamera("cam1", 1);
UsbCamera selectedCam = cam0;
CvSink video = CameraServer.getInstance().getVideo(selectedCam);
CvSource outputStream = CameraServer.getInstance().putVideo("Stream", 640, 480);
Mat image = new Mat();
}*/