/**
 *  This file is part of miniCDx benchmark of oSCJ.
 *
 *   miniCDx is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU Lesser General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   miniCDx is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU Lesser General Public License for more details.
 *
 *   You should have received a copy of the GNU Lesser General Public License
 *   along with miniCDx.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 *   Copyright 2009, 2010
 *   @authors  Daniel Tang, Ales Plsek, Kun Wei
 *
 *   See: http://sss.cs.purdue.edu/projects/oscj/
 */
package cdx;

/**
 * Objects of the <code>Motion</code> class provide all a-priori known
 * information about what the Aircraft was doing between two frames. Also
 * provides methods to do useful things with this object (so it's not just
 * dead data).
 *
 * @author Filip Pizlo
 */
class Motion {
  /**
   * The aircraft that we are referring to.
   */
  private final Aircraft aircraft;

  /**
   * The first position (from the last frame). Will be equal to
   * <code>pos_two</code> if we do not have a record for this aircraft for
   * any previous frames.
   */
  private final Vector3d pos_one;

  /**
   * The second position (from the current frame).
   */
  private final Vector3d pos_two;

  public Motion() {
    aircraft = new Aircraft();
    pos_one = new Vector3d();
    pos_two = new Vector3d();
  }

  /**
   * Initialise with an aircraft and two positions.
   */
  public Motion(Aircraft _aircraft, Vector3d _pos_one, Vector3d _pos_two) {
    aircraft = _aircraft;
    pos_one = _pos_one;
    pos_two = _pos_two;
  }

  /**
   * Initialise with an aircraft and one position.
   */
  public Motion(Aircraft _aircraft, Vector3d _pos) {
    this(_aircraft, _pos, _pos);
  }

  /**
   * Retrieve the aircraft.
   */
  public Aircraft getAircraft() {
    return aircraft;
  }

  /**
   * Retrieve the first position.
   */
  public Vector3d getFirstPosition() {
    return pos_one;
  }

  /**
   * Retrieve the second position.
   */
  public Vector3d getSecondPosition() {
    return pos_two;
  }

  public String toString() {
    return "Motion of " + getAircraft().toString() +
      " from " + getFirstPosition().toString() +
      " to " + getSecondPosition().toString();
  }

  public void copyfrom(
      Aircraft _aircraft, Vector3d _pos_one, Vector3d _pos_two) {

    for (int i = 0; i < aircraft.getCallsign().length; i++) {
      aircraft.getCallsign()[i]= _aircraft.getCallsign()[i];
    }

    pos_one.x = _pos_one.x;
    pos_one.y = _pos_one.y;
    pos_one.z = _pos_one.z;

    pos_two.x = _pos_two.x;
    pos_two.y = _pos_two.y;
    pos_two.z = _pos_two.z;
  }

