#include "WPILib.h"
#include "Shooter.h"
#include <ctime>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sys/time.h>
#include <vector>
#include <cmath>

#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT

#define TRIGGER 	1  	// Trigger button number
#define THUMB 		2	// Thumb button number
#define RAMP_RAISE  3   // Button 3 for Raising Ramp
#define RAMP_LOWER  4	// Button 4 to lower ramp.

#endif  // BUTTON_LAYOUT

#ifndef LOG
#define LOG(X) logA << X; logB << X; logC << X //for writing data to the csv file
#endif

class Robot: public IterativeRobot
{
	Talon left_drive, right_drive;
	CANTalon shooter1, shooter2,
			ramp;
	RobotDrive drive;
	Shooter shooter;
	Joystick driver_stick, operator_stick;
	float power;
public:
	Robot():
		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
		right_drive(1),			// Right DriveTrain Talons plug  	// left wheel 2
		shooter1(11),  			// shooter drive 1
		shooter2(10),   		// shooter drive 2
		ramp(12),
		drive(&left_drive, &right_drive),
		shooter(				// initialize Shooter object.
				&shooter1, &shooter2, &ramp),
		driver_stick(0), 				// right stick (operator)
		operator_stick(1)				// left stick (driver)
	{

	}

private:
	// instance variables
	bool pickupRunning;  // don't want to spam the Talon with set messages.  Toggle the pickup when a button is pressed or released.
	bool inverting;

	LiveWindow *lw = LiveWindow::GetInstance();
	SendableChooser *chooser;
	const std::string autoNameDefault = "Default";
	const std::string autoNameCustom = "My Auto";
	std::string autoSelected;

