package edu.berkeley.c3uv.trackers.impl;

import java.util.Map;

import org.apache.log4j.Logger;
import org.joda.time.Instant;

import edu.berkeley.c3uv.PPLI;
import edu.berkeley.c3uv.Path;
import edu.berkeley.c3uv.Point;
import edu.berkeley.c3uv.TelemetryStore;
import edu.berkeley.c3uv.math.Geo;
import edu.berkeley.c3uv.trackers.SensorTracker;

public class LeadingSensorPoint extends SensorTracker {
	private static Logger logger = Logger.getLogger(LeadingSensorPoint.class);

	private final double timeStep;
	private final double distanceThreshold;
	private final double leadDistance;
	
	private long lastSendingTime = 0;
	private double leadAngle = 0;

	public LeadingSensorPoint(TelemetryStore store,Map<String, String> properties) {
		super(store,properties);
		
		String timeStepString = properties.get("Tracker.Sensor.Lead.TimeStep");
		if (timeStepString!=null && !timeStepString.isEmpty()) {
			timeStep = Double.parseDouble(timeStepString);
		} else {
			timeStep = 1;
		}
		
		String distanceThresholdString = properties.get("Tracker.Sensor.Lead.DistanceThreshold");
		if (distanceThresholdString!=null && !distanceThresholdString.isEmpty()) {
			distanceThreshold = Double.parseDouble(distanceThresholdString);
		} else {
			distanceThreshold = 200;
		}
		
		String leadDistanceThresholdString = properties.get("Tracker.Sensor.Lead.LeadDistance");
		if (leadDistanceThresholdString!=null && !leadDistanceThresholdString.isEmpty()) {
			leadDistance = Double.parseDouble(leadDistanceThresholdString);
		} else {
			leadDistance = 50;
		}
	}

	@Override
	public Point getSensorTarget(Path path) {
		if (path == null || path.getWaypoints().isEmpty()) {
			return null;
		}
		if (new Instant().getMillis() - lastSendingTime <= minSendingInterval) {
//			System.out.println("SKIPPING");
			return null;
		}
		
		if (telemStore.getTelemetry() == null 
				|| telemStore.getTelemetry().getPpli() == null
				|| telemStore.getTelemetry().getPpli().getPoint() == null) {
			logger.warn("No telemetry");
			return path.getWaypoints().get(0);
		}

		double altitude;
		if (telemStore.getTelemetry() != null 
				&& telemStore.getTelemetry().getPpli() != null
				&& telemStore.getTelemetry().getPpli().getPoint() != null) {
			altitude = telemStore.getTelemetry().getPpli().getPoint().getAltitudeMSL()
				- telemStore.getTelemetry().getPpli().getAltitudeAGL();
		} else {
			altitude = path.getWaypoints().get(0).getAltitudeMSL();
		}
		
		PPLI ppli = telemStore.getTelemetry().getPpli();
		Point aircraftPoint = ppli.getPoint();
		Point myLocation = new Point(aircraftPoint.getLatitude(),
				aircraftPoint.getLongitude(),altitude,
				aircraftPoint.getCircularError(),aircraftPoint.getLinearError());
		
		double groundSpeed = ppli.getGroundSpeed();
		double turnRate = 9.81/groundSpeed * Math.tan(ppli.getAttitude().getRoll());
		double zeta = ppli.getGroundTrack() + ppli.getAttitude().getYaw();
		double zetanew = zeta + turnRate*timeStep;
		
		double n = groundSpeed/turnRate * (Math.sin(zetanew) - Math.sin(zeta));
		double e = groundSpeed/turnRate * (Math.cos(zeta) - Math.cos(zetanew));
		Geo.NorthEastDown ned = new Geo.NorthEastDown(n,e,0);
		
//		Geo.DistBearing db = new Geo.DistBearing(ppli.getGroundSpeed()*timeStep,ppli.getGroundTrack());
		
		Point origin = Geo.nedToLatLon(ned,myLocation);

		// First, try the closest point to me one timestep in the future
		double minDist = 45000 * 1000; //greater than circumference of the earth
		int minIndex = 0;
		for (int i=path.getWaypoints().size()-1;i>=0;i--) {
			double dist = Geo.getDistance(origin, path.getWaypoints().get(i));
			if (dist < minDist) {
				minIndex = i;
				minDist = dist;
			}
		}
		
		// If the closest point then is too far away, fall back to closest to me now
		if (minDist > distanceThreshold) {
			//System.err.println("Using my point");
			origin = myLocation;
			minDist = 45000 * 1000; //greater than circumference of the earth
			minIndex = 0;
			for (int i=0;i<path.getWaypoints().size();i++) {
				double dist = Geo.getDistance(origin, path.getWaypoints().get(i));
				if (dist < minDist) {
					minIndex = i;
					minDist = dist;
				}
			}
		} else {
			//System.err.println("Using lead point");
		}

		Point target;
		
		// If still too far away, look a little bit ahead
		if (minDist <= distanceThreshold) {
			target = path.getWaypoints().get(minIndex);
		} else {
			//System.err.println("Too far away");
			double rollFactor = ppli.getAttitude().getRoll() / (Math.PI/6.);
			//System.out.println("roll factor: "+rollFactor);
			Geo.DistBearing db2 = new Geo.DistBearing(leadDistance,ppli.getGroundTrack()+leadAngle*rollFactor);
			target = Geo.bearingDistToLatLon(myLocation, db2);
		}
		
		Geo.DistBearing finalDb = Geo.latLonToDistBearing(myLocation, target);
		logger.trace("\n Me: "+myLocation+" Target: "+target + "\n" +
				" Direction: "+Math.toDegrees(ppli.getGroundTrack())+" DB: "+finalDb);

		//TODO: remove
//		logger.warn("Setting SensorTracker.target");
		SensorTracker.setTarget(target);

		lastSendingTime = new Instant().getMillis();
		return target;
	}
}