package no.marintek.control;

import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.PrintStream;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.Set;

import no.marintek.generic.control.IController;
import no.marintek.generic.control.Parameters;
import no.marintek.generic.control.Signal;
import no.marintek.generic.control.SignalEntity;


/*************************************************************************
 *
 *   Example PID winch controller for SIMO with input dumping for testing
 *
 *   @version: 1.0
 *   @author Pl Levold
 *
 *
 * ***********************************************************************/
public class WinchController implements IController {

	private double zSet;
	private double err, previousErr, errDiff, errInt;
	private double Kp, Ki, Kd, u, uMax, dt;
	private double[] runSpeed_arr = new double[1];


	private boolean logValues = false;
	private PrintStream stream;
	private int logInterval = 1;
	private int stepsSinceLog = 0;

	private double z;
	private boolean hasCrossedZero = false;

	private double x0, y0, z0;
	private String bodyID;
	String format = "  % 1.4e"; //Number formatter


	/**
	 * Initialize controller, called at startup
	 *
	 * @param dt  Time step
	 * @param parameters  parameters sent from SIMO
	 */
	@Override
	public void init(double dt, Parameters parameters) {
		this.dt = dt;

		if (parameters.getIntNames().contains("logInterval")) {
			logInterval = parameters.getInt("logInterval");
		}

		if (parameters.getStringNames().contains("filename")) {
			if (logInterval > 0) {
				logValues = true;
				String filename = parameters.getString("filename");
				try {
					FileOutputStream fos = new FileOutputStream(filename);
					stream = new PrintStream(fos);
					System.out.println("Echoing to file: "+filename);
				} catch (FileNotFoundException e) {
					e.printStackTrace();
				}
			}
		}

		//Set point
		zSet = parameters.getReal("zSet");
		//Control system constants
		Kp = parameters.getReal("Kp");
		Ki = parameters.getReal("Ki");
		Kd = parameters.getReal("Kd");
		//Max run speed
		uMax = parameters.getReal("uMax");
		//Reference point on hanging body for calculating error
		x0 = parameters.getReal("x0");
		y0 = parameters.getReal("y0");
		z0 = parameters.getReal("z0");

		//Identifier of hanging body
		bodyID = parameters.getString("bodyID");

		err = 0.0;
		errDiff = 0.0;
		errInt = 0.0;
		previousErr = 0.0;
		u = 0.0;
		z = 0.0;

		if (logValues) {
			writeParameters(parameters);
		}

	} // end of init

	/**
	 * Called each time step
	 *
	 * @param t  Current simulation time
	 * @param measurements  Measurements from SIMO
	 * @param feedback  Feedback sent back to SIMO. Also contains the current values
	 *
	 */
	@Override
	public void step(double t, Signal measurements, Signal feedback)
	{
		//Log values
		if (logValues) {
			if (stepsSinceLog >= logInterval) {
				writeSignal(measurements);
				writeSignal(feedback);
			}
			stepsSinceLog++;
		}

		//Only find error on first step, but don't control
		if (t == 0.0) {
			previousErr = getErr(measurements);
			return;
		}

		//Find error, integral of error and differential of error
		err     = getErr(measurements);
		errInt  += (previousErr + err)*dt/2.0;
		errDiff = (err - previousErr) / dt;

		//Don't integrate error before first zero crossing.
		//This is to avoid accumulating a large errInt if
		//the simulation starts far away from the set point.
		if (!hasCrossedZero) {
			errInt = 0.0;
		}

		//Calculate run speed
		u = Kp*err + Kd*errDiff + Ki*errInt;
		//Ensure run speed is within limits
		if (u > uMax) {
			u = uMax;
		} else if (u < -uMax) {
			u = -uMax;
		}

		//Store run speed in array
		runSpeed_arr[0] = u;

		//Loop over all feedback entities available to control system.
		//If entity is a winch, set run speed as calculated
		Set<String> winches = feedback.getAvaliableEntities();
		for(String id : winches) {
			SignalEntity winch = feedback.getEntity(id);
			if(winch.getAvailableComponents().contains("runSpeed")) {
				winch.set("runSpeed", runSpeed_arr);
			}
		}

		//Check if set point is crossed for the first time
		if(err*previousErr < 0 && !hasCrossedZero) {
			hasCrossedZero = true;
		}

		//Store error for next step
		previousErr = err;
	}



	/**
	 * Called once after simulation is done
	 *
	 */
	@Override
	public void finish()
    {
		//Close logfile
		if (logValues) {
			stream.close();
		}
    }

	private void writeParameters(Parameters parameters) {
		stream.println("===== init() from external jar-file =====");
		stream.format("dt = "+format+"\n",dt);
		stream.println();

		stream.println("--- Integer parameters ---");
		List<String> names = new ArrayList<String>(parameters.getIntNames());
		Collections.sort(names);
		for(String name : names) {
			stream.println(name+": "+parameters.getInt(name));
		}
		stream.println();

		stream.println("--- Real number parameters ---");
		names = new ArrayList<String>(parameters.getRealNames());
		Collections.sort(names);
		for(String name : names) {
			stream.format("%s: "+format+"\n",name, parameters.getReal(name));
		}
		stream.println();

		stream.println("--- String parameters ---");
		names = new ArrayList<String>(parameters.getStringNames());
		Collections.sort(names);
		for(String name : names) {
			stream.println(name+": "+parameters.getString(name));
		}
		stream.println();

	}

	private void writeSignal(Signal signal) {
		List<String> entityIds = new ArrayList<String>(signal.getAvaliableEntities());
		Collections.sort(entityIds);
		for (String entityId : entityIds){
			SignalEntity entity = signal.getEntity(entityId);

			stream.println("--Entity: "+entityId);
			List<String> componentIds = new ArrayList<String>(entity.getAvailableComponents());
			Collections.sort(componentIds);
			for (String componentId : componentIds) {
				double[] values = entity.get(componentId);

				stream.println(" -Component: "+componentId);
				for(int i = 0; i < values.length; i++) {
					if(i > 0 && i%3 == 0) {
						stream.print("\n");
					}
					stream.format(format, values[i]);
				}
				stream.println();
			}
			stream.println();
		}

	}

	/**
	 * Method for calculating error
	 *
	 * @param measurements  Measurements from SIMO
	 */
	private double getErr(Signal measurements) {
		double[] T = measurements.getEntity(bodyID).get("T");
		double[] bodyPosition = measurements.getEntity(bodyID).get("position");
		z = bodyPosition[2] + T[2]*x0 + T[5]*y0 + T[8]*z0; //z = z_body + T_31*x0 + T_32*y0 + T_33*z0
		return z - zSet;
	}

}

