/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.GearTooth;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.highlevel.H_DriveWithJoystick;

/**
 *
 * @author team1987
 */
public class Chassis extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    //RobotDrive drive;
    Victor m_rightMotor = new Victor(RobotMap.RIGHT_DRIVE_MOTOR_PORT);
    Victor m_leftMotor = new Victor(RobotMap.LEFT_DRIVE_MOTOR_PORT);
    //DoubleSolenoid m_shifter = new DoubleSolenoid(RobotMap.SHIFTER_PORT1, RobotMap.SHIFTER_PORT2);
    Solenoid m_shifter1 = new Solenoid(RobotMap.SHIFTER_PORT1);
    Solenoid m_shifter2 = new Solenoid(RobotMap.SHIFTER_PORT2);
    GearTooth m_rightEncoder = new GearTooth(RobotMap.RIGHT_ENCODER_PORT);
    GearTooth m_leftEncoder = new GearTooth(RobotMap.LEFT_ENCODER_PORT);
    private boolean shifted = false;
    
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new H_DriveWithJoystick()); //set the default command
    }
    
    public Chassis(){
    }
    
    public void straight(int dir){
        if(dir == -1){
            m_leftMotor.set(RobotMap.LEFT_STRAIGHT_MOTOR_SPEED);
            m_rightMotor.set(-RobotMap.RIGHT_STRAIGHT_MOTOR_SPEED);
        }             
        else if(dir == 1){
            m_leftMotor.set(-RobotMap.LEFT_STRAIGHT_MOTOR_SPEED);
            m_rightMotor.set(RobotMap.RIGHT_STRAIGHT_MOTOR_SPEED);
        }
        
             
        //drives the robot straight
    }
    
    public void turn(int dir){
        //turns the robot a certain amount
        if(dir == -1){
            m_leftMotor.set(-RobotMap.LEFT_STRAIGHT_MOTOR_SPEED);
            m_rightMotor.set(-RobotMap.RIGHT_STRAIGHT_MOTOR_SPEED);
        }             
        else if(dir == 1){
            m_leftMotor.set(RobotMap.LEFT_STRAIGHT_MOTOR_SPEED);
            m_rightMotor.set(RobotMap.RIGHT_STRAIGHT_MOTOR_SPEED);
        }
    }
    
    public void stop() {
        m_leftMotor.set(0);
        m_rightMotor.set(0);
    }
    
    public void toggleFront(){
        if (RobotMap.normalFront == true){
            RobotMap.normalFront = false;
            RobotMap.frontToggled = true;
        }
        else if(RobotMap.normalFront == false){
            RobotMap.normalFront = true;
            RobotMap.frontToggled = true;
        }
    }
    
    
    public void setMotors(Joystick stick){
        double leftMotorSpeed = 0.0;
        double rightMotorSpeed = 0.0;
        if (RobotMap.normalFront == true){
            leftMotorSpeed = -(-stick.getAxis(Joystick.AxisType.kY) + stick.getAxis(Joystick.AxisType.kX) + stick.getAxis(Joystick.AxisType.kZ));
            rightMotorSpeed = (-stick.getAxis(Joystick.AxisType.kY) - stick.getAxis(Joystick.AxisType.kX) - stick.getAxis(Joystick.AxisType.kZ));
        }
        else if (RobotMap.normalFront == false){
            leftMotorSpeed = (-stick.getAxis(Joystick.AxisType.kY) + stick.getAxis(Joystick.AxisType.kX) + stick.getAxis(Joystick.AxisType.kZ));
            rightMotorSpeed = -(-stick.getAxis(Joystick.AxisType.kY) - stick.getAxis(Joystick.AxisType.kX) - stick.getAxis(Joystick.AxisType.kZ)); 
        }
        //SmartDashboard.putDouble("X:", stick.getAxis(Joystick.AxisType.kX));
        //SmartDashboard.putDouble("Y:", stick.getAxis(Joystick.AxisType.kY));
        //SmartDashboard.putDouble("Z:", stick.getAxis(Joystick.AxisType.kZ));
        
        //System.out.println("L Motor: " + leftMotorSpeed + " R Motor: " + rightMotorSpeed);
        
        m_leftMotor.set(leftMotorSpeed);
        m_rightMotor.set(rightMotorSpeed);
    }
    
    public void shift() { 
        shifted = false;
        if(m_shifter1.get() == false) {
            m_shifter1.set(true);
            m_shifter2.set(false);
            shifted = true;
        }
        else if (m_shifter1.get() == true) {
            m_shifter1.set(false);
            m_shifter2.set(true);
            shifted = true;
        }
    }
    
    public boolean getShifters() {
        return m_shifter1.get();
    }
    
    public boolean shifted() {
        return shifted;
    }
    
    public int getLeftCount() {
        return m_leftEncoder.get();
    }
    
    public int getRightCount(){
        return m_rightEncoder.get();
    }
}
