#include <iostream>
#include <string>
#include <memory>
#include <stdlib.h>
#include <math.h>
//Main header files.
#include <Robot.h>
#include "frc/WPILib.h"
//Peripheral header files.
#include "frc/Doublesolenoid.h"
#include "frc/Timer.h"
#include <frc/AnalogGyro.h>
#include <frc/ADXRS450_Gyro.h>
#include <ctre/Phoenix.h>
#include <AHRS.h>
//Vision header files
#include "cameraserver/CameraServer.h"
#include "vision/VisionPipeline.h"
#include "vision/VisionRunner.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include "C:\Users\GoldA\GRIP\GripPipeline.h"
#include "C:\Users\GoldA\GRIP\GripPipeline.cpp"

        const int valveWait = 0.25;

	enum rightJoystickMap{
		ACTUATOR_TOGGLE = 1,
		GEAR_CHANGE = 2,
	};

//Common control/toggling blocks.
//is[mechanism] controls state
//isIn[mechanism] Dashboard display state.
//[mechanism]SoloTest manages a single activation of solenoid [see teleopPeriodic]

        bool isHighGear;
        bool isInHighGear;
        bool shiftingSoloTest;

       bool REMOVETEMPisHatch;
       bool REMOVETEMPisHatchReady;
       bool REMOVETEMPhatchSoloTest;

	DoubleSolenoid *gearBox;
	DoubleSolenoid *hatchDisconnect;

void Robot::RobotInit() {
        isHighGear = false;
        isInHighGear = false;
        shiftingSoloTest = true;

        REMOVETEMPisHatch = false;
        REMOVETEMPisHatchReady = false;
        REMOVETEMPhatchSoloTest = false;

        leftJoystick = new Joystick(0);
        rightJoystick = new Joystick(1);
        gamePad = new Joystick(2);


        gearBox = new DoubleSolenoid{2, 1};
        hatchDisconnect = new DoubleSolenoid{6, 7};

}

void Robot::RobotPeriodic() {
    SmartDashboard::PutBoolean("Gearbox State", isInHighGear);
}

//This function simply shifts the robot into high gear.
//Wait(valveWait); exists to allow enough air to flow before switching the solenoid off.
//isHighGear != isHighGear is necessary to toggle a control block [see teleopPeriodic]
//Both SHIFT_HIGH and SHIFT_LOW use the same logic.
//isInHighGear simply tells the driver what state the transmissions are in.

void SHIFT_HIGH () {
	gearBox->Set(frc::DoubleSolenoid::Value::kForward);
	Wait(valveWait);
	gearBox->Set(frc::DoubleSolenoid::Value::kOff);
	isHighGear =! isHighGear;
	//Where green is high gear, and red is low gear.
	isInHighGear = false;
	SmartDashboard::PutBoolean("Gearbox State", isInHighGear);
}

void SHIFT_LOW () {
	gearBox->Set(frc::DoubleSolenoid::Value::kForward);
	Wait(valveWait);
	gearBox->Set(frc::DoubleSolenoid::Value::kOff);
	isHighGear =! isHighGear;
	isInHighGear = true;
	SmartDashboard::PutBoolean("Gearbox State", 
}

//The next two functions rely on the same operating principles as the two functions above.

void REMOVE_THIS_HATCH_OPEN() {
	hatchDisconnect->Set(DoubleSolenoid::Value::kForward);
	Wait(valveWait);
	hatchDisconnect->Set(DoubleSolenoid::Value::kOff);
	REMOVETEMPisHatch =! REMOVETEMPisHatch;
	//Where green is high gear, and red is low gear.
	REMOVETEMPisHatchReady = false;
	SmartDashboard::PutBoolean("Hatch State", isInHighGear);
}

void REMOVE_THIS_HATCH_CLOSE() {
	hatchDisconnect->Set(DoubleSolenoid::Value::kReverse);
	Wait(valveWait);
	hatchDisconnect->Set(DoubleSolenoid::Value::kOff);
	REMOVETEMPisHatch =! REMOVETEMPisHatch;
	REMOVETEMPisHatchReady = true;
	SmartDashboard::PutBoolean("Hatch State", isInHighGear);
}

void Robot::TeleopInit() {
    SHIFT_LOW();
    hatchDisconnect->Set(DoubleSolenoid::Value::kOff);
}

void Robot::TeleopPeriodic() {
    //Transmission control block.
    //Implements a bool control system that flips a bool shiftingSoloTest instantly after the if blocks are run, essentially running said if blocks once.
    //Function-side code flips isHighGear and assigns isHighGearReady bools for reuse and driver use.

    if(rightJoystick->GetRawButton(rightJoystickMap::GEAR_CHANGE) && shiftingSoloTest) {
		if(isHighGear) {
			SHIFT_HIGH();
		} else if(!isHighGear) {
			SHIFT_LOW();
		}
		shiftingSoloTest = false;
	}

    //Once shifting SoloTest flips in the block above, it waits for the user to release the button.
    //Because shifting SoloTest is checked in the first control loop and not in the second.
    //So, once the button is released shiftingSoloTest flips back ready for reuse.

	if(!rightJoystick->GetRawButton(rightJoystickMap::GEAR_CHANGE)) {
		shiftingSoloTest = true;
	}

//See description above, the following if blocks use the same logic.

    if(rightJoystick->GetRawButton(1) && REMOVETEMPhatchSoloTest) {
        if(REMOVETEMPisHatch) {
			REMOVE_THIS_HATCH_CLOSE();
            SmartDashboard::PutBoolean("Elevator Override", REMOVETEMPisHatchReady);
		} else if(!REMOVETEMPisHatch) {
			REMOVE_THIS_HATCH_CLOSE();
            SmartDashboard::PutBoolean("Elevator Override", REMOVETEMPisHatchReady);
		}
		REMOVETEMPhatchSoloTest = false;
    }
    if(!rightJoystick->GetRawButton(1)) {
        REMOVETEMPhatchSoloTest = true;
    }
}
void Robot::TestPeriodic() {
}

#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif