package org.usfirst.frc.team4272.robot2015;

import org.usfirst.frc.team4272.robotlib.Toggler;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Timer;

import java.io.File;
import java.io.FileWriter;
import java.io.BufferedWriter;
import java.io.IOException;
import java.io.PrintWriter;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	private HwRobot robot = new HwRobot();
	private HwOI oi;
	
	private  PrintWriter log;
	
	private class Control {
		double lDrive, rDrive, winch;
		Relay.Value lIntake, rIntake;
		boolean grab, push;
		public Control() {
			lDrive = rDrive = winch = 0;
			grab = push = false;
			lIntake = rIntake = Relay.Value.kOff;
		}
	}
	Control control = new Control();
	
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	try {
    		inLog();
    	} catch (Exception e) {}
    }
    
    private void inLog() throws IOException {
    	log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
    	log.println("\ntime"
    			+",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
    			+",i:lDrive.dist,i:lDrive.rate"
    			+",i:rDrive.dist,i:rDrive.rate"
    			+",i:winch.dist");
    }
    
    private void doLog() throws Exception {
    	log.println(0
    			+","+control.lDrive
    			+","+control.rDrive
    			+","+control.winch
    			+","+fmtRelay(control.lIntake)
    			+","+fmtRelay(control.rIntake)
    			+","+control.grab
    			+","+control.push
    			+","+robot.lDriveE.getDistance()
    			+","+robot.lDriveE.getRate()
    			+","+robot.rDriveE.getDistance()
    			+","+robot.rDriveE.getRate()
    			+","+robot.winchE.getRate()
    			);
    }
    
    private String fmtRelay(Relay.Value v) {
    	if (v == Relay.Value.kForward) {
    		return "kForward";
    	} else if (v == Relay.Value.kReverse) {
    		return "kReverse";
    	} else if (v == Relay.Value.kOn) {
    		return "kOn";
    	} else if (v == Relay.Value.kOff) {
    		return "kOff";
    	} else {
    		return "unknown";
    	}
    }
    
    private void doRobot() {
    	try {
    		robot.lDriveM.pidWrite( control.lDrive);
    		robot.rDriveM.pidWrite(-control.rDrive);    	
    		robot.winchM.pidWrite(-control.winch);
    		robot.lIntake.set(control.lIntake);
    		robot.rIntake.set(control.rIntake);
    		robot.grab.setForward(control.grab);
    		robot.push.setForward(!control.push);
    		doLog();
    	} catch (Exception e) {}
    }

    private Timer autotime = new Timer();
    public void autonomousInit() {
    	autotime.reset();
    	autotime.start();
    	robot.rDriveE.reset();
    	robot.lDriveE.reset();
    }
    public void autonomousPeriodic() {
    	if (/*autotime.get() < 4*/robot.rDriveE.getDistance() < (15.5*140*1.1)) {
    		control.rDrive = control.lDrive = -0.7;
    	} else {
    		control.rDrive = control.lDrive = 0;
    	}
    	doRobot();
    }

    private Toggler grabButton = new Toggler();
    private Toggler pushButton = new Toggler();
    private Toggler ledsButton = new Toggler();
    public void teleopInit() {
    	//robot.lDrive.reset();
    	//robot.rDrive.reset();
    	//robot.winch.reset();
    	//robot.lDrive.enable();
    	//robot.rDrive.enable();
    	//robot.winch.enable();
    	robot.grab.setEnabled(true);
    	robot.push.setEnabled(true);
    	oi = new HwOI();
    	grabButton.update(false);
    	pushButton.update(false);
    	ledsButton.update(false);
    }
    
    private static double jsScale(Joystick j) {
    	double y = -j.getY();/* +:forward; -:backward */
    	double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
    	return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
    }
    
    public void teleopPeriodic() {
    	/* This runs at 50Hz */

    	/* Drive */
    	control.lDrive = jsScale(oi.lStick);
    	control.rDrive = jsScale(oi.rStick);
    	/* Winch */
    	control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */
    	if (Math.abs(control.winch) < 0.1) { control.winch = 0; }
    	
    	SmartDashboard.putNumber("winch M", -control.winch);
    	SmartDashboard.putNumber("winch E", robot.winchE.get());
    	SmartDashboard.putBoolean("winch T", robot.winchT.get());
    	SmartDashboard.putBoolean("winch B", robot.winchB.get());
    	
    	/* left intake */
    	if (oi.xbox.getButton(Button.LB)) {
    		control.lIntake = Relay.Value.kReverse;
    	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
    		control.lIntake = Relay.Value.kForward;
    	} else {
    		control.lIntake = Relay.Value.kOff;
    	}
    	  	
    	/* right intake */
    	if (oi.xbox.getButton(Button.RB)) {
    		control.rIntake=(Relay.Value.kReverse);
    	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
    		control.rIntake=(Relay.Value.kForward);
    	} else {
    		control.rIntake=(Relay.Value.kOff);
    	}
    	
    	control.grab = grabButton.update(oi.xbox.getButton(Button.A));
    	control.push = pushButton.update(oi.xbox.getButton(Button.B));

    	doRobot();
    }
    
    public void disabledInit() {
    	//robot.lDrive.reset();
    	//robot.rDrive.reset();
    	//robot.winch.reset();
    	robot.grab.setEnabled(false);
    	robot.push.setEnabled(false);
    }
    
    public void disabledPeriodic() {
    	
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    
    }
    
}