package org.usfirst.frc.team4272.robot2015;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;

import java.io.FileWriter;
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 final HwRobot robot = new HwRobot();
	private final HwOI oi = new HwOI();
	private final Control control = new Control();
	private PrintWriter log;

	private Autonomous auto;
	private Teleop teleop;
	/**
	 * 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";
		}
	}

	public void autonomousInit() {
		auto = new Autonomous(robot);
	}
	public void autonomousPeriodic() {
		try {
			robot.run(auto.run(control));
			doLog();
		} catch (Exception e) {}
	}

	public void teleopInit() {
		teleop = new Teleop();
	}

	public void teleopPeriodic() {
		try {
			robot.run(teleop.run(control, oi));
			doLog();
		} catch (Exception e) {}
	}

	public void disabledInit() {
	}

	public void disabledPeriodic() {
	}

	/**
	 * This function is called periodically during test mode
	 */
	public void testPeriodic() {
	}
}