/*
 * The MIT License
 * Copyright © 2025 Johns Hopkins University Applied Physics Laboratory
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
package terrasaur.apps;

import java.io.File;
import java.io.PrintWriter;
import java.util.*;
import org.apache.commons.cli.CommandLine;
import org.apache.commons.cli.Option;
import org.apache.commons.cli.Options;
import org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.apache.logging.log4j.spi.StandardLevel;
import spice.basic.*;
import terrasaur.smallBodyModel.SmallBodyModel;
import terrasaur.templates.TerrasaurTool;
import terrasaur.utils.*;
import terrasaur.utils.math.MathConversions;
import vtk.vtkCellArray;
import vtk.vtkLine;
import vtk.vtkPoints;
import vtk.vtkPolyData;
import vtk.vtkPolyDataWriter;

/**
 * Find the impact point and time given an initial position and velocity.
 *
 * @author nairah1
 */
public class ImpactLocator implements UnivariateFunction, TerrasaurTool {

  private static final Logger logger = LogManager.getLogger();

  @Override
  public String shortDescription() {
    return "Calculate impact time and position from a sumFile.";
  }

  @Override
  public String fullDescription(Options options) {
    String header = "";
    String footer =
        """
               Given a sum file, shape model, and spacecraft velocity in the J2000 frame,
               this program will calculate an impact time and position.

               NOTE: Spacecraft position is assumed to be in kilometers.  If not, use the
               -distanceScale option to convert to km.

               NOTE:  Do not include a "pinpoint" or impact SPK in the kernels to
               load.""";
    return TerrasaurTool.super.fullDescription(options, header, footer);
  }

  private ReferenceFrame J2000;
  private ReferenceFrame bodyFixed;
  private SmallBodyModel sbm;
  private Double finalHeight;
  private Double finalStep;
  private TDBTime t0;
  private StateVector initialObserverJ2000;
  private StateVector initialTargetJ2000;

  private Vector3 observerAccelerationJ2000;
  private Vector3 targetAccelerationJ2000;

  private StateVector lastState;

  private vtkPolyData rayBundlePolyData;
  private vtkCellArray rayBundleCells;
  private vtkPoints rayBundlePoints;

  private ImpactLocator() {}

  public ImpactLocator(
      ReferenceFrame J2000,
      ReferenceFrame bodyFixed,
      SmallBodyModel sbm,
      Double finalHeight,
      Double finalStep,
      TDBTime t0,
      StateVector initialObserverJ2000,
      StateVector initialTargetJ2000,
      TDBTime t1,
      StateVector finalObserverJ2000,
      StateVector finalTargetJ2000)
      throws SpiceErrorException {
    this.J2000 = J2000;
    this.bodyFixed = bodyFixed;
    this.sbm = sbm;
    this.finalHeight = finalHeight;
    this.finalStep = finalStep;
    this.t0 = t0;
    this.initialObserverJ2000 = initialObserverJ2000;
    this.initialTargetJ2000 = initialTargetJ2000;

    if (t1 == null) {
      observerAccelerationJ2000 = new Vector3();
      targetAccelerationJ2000 = new Vector3();
    } else {
      double duration = t1.getTDBSeconds() - t0.getTDBSeconds();
      observerAccelerationJ2000 =
          finalObserverJ2000
              .getVelocity()
              .sub(initialObserverJ2000.getVelocity())
              .scale(1. / duration);
      targetAccelerationJ2000 =
          finalTargetJ2000.getVelocity().sub(initialTargetJ2000.getVelocity()).scale(1. / duration);
    }
  }

