/**
  * Internal. Typically after a hit: Recompute the new curves and upcoming collission candidates.
  *
  * @param hitMask
  * @return bitmask of rocks with new curves
  */
 int doRecomputeCurvesAndCollissionTimes(
     final int hitMask, double t0, final RockSet<Pos> cp, final RockSet<Vel> cv) {
   int computedMask = 0;
   // first compute the new curves:
   // TUNE Parallel
   for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
     if (!RockSet.isSet(hitMask, i16)) continue;
     curveStore.add(i16, t0, doComputeCurve(i16, t0, cp, cv, NoSweep), _30);
     computedMask |= 1 << i16;
   }
   // then add all combinations of potential collissions
   t0 += hitDt;
   // TUNE Parallel
   for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
     if (!RockSet.isSet(computedMask, i16)) continue;
     for (int j16 = RockSet.ROCKS_PER_SET - 1; j16 >= 0; j16--) {
       if (i16 == j16 || i16 > j16 && RockSet.isSet(computedMask, j16)) continue;
       collissionStore.replace(
           i16,
           j16,
           collissionDetector.compute(
               t0, _30, curveStore.getCurve(i16), curveStore.getCurve(j16)));
     }
   }
   return computedMask;
 }
 /**
  * Internal. Compute one rock curve segment and don't change internal state.
  *
  * @param i16 which rock
  * @param t0 starttime
  * @param sweepFactor
  * @return the new Curve in world coordinates.
  */
 CurveRock<Pos> doComputeCurve(
     final int i16,
     final double t0,
     final RockSet<Pos> p,
     final RockSet<Vel> s,
     final double sweepFactor) {
   final Rock<Pos> x = p.getRock(i16);
   final Rock<Vel> v = s.getRock(i16);
   final CurveRock<Pos> wc;
   if (v.p().distanceSq(0, 0) == 0) wc = CurveStill.newInstance(x);
   else
     // Convert the initial angle from WC to RC.
     // TUNE 2x sqrt, 2x atan2 to 1x each?
     wc =
         new CurveTransformed<Pos>(
             curler.computeRc(
                 x.getA() + Math.atan2(v.getX(), v.getY()),
                 MathVec.abs2D(v.p()),
                 v.getA(),
                 sweepFactor),
             CurveTransformed.createRc2Wc(x.p(), v.p(), null),
             t0);
   if (log.isDebugEnabled()) log.debug(i16 + " " + wc);
   return wc;
 }
 /** One of the initial rocks changed either pos or vel */
 public void stateChanged(final ChangeEvent arg0) {
   final Object src = arg0 == null ? null : arg0.getSource();
   if (src == null || src == initialPos || src == initialSpeed) ; // recompute(currentTime, true);
   else if (initialPos.findI16(src) >= 0) {
     log.debug("Startpos rock change");
     if (getSuspended()) collected |= 1 << initialPos.findI16(src);
     else recompute(currentTime, true);
   } else if (initialSpeed.findI16(src) >= 0) {
     log.debug("Startvel rock change");
     if (getSuspended()) collected |= 1 << initialSpeed.findI16(src);
     else recompute(currentTime, true);
   } else log.info(arg0);
 }
 /**
  * Internal. Does not {@link RockSet#fireStateChanged()}!
  *
  * @param currentTime
  */
 void doUpdatePosAndVel(final double currentTime, final RockSet<Pos> cp, final RockSet<Vel> cv) {
   // TUNE Parallel
   for (int i = RockSet.ROCKS_PER_SET - 1; i >= 0; i--) {
     final R1RNFunction c = curveStore.getCurve(i);
     double x = c.at(currentTime, 0, 0);
     double y = c.at(currentTime, 0, 1);
     double a = c.at(currentTime, 0, 2);
     cp.getRock(i).setLocation(x, y, a);
     x = c.at(currentTime, 1, 0);
     y = c.at(currentTime, 1, 1);
     a = c.at(currentTime, 1, 2);
     cv.getRock(i).setLocation(x, y, a);
   }
 }
 /** Do NOT update currentXY */
 private void recomputeCurves(final int bitmask) {
   if (bitmask <= 0) return;
   for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
     if (!RockSet.isSet(bitmask, i16)) continue;
     recomputeCurve(i16);
   }
   doUpdatePosAndVel(currentTime, currentPos, currentVel);
 }
 @SuppressWarnings("unchecked")
 private static CompoundEdit reset(
     final ComputedTrajectorySet cts, final BroomPromptModel broom, final boolean outPosition) {
   final RockSet<Pos> ipos = cts.getInitialPos();
   final RockSet<Vel> ivel = cts.getInitialVel();
   // store the initial state:
   final PosMemento[] pm = new PosMemento[RockSet.ROCKS_PER_SET];
   for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--)
     pm[i16] = new PosMemento(ipos, i16, ipos.getRock(i16).p());
   final IndexMemento bi = new IndexMemento(broom, broom.getIdx16());
   final HandleMemento bh = new HandleMemento(broom, broom.getOutTurn());
   final XYMemento bxy = new XYMemento(broom, broom.getBroom());
   final SplitMemento bs = new SplitMemento(broom, broom.getSplitTimeMillis().getValue());
   final boolean preS = cts.getSuspended();
   cts.setSuspended(true);
   try {
     // reset:
     RockSet.allZero(ivel);
     broom.setIdx16(-1);
     if (outPosition) RockSetUtils.allOut(ipos);
     else RockSetUtils.allHome(ipos);
     broom.setIdx16(1);
     broom.setBroom(new Point2D.Double(0, 0));
     broom.getSplitTimeMillis().setValue(3300);
   } finally {
     cts.setSuspended(preS);
   }
   // create a compound edit
   final CompoundEdit ce = new CompoundEdit();
   ce.addEdit(new UndoableMemento(new SuspendMemento(cts, preS), new SuspendMemento(cts, true)));
   for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--)
     ce.addEdit(new UndoableMemento(pm[i16], new PosMemento(ipos, i16, ipos.getRock(i16).p())));
   ce.addEdit(new UndoableMemento(bi, new IndexMemento(broom, broom.getIdx16())));
   ce.addEdit(new UndoableMemento(bh, new HandleMemento(broom, broom.getOutTurn())));
   ce.addEdit(new UndoableMemento(bxy, new XYMemento(broom, broom.getBroom())));
   ce.addEdit(
       new UndoableMemento(bs, new SplitMemento(broom, broom.getSplitTimeMillis().getValue())));
   ce.addEdit(new UndoableMemento(new SuspendMemento(cts, true), new SuspendMemento(cts, preS)));
   ce.end();
   return ce;
 }
 public CurveManager() {
   initialPos.addRockListener(this);
   initialSpeed.addRockListener(this);
 }
