/**
 * Copyright (c) 2011-2012, 2015, 2017 Luke Shumaker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the FIRST nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @author Luke Shumaker <lukeshu@sbcglobal.net>
 */
package org.usfirst.frc.team4272.robotlib;

import edu.wpi.first.wpilibj.PIDOutput;

/**
 * PIDOutputSplitter sets a single setpoint on many underlying {@link PIDOutput}s.
 *
 * Because it is undesirable that an exception from one underlying
 * PIDOutput prevent the others from receiving the value; rather than
 * letting an exception be thrown normally, it is handle by the
 * {@link error(Exception)} method--see the documentation there for
 * advanced error handling.
 */
public class PIDOutputSplitter implements PIDOutput {
	private final PIDOutput[] outputs;
	private final double[] scalars;

	/**
	 * Construct with a scalar for each underlying output.  If the
	 * length of the arrays do not match, a
	 * {@link RuntimeException} is thrown.
	 *
	 * @param outputs The underlying PIDOutputs that are written
	 *        to.
	 * @param scalars How much to scale the setpoint written to
	 *        each output by.
	 */
	public PIDOutputSplitter(PIDOutput[] outputs, double[] scalars) {
		this.outputs = outputs;
		this.scalars = scalars;
		if (outputs.length != scalars.length) {
			throw new RuntimeException("outputs and scalars must be same len");
		}
	}

	/**
	 * Construct a PIDOutputSplitter that applies no scaling.
	 *
	 * @param outputs The underlying PIDOutputs that are written
	 *        to.
	 */
	public PIDOutputSplitter(PIDOutput... outputs) {
		double[] s = new double[outputs.length];
		for (int i=0; i<s.length; i++) {
			s[i]=1;
		}
		this.outputs = outputs;
		this.scalars = s;
	}

	public void pidWrite(double output) {
		for(int i=0; i<outputs.length; i++) {
			try {
				outputs[i].pidWrite(output*scalars[i]);
			} catch (Exception e) {
				error(e);
			}
		}
	}

	/**
	 * Handles exceptions from pidWrite().
	 *
	 * By default it writes to System.out; you may extend+override this to
	 * change the behavior.
	 *
	 * @param e Exception
	 */
	public void error(Exception e) {
		e.printStackTrace();
	}
}