import nddsJava.*;
import Telemetry.*;
import tools.*;

public class posDerivedStateCallback extends nddsCallback {
  volatile static int lastN = 0;  // old north value
  volatile static int lastW = 0;  // old west value
  volatile static int lastU = 0;  // old up value
  volatile static boolean validP = false; // valid position

  volatile static long time = 0;  // last time
  volatile static boolean validT = false; // valid time
  
  // This constructor is required for any class that extends nddsCallback
  public posDerivedStateCallback(posDerivedState msg) {
    super(msg);
  }

  public void exec(Object o) {
    posDerivedState m = (posDerivedState) msg;
    NomadFrame nframe = (NomadFrame)o;

    // GPS position comes in as cm (int), passed as meters (double)
    nframe.setGPS(((double)m.posNorth)/100.0,  // x = north
		  ((double)m.posWest)/100.0,   // y = west   Compass
		  ((double)m.posUp)/100.0);    // z = up     Directions...
    nframe.setRobotSpeed((int)Math.sqrt(Math.pow((double)m.posNorthDot,2) +
					Math.pow((double)m.posWestDot,2) +
					Math.pow((double)m.deltaUpDot,2)));
    nframe.setHeading(m.rotZ);  // radians * 100
    nframe.setOdometer(distance(m.posNorth, m.posWest, m.posUp)); // cm
    Config.setOdometer(distance(m.posNorth, m.posWest, m.posUp)); // cm

    long t = ((long)m.week)*604800000 + (long)m.msecs; // change time to msecs
    if (validT) {
      nframe.setTime(t - time);
      Config.setPower(t - time);
      time = t;
    } else {
      time = t;
      validT = true;
    }
  }

  synchronized private long distance(int n, int w, int u) {
    // compute distance between (n,w,u) and old values (lastN, lastW, lastU)
    long result = (long)Math.sqrt(Math.pow(n - lastN, 2) +
				  Math.pow(w - lastW, 2) +
				  Math.pow(u - lastU, 2));
    if (!validP)  // first setting this time on
      { validP = true; lastN = n;  lastW = w;  lastU = u;}
    
    if (result < 100) {  // within 1m change... more is probably a bad reading
      lastN = n;  lastW = w;  lastU = u;
      return result;
    } else {
      System.err.println("ERROR: GPS giving incorrect values:");
      System.err.println("  old: ("+lastN+" cm, "+lastW+" cm, "+lastU+" cm)");
      System.err.println("  new: ("+n+" cm, "+w+" cm, "+u+" cm)");
      System.err.println("  change: "+result+" cm");
      return 0;          // result was probably a mistake, don't record values
    }
  }
}