/**
 * Bring it all together and trigger computation.
 *
 * <p>Registers itself as listener to {@link RockSet#addRockListener(ChangeListener)} for both -
 * initial rock locations and velocities to trigger recomputation.
 *
 * <p>TODO re-work the whole computation/update strategy to reduce the number of curves recomputed
 * for free rocks' moves.
 *
 * @author <a href="mailto:[email protected]">M. Rohrmoser </a>
 * @version $Id:CurveManager.java 682 2007-08-12 21:25:04Z mrohrmoser $
 */
public class CurveManager extends MutableObject
    implements ChangeListener, ComputedTrajectorySet, Serializable {

  // compute beyond a realistic amount of time
  private static final double _30 = 60.0;
  /** Time leap during a hit. */
  private static final double hitDt = 1e-6;

  private static final Log log = JCLoggerFactory.getLogger(CurveManager.class);
  private static final double NoSweep = 0;
  private static final long serialVersionUID = 7198540442889130378L;
  private static final StopDetector stopper = new NewtonStopDetector();
  private final Map<CharSequence, CharSequence> annotations =
      new HashMap<CharSequence, CharSequence>();
  /** Trajectory (Rock) Bitmask or -1 for "currently not suspended". */
  private transient int collected = -1;

  private Collider collider = null;
  private transient CollissionDetector collissionDetector = new BisectionCollissionDetector();
  private final transient CollissionStore collissionStore = new CollissionStore();
  private Curler curler = null;
  private final transient RockSet<Pos> currentPos = RockSetUtils.allHome();
  private transient double currentTime = 0;
  private final transient RockSet<Vel> currentVel = RockSet.allZero(null);
  private transient CurveStore curveStore = new CurveStoreImpl(stopper, RockSet.ROCKS_PER_SET);
  private final RockSet<Pos> initialPos = RockSetUtils.allHome();
  private final RockSet<Vel> initialSpeed = RockSet.allZero(null);
  // don't fire change events on intermediate updates. Use another variable.
  private final transient RockSet<Pos> tmpPos = RockSetUtils.allHome();
  // don't fire change events on intermediate updates. Use another variable.
  private final transient RockSet<Vel> tmpVel = RockSet.allZero(null);

  public CurveManager() {
    initialPos.addRockListener(this);
    initialSpeed.addRockListener(this);
  }

  /**
   * Internal. Compute one rock curve segment and don't change internal state.
   *
   * @param i16 which rock
   * @param t0 starttime
   * @param sweepFactor
   * @return the new Curve in world coordinates.
   */
  CurveRock<Pos> doComputeCurve(
      final int i16,
      final double t0,
      final RockSet<Pos> p,
      final RockSet<Vel> s,
      final double sweepFactor) {
    final Rock<Pos> x = p.getRock(i16);
    final Rock<Vel> v = s.getRock(i16);
    final CurveRock<Pos> wc;
    if (v.p().distanceSq(0, 0) == 0) wc = CurveStill.newInstance(x);
    else
      // Convert the initial angle from WC to RC.
      // TUNE 2x sqrt, 2x atan2 to 1x each?
      wc =
          new CurveTransformed<Pos>(
              curler.computeRc(
                  x.getA() + Math.atan2(v.getX(), v.getY()),
                  MathVec.abs2D(v.p()),
                  v.getA(),
                  sweepFactor),
              CurveTransformed.createRc2Wc(x.p(), v.p(), null),
              t0);
    if (log.isDebugEnabled()) log.debug(i16 + " " + wc);
    return wc;
  }

  /**
   * Internal.
   *
   * @return when is the next hit, which are the rocks involved.
   */
  Tupel doGetNextHit() {
    return collissionStore.first();
  }

  /**
   * Internal. Typically after a hit: Recompute the new curves and upcoming collission candidates.
   *
   * @param hitMask
   * @return bitmask of rocks with new curves
   */
  int doRecomputeCurvesAndCollissionTimes(
      final int hitMask, double t0, final RockSet<Pos> cp, final RockSet<Vel> cv) {
    int computedMask = 0;
    // first compute the new curves:
    // TUNE Parallel
    for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
      if (!RockSet.isSet(hitMask, i16)) continue;
      curveStore.add(i16, t0, doComputeCurve(i16, t0, cp, cv, NoSweep), _30);
      computedMask |= 1 << i16;
    }
    // then add all combinations of potential collissions
    t0 += hitDt;
    // TUNE Parallel
    for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
      if (!RockSet.isSet(computedMask, i16)) continue;
      for (int j16 = RockSet.ROCKS_PER_SET - 1; j16 >= 0; j16--) {
        if (i16 == j16 || i16 > j16 && RockSet.isSet(computedMask, j16)) continue;
        collissionStore.replace(
            i16,
            j16,
            collissionDetector.compute(
                t0, _30, curveStore.getCurve(i16), curveStore.getCurve(j16)));
      }
    }
    return computedMask;
  }

  /**
   * Internal. Does not {@link RockSet#fireStateChanged()}!
   *
   * @param currentTime
   */
  void doUpdatePosAndVel(final double currentTime, final RockSet<Pos> cp, final RockSet<Vel> cv) {
    // TUNE Parallel
    for (int i = RockSet.ROCKS_PER_SET - 1; i >= 0; i--) {
      final R1RNFunction c = curveStore.getCurve(i);
      double x = c.at(currentTime, 0, 0);
      double y = c.at(currentTime, 0, 1);
      double a = c.at(currentTime, 0, 2);
      cp.getRock(i).setLocation(x, y, a);
      x = c.at(currentTime, 1, 0);
      y = c.at(currentTime, 1, 1);
      a = c.at(currentTime, 1, 2);
      cv.getRock(i).setLocation(x, y, a);
    }
  }

  @Override
  public boolean equals(final Object obj) {
    return false;
  }

  public Map<CharSequence, CharSequence> getAnnotations() {
    return annotations;
  }

  public Collider getCollider() {
    return collider;
  }

  public CollissionDetector getCollissionDetector() {
    return collissionDetector;
  }

  public Curler getCurler() {
    return curler;
  }

  public RockSet<Pos> getCurrentPos() {
    return currentPos;
  }

  public double getCurrentTime() {
    return currentTime;
  }

  public RockSet<Vel> getCurrentVel() {
    return currentVel;
  }

  public CurveStore getCurveStore() {
    return curveStore;
  }

  public RockSet<Pos> getInitialPos() {
    return initialPos;
  }

  public RockSet<Vel> getInitialVel() {
    return initialSpeed;
  }

  public boolean getSuspended() {
    return collected >= 0;
  }

  @Override
  public int hashCode() {
    return 0;
  }

  protected Object readResolve() throws ObjectStreamException {
    final CurveManager m = new CurveManager();
    m.setSuspended(true);
    m.annotations.putAll(annotations);
    m.setCollider(getCollider());
    m.setCurler(getCurler());
    m.setInitialPos(getInitialPos());
    m.setInitialVel(getInitialVel());
    // m.setCurrentTime(this.getCurrentTime());
    m.setSuspended(false);
    return m;
  }

  private void recompute(final double currentTime, final boolean complete) {
    if (getSuspended()) return;
    if (complete) {
      {
        // initial
        final double t0 = 0.0;
        // TUNE Parallel
        // initial curves:
        for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
          curveStore.reset(i16);
          curveStore.add(i16, t0, doComputeCurve(i16, t0, initialPos, initialSpeed, NoSweep), _30);
        }
        // initial collission detection:
        collissionStore.clear();
        // TUNE Parallel
        for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--)
          for (int j16 = i16 - 1; j16 >= 0; j16--)
            // log.info("collissionDetect " + i + ", " + j);
            collissionStore.add(
                collissionDetector.compute(
                    t0, _30, curveStore.getCurve(i16), curveStore.getCurve(j16)),
                i16,
                j16);
      }
      final AffineTransform m = new AffineTransform();
      // NaN-safe time range check (are we navigating known ground?):
      while (currentTime > doGetNextHit().t) {
        final Tupel nh = doGetNextHit();
        if (log.isDebugEnabled()) log.debug(nh.a + " - " + nh.b + " : " + nh.t);
        doUpdatePosAndVel(nh.t, tmpPos, tmpVel);
        // compute collission(s);
        final int mask = collider.compute(tmpPos, tmpVel, m);
        if (mask == 0) break;
        doRecomputeCurvesAndCollissionTimes(mask, nh.t, tmpPos, tmpVel);
      }
    }
    doUpdatePosAndVel(currentTime, currentPos, currentVel);
  }

  /** Do NOT update currentXY */
  private void recomputeCurve(final int i16) {
    if (i16 < 0) return;
    // TODO find the first collission with this (old) curve involved from
    // the collissionstore.

    // TODO find the first collission with this (new) curve involved.

    // TODO if none -> return

    // TODO clear the collissionstore/curvestore until the earlier of the
    // two

  }

  /** Do NOT update currentXY */
  private void recomputeCurves(final int bitmask) {
    if (bitmask <= 0) return;
    for (int i16 = RockSet.ROCKS_PER_SET - 1; i16 >= 0; i16--) {
      if (!RockSet.isSet(bitmask, i16)) continue;
      recomputeCurve(i16);
    }
    doUpdatePosAndVel(currentTime, currentPos, currentVel);
  }

  public void setCollider(final Collider collider) {
    final Collider old = this.collider;
    if (old == collider) return;
    propChange.firePropertyChange("collider", old, this.collider = collider);
  }

  public void setCollissionDetector(final CollissionDetector collissionDetector) {
    // FIXME currently use ONLY Bisection.
    // collissionDetector = new BisectionCollissionDetector();
    final CollissionDetector old = this.collissionDetector;
    if (old == collissionDetector) return;
    propChange.firePropertyChange(
        "collissionDetector", old, this.collissionDetector = collissionDetector);
  }

  public void setCurler(final Curler curler) {
    final Curler old = this.curler;
    if (old == curler) return;
    propChange.firePropertyChange("curler", old, this.curler = curler);
  }

  public void setCurrentTime(final double currentTime) {
    final double old = this.currentTime;
    if (old == currentTime) return;
    this.currentTime = currentTime;
    if (this.currentTime > old) recompute(currentTime, true);
    propChange.firePropertyChange("currentTime", old, this.currentTime);
  }

  public void setCurveStore(final CurveStore curveStore) {
    this.curveStore = curveStore;
    recompute(currentTime, true);
    propChange.firePropertyChange("curveStore", this.curveStore, curveStore);
  }

  public void setInitialPos(final RockSet<Pos> initialPos) {
    this.initialPos.setLocation(initialPos);
  }

  public void setInitialVel(final RockSet<Vel> initialVel) {
    initialSpeed.setLocation(initialVel);
  }

  public void setSuspended(final boolean suspend) {
    final int old = collected;
    if (suspend) {
      if (collected < 0) collected = 0;
    } else {
      collected = -1;
      if (old <= 0) return;
      recompute(currentTime, true);
    }
  }

  /** One of the initial rocks changed either pos or vel */
  public void stateChanged(final ChangeEvent arg0) {
    final Object src = arg0 == null ? null : arg0.getSource();
    if (src == null || src == initialPos || src == initialSpeed) ; // recompute(currentTime, true);
    else if (initialPos.findI16(src) >= 0) {
      log.debug("Startpos rock change");
      if (getSuspended()) collected |= 1 << initialPos.findI16(src);
      else recompute(currentTime, true);
    } else if (initialSpeed.findI16(src) >= 0) {
      log.debug("Startvel rock change");
      if (getSuspended()) collected |= 1 << initialSpeed.findI16(src);
      else recompute(currentTime, true);
    } else log.info(arg0);
  }
}
 public void setInitialVel(final RockSet<Vel> initialVel) {
   initialSpeed.setLocation(initialVel);
 }