  /**
   * find the body state at time t. Assume a constant velocity in the J2000 frame.
   *
   * @param et ephemeris time
   * @return Body state at time et
   */
  public StateVector getStateBodyFixed(TDBTime et) {

    try {
      double delta = et.sub(t0).getMeasure();

      Vector3 observerPosJ2000 =
          initialObserverJ2000
              .getPosition()
              .add(initialObserverJ2000.getVelocity().scale(delta))
              .add(observerAccelerationJ2000.scale(0.5 * delta * delta));

      Vector3 targetPosJ2000 =
          initialTargetJ2000
              .getPosition()
              .add(initialTargetJ2000.getVelocity().scale(delta))
              .add(targetAccelerationJ2000.scale(0.5 * delta * delta));

      Vector3 scPosJ2000 = observerPosJ2000.sub(targetPosJ2000);
      Vector3 observerVelJ2000 =
          initialObserverJ2000.getVelocity().add(observerAccelerationJ2000.scale(delta));
      Vector3 targetVelJ2000 =
          initialTargetJ2000.getVelocity().add(targetAccelerationJ2000.scale(delta));
      Vector3 scVelJ2000 = observerVelJ2000.sub(targetVelJ2000);

      StateVector scStateJ2000 = new StateVector(scPosJ2000, scVelJ2000);
      StateVector scStateBodyFixed =
          new StateVector(J2000.getStateTransformation(bodyFixed, et).mxv(scStateJ2000));

      if (lastState == null) {
        lastState = scStateBodyFixed;
        rayBundlePolyData = new vtkPolyData();
        rayBundleCells = new vtkCellArray();
        rayBundlePoints = new vtkPoints();

        rayBundlePolyData.SetPoints(rayBundlePoints);
        rayBundlePolyData.SetLines(rayBundleCells);
      }
      long id0 = rayBundlePoints.InsertNextPoint(lastState.getPosition().toArray());
      long id1 = rayBundlePoints.InsertNextPoint(scStateBodyFixed.getPosition().toArray());
      lastState = scStateBodyFixed;

      vtkLine line = new vtkLine();
      line.GetPointIds().SetId(0, id0);
      line.GetPointIds().SetId(1, id1);

      rayBundleCells.InsertNextCell(line);

      return scStateBodyFixed;

    } catch (SpiceException e) {
      logger.error(e.getLocalizedMessage(), e);
    }
    return null;
  }

  /** return range from surface at time t */
  @Override
  public double value(double t) {

    TDBTime thisTime = new TDBTime(t);

    StateVector scStateBodyFixed = getStateBodyFixed(thisTime);

    Vector3 closestPoint =
        new Vector3(sbm.findClosestPoint(scStateBodyFixed.getPosition().toArray()));

    Vector3 toSurface = scStateBodyFixed.getPosition().sub(closestPoint);
    return toSurface.norm();
  }

  private NavigableSet<ImpactRecord> findTrajectory() {
    NavigableSet<ImpactRecord> records = new TreeSet<>();
    try {

      lastState = null;

      TDBTime et = t0;

      StateVector scStateBodyFixed = getStateBodyFixed(et);

      Vector3 closestPoint =
          new Vector3(sbm.findClosestPoint(scStateBodyFixed.getPosition().toArray()));
      Vector3 toSurface = scStateBodyFixed.getPosition().sub(closestPoint);
      double altitude = toSurface.norm();

      // time it takes to get halfway to the surface
      TDBDuration delta = new TDBDuration(altitude / (2 * scStateBodyFixed.getVelocity().norm()));

      boolean keepGoing = true;
      while (keepGoing) {
        LatitudinalCoordinates lc = new LatitudinalCoordinates(scStateBodyFixed.getPosition());
        records.add(
            new ImpactRecord(
                et,
                scStateBodyFixed,
                new LatitudinalCoordinates(altitude, lc.getLongitude(), lc.getLatitude())));

        et = et.add(delta);

        scStateBodyFixed = getStateBodyFixed(et);

        closestPoint = new Vector3(sbm.findClosestPoint(scStateBodyFixed.getPosition().toArray()));
        toSurface = scStateBodyFixed.getPosition().sub(closestPoint);
        altitude = toSurface.norm();

        // check that we're still moving towards the target
        if (scStateBodyFixed.getPosition().dot(scStateBodyFixed.getVelocity()) > 0) {
          logger.warn(
              "Stopping at {}; passed closest approach to the body center.",
              et.toUTCString("ISOC", 3));
          keepGoing = false;
        }

        if (altitude > finalHeight) {
          delta = new TDBDuration(toSurface.norm() / (2 * scStateBodyFixed.getVelocity().norm()));
        } else if (altitude > finalStep) {
          delta = new TDBDuration(finalStep / scStateBodyFixed.getVelocity().norm());
        } else {
          keepGoing = false;
        }
      }

      LatitudinalCoordinates lc = new LatitudinalCoordinates(scStateBodyFixed.getPosition());
      records.add(
          new ImpactRecord(
              et,
              scStateBodyFixed,
              new LatitudinalCoordinates(altitude, lc.getLongitude(), lc.getLatitude())));

    } catch (SpiceException e) {
      logger.error(e.getLocalizedMessage(), e);
    }
    return records;
  }