  /**
   * Find an intersection between this Motion and another Motion.
   *
   * @return a Vector3d object with the intersection point if an intersection
   * was found, null otherwise.
   *
   * @author Jeff Hagelberg, Filip Pizlo
   */
  public Vector3d findIntersection(Motion other) {
    Vector3d i1 = new Vector3d(),
      f1 = new Vector3d(),
      i2 = new Vector3d(),
      f2 = new Vector3d();

    i1.set(getFirstPosition());
    f1.set(getSecondPosition());
    i2.set(other.getFirstPosition());
    f2.set(other.getSecondPosition());

    final float r = Constants.PROXIMITY_RADIUS;

    final float vx1 = f1.x - i1.x;
    final float vx2 = f2.x - i2.x;
    final float vy1 = f1.y - i1.y;
    final float vy2 = f2.y - i2.y;
    final float vz1 = f1.z - i1.z;
    final float vz2 = f2.z - i2.z;

    /* This test is not a geometrical 3d intersection test, it takes the fact
     * that the aircraft move into account; so it is more like a 4d test (it
     * assumes that both of the aircraft have a constant speed over the tested
     * interval). We thus have two points, each of them moving on its line
     * segment at constant speed; we are looking for times when the distance
     * between these two points is smaller than r.
     *
     * V1 is vector of aircraft 1
     * V2 is vector of aircraft 2
     *
     * if a = 0 iff the planes are moving in parallel and have the same speed
     * (can be zero - they may not be moving at all)
     *
     * a = (V2 - V1)^T * (V2 - V1) =< (V2 - V1), (V2 - V1) >=
     *   sqrt(|| V2 - V1 ||) */
    final float a =
      (vx2 - vx1) * (vx2 - vx1) +
      (vy2 - vy1) * (vy2 - vy1) +
      (vz2 - vz1) * (vz2 - vz1);

    if (a != 0.0f) {
      /* We are first looking for instances of time when the planes are exactly
       * r from each other and at least one plane is moving; if the planes are
       * moving in parallel and do not have constant speed, then if the faster
       * starts behind the slower, we can have 2, 1, or 0 solutions; if the
       * faster plane starts in front of the slower, we can have 0 or 1
       * solutions.
       *
       * point P1 = I1 + vV1
       * point P2 = I2 + vV2
       * - looking for v such that dist(P1,P2) = || P1 - P2 || = r
       *
       * it follows that || P1 - P2 || = sqrt( < P1-P2, P1-P2 > )
       * 0 = -r^2 + < P1 - P2, P1 - P2 >
       * from properties of dot product
       * 0 = -r^2 + <I1-I2,I1-I2> + v * 2<I1-I2, V1-V2> + v^2 *<V1-V2,V1-V2>
       * so we calculate a, b, c - and solve the quadratic equation
       * 0 = c + bv + av^2
       *
       * b = 2 * <I1-I2, V1-V2> */
      final float b = 2.0f *
        (i2.x * vx2 - i2.x * vx1 -
         i1.x * vx2 + i1.x * vx1 +
         i2.y * vy2 - i2.y * vy1 -
         i1.y * vy2 + i1.y * vy1 +
         i2.z * vz2 - i2.z * vz1 -
         i1.z * vz2 + i1.z * vz1);

      // c = -r^2 + (I2 - I1)^T * (I2 - I1)
      final float c = -r * r +
        (i2.x - i1.x) * (i2.x - i1.x) +
        (i2.y - i1.y) * (i2.y - i1.y) +
        (i2.z - i1.z) * (i2.z - i1.z);

      final float discr = b * b - 4.0f * a * c;
      if (discr < 0.0f) return null;

      /* the left side */
      final float v1 = (-b - (float) Math.sqrt(discr)) / (2.0f * a);
      /* the right side */
      final float v2 = (-b + (float) Math.sqrt(discr)) / (2.0f * a);

      // FIXME: v1 <= v2 always holds, correct ? because v1 > v2 only if
      // a < 0, which would mean <V1-V2,V1-V2> < 0, which is impossible.
      if (v1 <= v2 &&
        (v1 <= 1.0f && 1.0f <= v2 || v1 <= 0.0f && 0.0f <= v2 ||
         0.0f <= v1 && v2 <= 1.0f)) {
        // new: calculate the location of the collision; if it is
        // outside of the bounds of the Simulation, don't do anything!
        final float x1col = i1.x + vx1 * (v1 + v2) / 2.0f;
        final float y1col = i1.y + vy1 * (v1 + v2) / 2.0f;
        final float z1col = i1.z + vz1 * (v1 + v2) / 2.0f;
        if (z1col > Constants.MIN_Z && z1col <= Constants.MAX_Z &&
            x1col >= Constants.MIN_X && x1col <= Constants.MAX_X &&
            y1col >= Constants.MIN_Y && y1col <= Constants.MAX_Y)
          return new Vector3d(x1col, y1col, z1col);
      }
    }
    else {
      /* The planes have the same speeds and are moving in parallel (or they
       * are not moving at all) they thus have the same distance all the time;
       * we calculate it from the initial point
       *
       * dist = || i2 - i1 || = sqrt((i2 - i1)^T * (i2 - i1)) */
      float dist =
        (i2.x - i1.x) * (i2.x - i1.x) +
        (i2.y - i1.y) * (i2.y - i1.y) +
        (i2.z - i1.z) * (i2.z - i1.z);

      dist = (float) Math.sqrt(dist);

      //System.out.println("i1 = " + i1 + ", i2 = " + i2 + ", dist = " + dist);

      if (dist <= r)
        /*System.out.println(
          "Planes were travelling in parallel. Collision.");*/
        return getFirstPosition();
    }
    return null;
  }
}
