package org.usfirst.frc.team4272.robot2016;


import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import java.util.Date;
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 DriverStation ds = DriverStation.getInstance();
	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("/home/lvuser/tweeterbot/var/data.log", true));
		log.println("\ntime"
			+",ds:FMS,ds:alliance,ds:voltage,ds:FMStime"
			+",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 doLogOnce(String tag) throws Exception {
		log.println(new Date()
			+","+ds.isFMSAttached()
			+","+tag);
	}

	private void doLog() throws Exception {
		SmartDashboard.putNumber("dist", robot.rDriveE.getDistance());
		SmartDashboard.putNumber("raw", robot.rDriveE.getRaw());
		if (!ds.isFMSAttached())
			return;
		log.println(new Date()
			+","+ds.isFMSAttached()
			+","+ds.getAlliance()
			+","+ds.getBatteryVoltage()
			+","+ds.getMatchTime()
			+","+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() {
		try {
			auto = new Autonomous(robot);
			doLogOnce("autonomous");
		} catch (Exception e) {}
	}
	public void autonomousPeriodic() {
		try {
			robot.run(auto.run(control));
			doLog();
		} catch (Exception e) {}
	}

	public void teleopInit() {
		try {
			robot.rDriveE.reset();
			teleop = new Teleop();
			doLogOnce("teleop");
		} catch (Exception e) {}
	}

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

	public void disabledInit() {
		try {
			doLogOnce("disabled");
			log.flush();
		} catch (Exception e) {}
	}

	public void disabledPeriodic() {
	}

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