  static class ImpactRecord implements Comparable<ImpactRecord> {
    TDBTime et;
    StateVector scStateBodyFixed;
    LatitudinalCoordinates lc;

    private ImpactRecord(TDBTime et, StateVector scStateBodyFixed, LatitudinalCoordinates lc) {
      this.et = et;
      this.scStateBodyFixed = scStateBodyFixed;
      this.lc = lc;
    }

    @Override
    public int compareTo(ImpactRecord o) {
      try {
        return Double.compare(et.getTDBSeconds(), o.et.getTDBSeconds());
      } catch (SpiceErrorException e) {
        // completely unnecessary exception
        return 0;
      }
    }
  }

  private static Vector3 correctForAberration(
      Vector3 targetLTS, Body observer, Body target, TDBTime t) throws SpiceException {
    RemoveAberration ra = new RemoveAberration(target, observer);

    return ra.getGeometricPosition(t, targetLTS);
  }

  private static Options defineOptions() {
    Options options = TerrasaurTool.defineOptions();
    options.addOption(
        Option.builder("date")
            .hasArgs()
            .desc("Initial UTC date.  Required if -sumFile is not used.")
            .build());
    options.addOption(
        Option.builder("finalHeight")
            .hasArg()
            .desc("Height above surface in meters to consider \"impact\".  Default is 1 meter.")
            .build());
    options.addOption(
        Option.builder("finalStep")
            .hasArg()
            .desc(
                "Continue printing output below finalHeight in increments of approximate finalStep "
                    + "(in meters) until zero.  Default is to stop at finalHeight.")
            .build());
    options.addOption(
        Option.builder("frame")
            .required()
            .hasArg()
            .desc("Required.  Name of body fixed frame.")
            .build());
    options.addOption(
        Option.builder("instrumentFrame")
            .hasArg()
            .desc(
                "SPICE ID for the camera reference frame.  Required if -outputTransform "
                    + "AND -sumFile are used.")
            .build());
    options.addOption(
        Option.builder("logFile")
            .hasArg()
            .desc("If present, save screen output to log file.")
            .build());
    StringBuilder sb = new StringBuilder();
    for (StandardLevel l : StandardLevel.values()) sb.append(String.format("%s ", l.name()));
    options.addOption(
        Option.builder("logLevel")
            .hasArg()
            .desc(
                "If present, print messages above selected priority.  Valid values are "
                    + sb.toString().trim()
                    + ".  Default is INFO.")
            .build());
    options.addOption(
        Option.builder("objFile")
            .required()
            .hasArg()
            .desc("Required.  Name of OBJ shape file.")
            .build());
    options.addOption(
        Option.builder("observer")
            .required()
            .hasArg()
            .desc("Required.  SPICE ID for the impactor.")
            .build());
    options.addOption(
        Option.builder("observerFrame")
            .hasArg()
            .desc(
                "SPICE ID for the impactor's reference frame.  Required if -outputTransform is used.")
            .build());
    options.addOption(
        Option.builder("outputTransform")
            .hasArg()
            .desc(
                "If present, write out a transform file that can be used by TransformShape to place "
                    + "coordinates in the spacecraft frame in the body fixed frame.  The rotation "
                    + " is evaluated at the sumfile time.  The translation is evaluated at the impact time.  "
                    + "Requires -observerFrame option.")
            .build());
    options.addOption(
        Option.builder("position")
            .hasArg()
            .desc(
                "Spacecraft to body vector in body fixed coordinates.  Units are km.  "
                    + "Spacecraft is at the origin to be consistent with sumFile convention.")
            .build());
    options.addOption(
        Option.builder("spice")
            .required()
            .hasArgs()
            .desc(
                "Required.  SPICE metakernel file containing body fixed frame and spacecraft kernels.  "
                    + "Can specify more than one kernel, separated by whitespace.")
            .build());
    options.addOption(
        Option.builder("sumFile")
            .hasArg()
            .desc(
                "Name of sum file to read.  Coordinate system is assumed to be in the body "
                    + "fixed frame with the spacecraft at the origin.")
            .build());
    options.addOption(
        Option.builder("target")
            .required()
            .hasArg()
            .desc("Required.  SPICE ID for the target.")
            .build());
    options.addOption(
        Option.builder("trajectory")
            .hasArg()
            .desc(
                "If present, name of output VTK file containing trajectory in body fixed coordinates.")
            .build());
    options.addOption(
        Option.builder("verbosity")
            .hasArg()
            .desc("This option does nothing!  Use -logLevel instead.")
            .build());
    options.addOption(
        Option.builder("velocity")
            .hasArg()
            .desc(
                "Spacecraft velocity in J2000 relative to the body.  Units are km/s.  "
                    + "If not specified, velocity is calculated using SPICE.")
            .build());
    return options;
  }

