/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2014. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj;

import static org.junit.Assert.*;

import java.util.concurrent.Delayed;
import java.util.logging.Logger;

import org.junit.After;
import org.junit.AfterClass;
import org.junit.Before;
import org.junit.BeforeClass;
import org.junit.Test;

import edu.wpi.first.wpilibj.test.AbstractComsSetup;

/**
 * @author Kacper Puczydlowski
 *
 */

public class PCMTest extends AbstractComsSetup {
	private static final Logger logger = Logger.getLogger(PCMTest.class.getName());
	/* The PCM switches the compressor up to 2 seconds after the pressure switch
	changes. */
	protected static final double kCompressorDelayTime = 2.0;

	/* Solenoids should change much more quickly */
	protected static final double kSolenoidDelayTime = 1.0;

	/* The voltage divider on the test bench should bring the compressor output
	to around these values. */
	protected static final double kCompressorOnVoltage = 5.00;
	protected static final double kCompressorOffVoltage = 1.68;

	private static Compressor compressor;

	private static  DigitalOutput fakePressureSwitch;
	private static AnalogInput fakeCompressor;

	private static  DigitalInput fakeSolenoid1, fakeSolenoid2;

	@BeforeClass
	public static void setUpBeforeClass() throws Exception {
	    compressor = new Compressor();

	    fakePressureSwitch = new DigitalOutput(11);
	    fakeCompressor = new AnalogInput(1);

	    fakeSolenoid1 = new DigitalInput(12);
	    fakeSolenoid2 = new DigitalInput(13);
	}

	@AfterClass
	public static void tearDownAfterClass() throws Exception {
		compressor.free();

		fakePressureSwitch.free();
		fakeCompressor.free();

		fakeSolenoid1.free();
		fakeSolenoid2.free();
	}

	@Before
	public void setUp() throws Exception {
	}

	@Before
	public void reset() throws Exception {
		compressor.stop();
	    fakePressureSwitch.set(false);
	}

	@After
	public void tearDown() throws Exception {
	}

	/**
	 * Test if the compressor turns on and off when the pressure switch is toggled
	 */
	@Test
	public void testPressureSwitch() throws Exception {
		double range = 0.1;
		reset();
		compressor.setClosedLoopControl(true);

		// Turn on the compressor
		fakePressureSwitch.set(true);
		Timer.delay(kCompressorDelayTime);
		assertEquals("Compressor did not turn on when the pressure switch turned on.",
			kCompressorOnVoltage, fakeCompressor.getVoltage(), range);

		// Turn on the compressor
		fakePressureSwitch.set(false);
		Timer.delay(kCompressorDelayTime);
		assertEquals("Compressor did not turn off when the pressure switch turned off.",
			kCompressorOffVoltage, fakeCompressor.getVoltage(), range);
	}

	/**
	 * Test if the correct solenoids turn on and off when they should
	 */
	@Test
	public void testSolenoid() throws Exception {
		reset();

		Solenoid solenoid1 = new Solenoid(0);
		Solenoid solenoid2 = new Solenoid(1);

		solenoid1.set(false);
		solenoid2.set(false);
		Timer.delay(kSolenoidDelayTime);
		assertTrue("Solenoid #1 did not turn on",fakeSolenoid1.get());
		assertTrue("Solenoid #2 did not turn off",fakeSolenoid2.get());

		// Turn Solenoid #1 on, and turn Solenoid #2 off
		solenoid1.set(true);
		solenoid2.set(false);
		Timer.delay(kSolenoidDelayTime);
		assertFalse("Solenoid #1 did not turn on",fakeSolenoid1.get());
		assertTrue("Solenoid #2 did not turn off",fakeSolenoid2.get());

		// Turn Solenoid #1 off, and turn Solenoid #2 on
		solenoid1.set(false);
		solenoid2.set(true);
		Timer.delay(kSolenoidDelayTime);
		assertTrue("Solenoid #1 did not turn off",fakeSolenoid1.get());
		assertFalse("Solenoid #2 did not turn on",fakeSolenoid2.get());

		// Turn both Solenoids on
		solenoid1.set(true);
		solenoid2.set(true);
		Timer.delay(kSolenoidDelayTime);
		assertFalse("Solenoid #1 did not turn on",fakeSolenoid1.get());
		assertFalse("Solenoid #2 did not turn on",fakeSolenoid2.get());

		solenoid1.free();
		solenoid2.free();
	}

	/**
	* Test if the correct solenoids turn on and off when they should when used
	* with the DoubleSolenoid class.
	*/
	@Test
	public void doubleSolenoid() {
		DoubleSolenoid solenoid = new DoubleSolenoid(0, 1);

		solenoid.set(DoubleSolenoid.Value.kOff);
		Timer.delay(kSolenoidDelayTime);
		assertTrue("Solenoid #1 did not turn off", fakeSolenoid1.get());
		assertTrue("Solenoid #2 did not turn off", fakeSolenoid2.get());

		solenoid.set(DoubleSolenoid.Value.kForward);
		Timer.delay(kSolenoidDelayTime);
		assertFalse("Solenoid #1 did not turn on", fakeSolenoid1.get());
		assertTrue("Solenoid #2 did not turn off", fakeSolenoid2.get());

		solenoid.set(DoubleSolenoid.Value.kReverse);
		Timer.delay(kSolenoidDelayTime);
		assertTrue("Solenoid #1 did not turn off", fakeSolenoid1.get());
		assertFalse("Solenoid #2 did not turn on", fakeSolenoid2.get());

		solenoid.free();
	}

	protected Logger getClassLogger(){
		return logger;
	}
}
