#include "DriveTrain.h"
#include "../Robotmap.h"
#include "../Commands/DriveTrain/Drive.h"

DriveTrain::DriveTrain() : Subsystem("DriveTrain") 
{
	m_motorLeft  = new Talon(DRIVE_MOTOR_LEFT);
	m_motorRight = new Talon(DRIVE_MOTOR_RIGHT);
	m_drive = new RobotDrive(m_motorLeft, m_motorRight);
	m_drive->SetSafetyEnabled(false);
	m_shifters = new DoubleSolenoid(DRIVE_SOLENOID_SHIFT_LO, DRIVE_SOLENOID_SHIFT_HI);
	m_gyro = new Gyro(DRIVE_GYRO);
	m_leftEncoder = new Encoder(DRIVE_LEFT_ENCODER_A, DRIVE_LEFT_ENCODER_B, false, Encoder::k4X);
	m_leftEncoder->SetDistancePerPulse(PI * WHEEL_DIAMETER / ENCODER_RESOLUTION);
	m_leftEncoder->SetMinRate(MIN_RATE);
	m_leftEncoder->SetSamplesToAverage(SAMPLES_AVG);
    StartEncoder();
	LiveWindow::GetInstance()->AddSensor("Drive Train", "Gyro", m_gyro);
	LiveWindow::GetInstance()->AddSensor("Drive Train", "Encoder", m_leftEncoder);
	LiveWindow::GetInstance()->AddActuator("Drive Train", "Left Motor", m_motorLeft);
	LiveWindow::GetInstance()->AddActuator("Drive Train", "Right Motor", m_motorRight);
}
    
void DriveTrain::InitDefaultCommand() 
{
	// Set the default command for a subsystem here.
	SetDefaultCommand(new Drive());
}


// Put methods for controlling this subsystem
// here. Call these from Commands.

void DriveTrain::DriveArcade(Joystick * stick) 
{
	m_drive->ArcadeDrive(stick);
}

void DriveTrain::DriveArcadeAuto(float speed, float rotation) 
{
	m_drive->ArcadeDrive(speed, rotation);
}

void DriveTrain::DriveTankDrive(float leftValue, float rightValue) 
{
	m_drive->TankDrive(leftValue, rightValue);
}

void DriveTrain::DriveXbox(XboxController * xbox) 
{
	m_drive->ArcadeDrive(xbox->GetY(), xbox->GetX());
}

void DriveTrain::ShiftHi() 
{
	m_shifters->Set(DoubleSolenoid::kForward);
}

void DriveTrain::ShiftLo() 
{
	m_shifters->Set(DoubleSolenoid::kReverse);
}

float DriveTrain::GetGyroAngle()
{
	return m_gyro->GetAngle();
}

void DriveTrain::GyroReset()
{
	m_gyro->Reset();
}

void DriveTrain::XboxTankDrive(XboxController * xbox) 
{
	m_drive->TankDrive(-xbox->GetLeftJoyY(), -xbox->GetRightJoyY());
}


void DriveTrain::XboxPaulDrive(XboxController * xbox) 
{
	m_drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
	m_drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
	m_drive->ArcadeDrive(xbox, 2, xbox, 4);
}

int32_t DriveTrain::GetEncoder()
{
	return m_leftEncoder->Get();
}

void DriveTrain::StartEncoder()
{
	m_leftEncoder->Start();
}

void DriveTrain::StopEncoder()
{
	m_leftEncoder->Stop();
}

void DriveTrain::ResetEncoder() 
{
	m_leftEncoder->Reset();
}

double DriveTrain::GetDistance() 
{
	return m_leftEncoder->GetDistance();
}	

bool DriveTrain::GetDirection() 
{
	return m_leftEncoder->GetDirection();	
}

double DriveTrain::GetRate()
{
	return m_leftEncoder->GetRate();
}

bool DriveTrain::GetStopped()
{
	return m_leftEncoder->GetStopped();
}