  public static void main(String[] args) throws Exception {
    TerrasaurTool defaultOBJ = new ImpactLocator();

    Options options = defineOptions();

    CommandLine cl = defaultOBJ.parseArgs(args, options);

    Map<MessageLabel, String> startupMessages = defaultOBJ.startupMessages(cl);
    for (MessageLabel ml : startupMessages.keySet())
      logger.info(String.format("%s %s", ml.label, startupMessages.get(ml)));

    NativeLibraryLoader.loadVtkLibraries();
    NativeLibraryLoader.loadSpiceLibraries();

    String objFile = cl.getOptionValue("objFile");
    SmallBodyModel sbm = new SmallBodyModel(PolyDataUtil.loadShapeModel(objFile));

    for (String kernel : cl.getOptionValues("spice")) KernelDatabase.load(kernel);
    ReferenceFrame J2000 = new ReferenceFrame("J2000");
    ReferenceFrame bodyFixed = new ReferenceFrame(cl.getOptionValue("frame"));
    Body observer = new Body(cl.getOptionValue("observer"));
    Body target = new Body(cl.getOptionValue("target"));

    final double finalHeight =
        cl.hasOption("finalHeight")
            ? Double.parseDouble(cl.getOptionValue("finalHeight")) / 1e3
            : 1e-3;
    if (finalHeight <= 0) {
      logger.warn("Argument to -finalHeight must be positive!");
      System.exit(0);
    }

    final double finalStep =
        cl.hasOption("finalStep")
            ? Double.parseDouble(cl.getOptionValue("finalStep")) / 1e3
            : Double.MAX_VALUE;
    if (finalStep <= 0) {
      logger.warn("Argument to -finalStep must be positive!");
      System.exit(0);
    }

    // initial spacecraft position relative to target body
    Vector3 initialPos = new Vector3();
    TDBTime et = null;
    SumFile sumFile = null;
    if (cl.hasOption("sumFile")) {
      sumFile = SumFile.fromFile(new File(cl.getOptionValue("sumFile")));

      et = new TDBTime(sumFile.utcString());

      Matrix33 bodyFixedToJ2000 = bodyFixed.getPositionTransformation(J2000, et);
      Vector3 scObjJ2000 = bodyFixedToJ2000.mxv(MathConversions.toVector3(sumFile.scobj()));
      initialPos = correctForAberration(scObjJ2000, observer, target, et);
      initialPos = bodyFixedToJ2000.mtxv(initialPos).negate();

    } else if (cl.hasOption("date")) {
      String[] parts = cl.getOptionValues("date");
      StringBuilder sb = new StringBuilder();
      for (String part : parts) sb.append(part).append(" ");
      et = new TDBTime(sb.toString());
    } else {
      logger.warn("Either -sumFile or -date must be specified.");
      System.exit(0);
    }
    TDBTime et0 = et;
    AberrationCorrection abCorrNone = new AberrationCorrection("NONE");

    // target's state relative to observer
    StateRecord sr = new StateRecord(target, et, bodyFixed, abCorrNone, observer);

    /*-
    // aberration test
    sr = new StateRecord(target, et, J2000, abCorrNone, observer);
    StateRecord srLTS =
        new StateRecord(target, et, J2000, new AberrationCorrection("LT+S"), observer);
    RemoveAberration ra = new RemoveAberration(target, observer);
    Vector3 estimatedGeometric = ra.getGeometricPosition(et, srLTS.getPosition());

    System.out.printf("LT+S position:       %s\n", new Vector3(srLTS.getPosition()));
    System.out.printf("geometric position:  %s\n", new Vector3(sr.getPosition()));
    Vector3 difference = sr.getPosition().sub(srLTS.getPosition());
    System.out.printf("difference:          %s %f\n", difference, difference.norm());
    System.out.printf("aberration angle:    %.3e\n", srLTS.getPosition().sep(sr.getPosition()));
    System.out.printf("estimated geometric: %s\n", estimatedGeometric);
    difference = sr.getPosition().sub(estimatedGeometric);
    System.out.printf("difference:          %s %f\n", difference, difference.norm());
    System.out.printf("angle:               %.3e\n", estimatedGeometric.sep(sr.getPosition()));
    System.out.println();
    System.exit(0);
    */

    // this is only true with aberration correction NONE!
    Vector3 scPosBodyFixed = sr.getPosition().negate();

    if (cl.hasOption("position")) {
      String[] parts = cl.getOptionValue("position").split(",");
      double[] tmp = new double[3];
      for (int i = 0; i < 3; i++) tmp[i] = Double.parseDouble(parts[i].trim());
      initialPos.assign(tmp);
    } else if (!cl.hasOption("sumFile")) {
      // use position calculated by SPICE
      initialPos = scPosBodyFixed;
    }

    if (Math.abs(scPosBodyFixed.sub(initialPos).norm()) > 0) {
      logger.warn(
          String.format(
              "Warning!  Spacecraft position relative to target from SPICE is %s while input position is %s",
              new Vector3(scPosBodyFixed), initialPos.toString()));
      logger.warn(String.format("Difference is %e km", initialPos.sub(scPosBodyFixed).norm()));
      logger.warn("Continuing with input position");
    }

    Vector3 initialPosJ2000 = bodyFixed.getPositionTransformation(J2000, et).mxv(initialPos);

    // relative to solar system barycenter in J2000
    StateVector initialTargetJ2000 =
        new StateRecord(target, et, J2000, abCorrNone, new Body(0)).getStateVector();
    StateVector initialObserverJ2000 =
        new StateVector(
            initialPosJ2000.add(initialTargetJ2000.getPosition()),
            new StateRecord(observer, et, J2000, abCorrNone, new Body(0)).getVelocity());

    if (cl.hasOption("velocity")) {
      String[] parts = cl.getOptionValue("velocity").split(",");
      double[] tmp = new double[3];
      for (int i = 0; i < 3; i++) tmp[i] = Double.parseDouble(parts[i].trim());
      Vector3 scVelJ2000 = new Vector3(tmp);

      initialObserverJ2000 =
          new StateVector(
              initialObserverJ2000.getPosition(), scVelJ2000.add(initialTargetJ2000.getVelocity()));

      StateRecord obs = new StateRecord(observer, et, J2000, abCorrNone, new Body(0));
      logger.info(
          String.format(
              "spacecraft velocity relative to target from SPICE at %s is %s",
              et.toUTCString("ISOC", 3), obs.getVelocity().sub(initialTargetJ2000.getVelocity())));
      logger.info(String.format("Specified velocity is %s", scVelJ2000));
    }

    ImpactLocator ifsf =
        new ImpactLocator(
            J2000,
            bodyFixed,
            sbm,
            finalHeight,
            finalStep,
            et,
            initialObserverJ2000,
            initialTargetJ2000,
            null,
            null,
            null);

    NavigableSet<ImpactRecord> records = ifsf.findTrajectory();
    TDBTime first = records.first().et;

    StateVector scStateBodyFixed = ifsf.getStateBodyFixed(first);
    StateVector scStateJ2000 =
        new StateVector(bodyFixed.getStateTransformation(J2000, first).mxv(scStateBodyFixed));

    System.out.printf("T0: %s%n", first.toUTCString("ISOC", 3));
    System.out.printf("Frame %s: %s%n", bodyFixed.getName(), scStateBodyFixed);
    System.out.printf("Frame J2000: %s%n", scStateJ2000);
    System.out.printf(
        "%s: Observer velocity relative to SSB (J2000): %s%n",
        first.toUTCString("ISOC", 3), initialObserverJ2000.getVelocity());

    // find coverage of observer and target
    int numSPK = KernelDatabase.ktotal("SPK");
    double lastTarget = -Double.MAX_VALUE;
    double lastObserver = -Double.MAX_VALUE;
    for (int i = 0; i < numSPK; i++) {
      String filename = KernelDatabase.getFileName(i, "SPK");
      SPK thisSPK = SPK.openForRead(filename);
      SpiceWindow coverage = thisSPK.getCoverage(target.getIDCode());
      if (coverage.card() > 0) {
        double[] lastInterval = coverage.getInterval(coverage.card() - 1);
        lastTarget = Math.max(lastTarget, lastInterval[1]);
        logger.debug(
            "SPK {}: body {}, last time is {}",
            filename,
            target.getName(),
            new TDBTime(lastTarget).toUTCString("ISOC", 3));
      }
      coverage = thisSPK.getCoverage(observer.getIDCode());
      if (coverage.card() > 0) {
        double[] lastInterval = coverage.getInterval(coverage.card() - 1);
        lastObserver = Math.max(lastObserver, lastInterval[1]);
        logger.debug(
            "SPK {}: body {}, last time is {}",
            filename,
            observer.getName(),
            new TDBTime(lastObserver).toUTCString("ISOC", 3));
      }
    }

    double lastET = Math.min(records.last().et.getTDBSeconds(), lastTarget);
    lastET = Math.min(lastET, lastObserver);
    TDBTime last = new TDBTime(lastET);
    StateRecord finalObserverJ2000 =
        new StateRecord(observer, last, J2000, abCorrNone, new Body(0));
    StateRecord finalTargetJ2000 = new StateRecord(target, last, J2000, abCorrNone, new Body(0));
    System.out.printf(
        "%s: Observer velocity relative to SSB (J2000): %s%n",
        last.toUTCString("ISOC", 3), finalObserverJ2000.getVelocity());

    double duration = last.getTDBSeconds() - first.getTDBSeconds();
    Vector3 observerAccelerationJ2000 =
        finalObserverJ2000
            .getVelocity()
            .sub(initialObserverJ2000.getVelocity())
            .scale(1. / duration);
    Vector3 targetAccelerationJ2000 =
        finalTargetJ2000.getVelocity().sub(initialTargetJ2000.getVelocity()).scale(1. / duration);

    System.out.printf("Estimated time of impact %s\n", last.toUTCString("ISOC", 6));
    System.out.printf("Estimated time to impact %.6f seconds\n", duration);
    System.out.printf("Estimated observer acceleration (J2000): %s\n", observerAccelerationJ2000);
    System.out.printf("Estimated target acceleration (J2000): %s\n", targetAccelerationJ2000);
    System.out.printf(
        "observer acceleration relative to target: %s\n",
        observerAccelerationJ2000.sub(targetAccelerationJ2000));

    System.out.println();

    // Run again with constant accelerations for target and observer
    ifsf =
        new ImpactLocator(
            J2000,
            bodyFixed,
            sbm,
            finalHeight,
            finalStep,
            first,
            initialObserverJ2000,
            initialTargetJ2000,
            last,
            finalObserverJ2000,
            finalTargetJ2000);
    records = ifsf.findTrajectory();

    System.out.printf(
        "%26s, %13s, %13s, %13s, %13s, %13s, %13s, %12s, %12s, %12s",
        "UTC",
        "X (km)",
        "Y (km)",
        "Z (km)",
        "VX (km/s)",
        "VY (km/s)",
        "VZ (km/s)",
        "Lat (deg)",
        "Lon (deg)",
        "Alt (m)\n");
    for (ImpactRecord record : records) {
      PositionVector p = record.scStateBodyFixed.getPosition();
      VelocityVector v = record.scStateBodyFixed.getVelocity();
      System.out.printf(
          String.format(
              "%26s, %13.6e, %13.6e, %13.6e, %13.6e, %13.6e, %13.6e, %12.4f, %12.4f, %12.4f\n",
              record.et.toUTCString("ISOC", 6),
              p.getElt(0),
              p.getElt(1),
              p.getElt(2),
              v.getElt(0),
              v.getElt(1),
              v.getElt(2),
              Math.toDegrees(record.lc.getLatitude()),
              Math.toDegrees(record.lc.getLongitude()),
              record.lc.getRadius() * 1e3));
    }

    if (cl.hasOption("trajectory")) {

      File trajectoryFile = new File(cl.getOptionValue("trajectory"));
      File parent = trajectoryFile.getParentFile();
      if (parent != null && !parent.exists()) parent.mkdirs();

      vtkPolyDataWriter writer = new vtkPolyDataWriter();
      writer.SetInputData(ifsf.rayBundlePolyData);
      writer.SetFileName(cl.getOptionValue("trajectory"));
      writer.SetFileTypeToBinary();
      writer.Update();
    }

    if (cl.hasOption("outputTransform")) {
      if (cl.hasOption("observerFrame")) {
        // evaluate rotation at -date or -sumFile time

        File transformFile = new File(cl.getOptionValue("outputTransform"));
        File parent = transformFile.getParentFile();
        if (!parent.exists()) parent.mkdirs();

        ReferenceFrame scFrame =
            new ReferenceFrame(cl.getOptionValue("observerFrame").toUpperCase());
        Matrix33 scToBodyFixed;

        if (sumFile != null) {

          // scToBodyFixed = scFrame.getPositionTransformation(bodyFixed, et0);
          // logger.info("scToBody (SPICE):\n" + scToBodyFixed);

          ReferenceFrame instrFrame = null;
          if (cl.hasOption("instrumentFrame"))
            instrFrame = new ReferenceFrame(cl.getOptionValue("instrumentFrame").toUpperCase());
          if (instrFrame == null) {
            logger.error("-instrumentFrame needed for -outputTransform.  Exiting.");
            System.exit(0);
          }
          Matrix33 scToCamera = scFrame.getPositionTransformation(instrFrame, et0);

          // DART SPECIFIC!!!!!! TODO: create a Terrasaur config file with project-specific
          // defaults,
          // like spice kernel, camera flips, etc.

          // flip -1, 2, -3

          Vector3 row0 = MathConversions.toVector3(sumFile.cx().negate());
          Vector3 row1 = MathConversions.toVector3(sumFile.cy());
          Vector3 row2 = MathConversions.toVector3(sumFile.cz().negate());

          Matrix33 bodyFixedToCamera = new Matrix33(row0, row1, row2);

          scToBodyFixed = bodyFixedToCamera.mtxm(scToCamera);

          // logger.info("scToBody (SUMFILE):\n" + scToBodyFixed);
        } else {
          scToBodyFixed = scFrame.getPositionTransformation(bodyFixed, et0);
        }

        PositionVector p = records.last().scStateBodyFixed.getPosition();
        try (PrintWriter pw = new PrintWriter(transformFile)) {

          List<String> transform = new ArrayList<>();
          for (int i = 0; i < 3; i++) {
            StringBuilder sb = new StringBuilder();
            for (int j = 0; j < 3; j++) sb.append(String.format("%f ", scToBodyFixed.getElt(i, j)));
            sb.append(String.format("%f ", p.getElt(i)));
            transform.add(sb.toString());
          }
          transform.add("0 0 0 1");
          StringBuilder sb = new StringBuilder();
          for (String s : transform) sb.append(String.format("%s\n", s));

          pw.println(sb);
        }
        /*-
        Pair<Vector3, Matrix33> transform =
            CommandLineOptionsUtil.getTransformation(cl.getOptionValue("outputTransform"));
        System.out.printf("translate\n\t%s\n\t%s\n", p.toString(),
            transform.getFirst().toString());
        System.out.printf("rotate\n\t%s\n\t%s\n", scToBodyFixed.toString(),
            transform.getSecond().toString());
            */
      } else {
        logger.warn("-observerFrame needed for -outputTransform");
      }
    }
  }
}