	void LogData()
	{
		static PowerDistributionPanel pdp;
		static DriverStation* ds = DriverStation::GetInstance();
		static DriverStation* ds = DriverStation::GetInstance();
		static std::vector<CANTalon*> motors;

		static std::ofstream logA, logB, logC;
		timeval tm;

		SmartDashboard::PutBoolean("log A", logA.is_open());
		SmartDashboard::PutBoolean("log B", logB.is_open());
		SmartDashboard::PutBoolean("log C", logC.is_open());
	}

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		ramp.Enable();
		shooter1.Enable();
		shooter2.Enable();
		left_drive.SetInverted(true);
		right_drive.SetInverted(true);
		inverting = false;

	}


	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		shooter.CalibrateRamp();
		shooter.SetRamp(Shoot);

		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	void AutonomousPeriodic()
	{
		LogCSVData();
		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	void TeleopInit()
	{
		shooter.CalibrateRamp();
		shooter.SetRamp(Shoot); // start in the full up position!
		power = 0;
	}

	void TeleopPeriodic()
	{
		LogCSVData();
		std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl;
		drive.ArcadeDrive(&driver_stick, true);

		//  This is shit code for testing.  Replace it with real code.
		if(operator_stick.GetRawButton(RAMP_RAISE))
		{
			ramp.Set(1);
		}
		else if(operator_stick.GetRawButton(RAMP_LOWER))
		{
			ramp.Set(-1);
		}
		else
		{
			ramp.Set(0);
		}

		if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
		{
			shooter.PickUp();
			pickupRunning = true;
		}
		else if(pickupRunning)
		{
			shooter.PickUp(false);
			pickupRunning = false;
		}

		if(driver_stick.GetRawButton(THUMB) && !inverting)
		{
			left_drive.SetInverted(!left_drive.GetInverted());
			right_drive.SetInverted(!right_drive.GetInverted());
			inverting = true;
		}
		else if(!driver_stick.GetRawButton(THUMB))
		{
			inverting = false;
		}

		if(((1.0 - operator_stick.GetThrottle()) / 2.0) > power + 0.005||((1.0 - operator_stick.GetThrottle()) / 2.0) < power -0.005)
		{
			power = (1.0 - operator_stick.GetThrottle()) / 2.0;
			shooter.SetPower(power);
		}
	}


	void TestPeriodic()
	{
		lw->Run();
		LogCSVData();
	}
	void LogCSVData()
	{
		static PowerDistributionPanel pdp;	// preparing to read from the pdp
		static DriverStation* ds = DriverStation::GetInstance();
		static std::vector<CANTalon*> motors;

		static std::ofstream logA, logB, logC;
		timeval tm;

		SmartDashboard::PutBoolean("log A", logA.is_open());
		SmartDashboard::PutBoolean("log B", logB.is_open());
		SmartDashboard::PutBoolean("log C", logC.is_open());

		if (!logA.is_open() && !logB.is_open() && !logC.is_open())
		{
			std::fstream logNumFile;
			int logNum;
			logNumFile.open("/home/lvuser/logNum");
			logNumFile >> logNum;
			logNum++;
			logNumFile.seekp(0);
			logNumFile << logNum;
			// writing to /home/lvuser/logs/[unixtime].log
			logA.open("/media/sda1/logs/log" + std::to_string(logNum) + ".csv");
			std::cerr << (logA.is_open() ? "Opened" : "Failed to open") << "log A." << std::endl;
			logB.open("/media/sdb1/logs/log" + std::to_string(logNum) + ".csv");
			std::cerr << (logB.is_open() ? "Opened" : "Failed to open") << "log B." << std::endl;
			logC.open("/home/lvuser/logs/log" + std::to_string(logNum) + ".csv");
			std::cerr << (logC.is_open() ? "Opened" : "Failed to open") << "log C." << std::endl;

			LOG("Time\tpdpInput voltage\tpdpTemperature\tpdpTotal Current\t");
			for (int ii = 0; ii < 16; ii++)
			{
				LOG("pdpChannel " << ii << " current\t");
			}

			LOG("tlaunch_spinny Bus Voltage\ttlaunch_spinny Output Current\ttlaunch_spinny Output Voltage\ttlaunch_spinny Temperature");
			motors.push_back(&ramp);
			LOG("\tShooter1 Bus Voltage\tShooter1 Output Current\tShooter1 Output Voltage\tShooter1 Temperature");
			motors.push_back(&shooter1);
			LOG("\tShooter2 Bus Voltage\tShooter2 Output Current\tShooter2 Output Voltage\tShooter2 Temperature");
			motors.push_back(&shooter2);

			LOG("\tJoystick X\tJoystick Y\tJoystick Twist");
			LOG("\tAlliance\tLocation\tMatch Time\tFMS Attached\tBrowned Out");
			LOG("\tTestStage");
			LOG(std::endl);
		}
		gettimeofday(&tm, NULL);
		LOG(time(0) << '.' << std::setfill('0') << std::setw(3) << tm.tv_usec/1000);
		// Some general information
		LOG("\t" << pdp.GetVoltage());
		LOG("\t" << pdp.GetTemperature());
		LOG("\t" << pdp.GetTotalCurrent());
		// current on each channel
		for (int ii = 0; ii < 16; ii++)
		{
			LOG("\t" << pdp.GetCurrent(ii));
		}

		//Talon Data
		for(int ii = 0; ii < motors.size(); ii++)
		{
			LOG("\t" << motors[ii]->GetBusVoltage());
			LOG("\t" << motors[ii]->GetOutputVoltage());
			LOG("\t" << motors[ii]->GetOutputCurrent());
			LOG("\t" << motors[ii]->GetTemperature());
		}
		//control data
		LOG("\t" << driver_stick.GetX());
		LOG("\t" << driver_stick.GetY());
		LOG("\t" << driver_stick.GetTwist());

		//DriverStation Data
		LOG("\t" << ds->GetAlliance());
		LOG("\t" << ds->GetLocation());
		LOG("\t" << ds->GetMatchTime());
		LOG("\t" << ds->IsFMSAttached());
		LOG("\t" << ds->IsSysBrownedOut());
		LOG(std::endl);
	}
};

START_ROBOT_CLASS(Robot)