package terrasaur.apps;

import java.io.File;
import java.nio.charset.Charset;
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.io.FileUtils;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import terrasaur.smallBodyModel.BoundingBox;
import terrasaur.smallBodyModel.SmallBodyModel;
import terrasaur.templates.TerrasaurTool;
import terrasaur.utils.Log4j2Configurator;
import terrasaur.utils.NativeLibraryLoader;
import terrasaur.utils.PolyDataUtil;
import spice.basic.Plane;
import spice.basic.Vector3;
import vtk.vtkGenericCell;
import vtk.vtkPoints;
import vtk.vtkPolyData;
import vtk.vtksbCellLocator;

/**
 * AdjustShapeModelToOtherShapeModel program. See the usage string for more information about this
 * program.
 *
 * @author Eli Kahn
 * @version 1.0
 */
public class AdjustShapeModelToOtherShapeModel implements TerrasaurTool {

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

  @Override
  public String shortDescription() {
    return "Adjust vertices of one shape model to lie on the surface of another.";
  }

  @Override
  public String fullDescription(Options options) {

    String header =
        """
                \n
                This program takes 2 shape models in OBJ format and tries to adjust
                to vertices of the first shape model so they lie on the surface of the
                second shape model. It does this by shooting a ray starting from the origin
                in the direction of each point of the first model into the second model and
                then changes the point of the first model to the intersection point.""";
    return TerrasaurTool.super.fullDescription(options, header, "");
  }

    private static Options defineOptions() {
    Options options = TerrasaurTool.defineOptions();
    options.addOption(
        Option.builder("from")
            .hasArg()
            .desc(
                "path to first shape model in OBJ format which will get shifted to the second shape model")
            .build());
    options.addOption(
        Option.builder("to")
            .hasArg()
            .desc(
                "path to second shape model in OBJ format which the first shape model will try to match to")
            .build());
    options.addOption(
        Option.builder("output")
            .hasArg()
            .desc(
                "path to adjusted shape model in OBJ format generated by this program by shifting first to second")
            .build());
    options.addOption(
        Option.builder("filelist")
            .desc(
                """
                  If specified then the second required argument to this program,
                  "to" is a file containing a list of OBJ files to match to.
                  In this situation the ray is shot into each of the shape models in this
                  list and any intersection points are averaged together to produce the
                  final intersection point. Note that any individual shape model in this
                  list may be only a piece of the the complete shape model (e.g. a mapola).
                  However, the global shape model formed when all these pieces are
                  combined together, may not have any holes or gaps.""")
            .build());
    options.addOption(
        Option.builder("fit-plane-radius")
            .hasArg()
            .desc(
                """
                  If present, find a local normal at each point in the first shape
                  model by fitting a plane to all points within the specified radius.
                  Use this normal to adjust the point to the second shape model rather
                  than the radial vector.""")
            .build());
    options.addOption(
        Option.builder("local")
            .desc(
                """
                  Use when adjusting a local OBJ file to another.  The best fit plane to the
                  first shape model is used to adjust the vertices rather than the radial vector
                  for each point.
""")
            .build());
    return options;
  }

  static Vector3D computeMeanPoint(List<Vector3D> points) {
    Vector3D meanPoint = new Vector3D(0., 0., 0.);
    for (Vector3D point : points) meanPoint = meanPoint.add(point);
    meanPoint = meanPoint.scalarMultiply(1. / points.size());
    return meanPoint;
  }

  public static void adjustShapeModelToOtherShapeModel(
      vtkPolyData frompolydata,
      ArrayList<vtkPolyData> topolydata,
      double planeRadius,
      boolean localModel)
      throws Exception {
    vtkPoints points = frompolydata.GetPoints();
    long numberPoints = frompolydata.GetNumberOfPoints();

    boolean fitPlane = (planeRadius > 0);
    SmallBodyModel sbModel = new SmallBodyModel(frompolydata);
    double diagonalLength = new BoundingBox(frompolydata.GetBounds()).getDiagonalLength();

    ArrayList<vtksbCellLocator> cellLocators = new ArrayList<>();
    for (vtkPolyData polydata : topolydata) {
      vtksbCellLocator cellLocator = new vtksbCellLocator();
      cellLocator.SetDataSet(polydata);
      cellLocator.CacheCellBoundsOn();
      cellLocator.AutomaticOn();
      cellLocator.BuildLocator();
      cellLocators.add(cellLocator);
    }

    vtkGenericCell cell = new vtkGenericCell();
    double tol = 1e-6;
    double[] t = new double[1];
    double[] pcoords = new double[3];
    int[] subId = new int[1];
    long[] cell_id = new long[1];

    double[] localNormal = null;
    if (localModel) {
      // fit a plane to the local model and check that the normal points outward
      Plane localPlane = PolyDataUtil.fitPlaneToPolyData(frompolydata);
      Vector3 localNormalVector = localPlane.getNormal();
      if (localNormalVector.dot(localPlane.getPoint()) < 0)
        localNormalVector = localNormalVector.negate();
      localNormal = localNormalVector.toArray();
    }

    double[] p = new double[3];
    Vector3D origin = new Vector3D(0., 0., 0.);
    for (int i = 0; i < numberPoints; ++i) {
      points.GetPoint(i, p);

      Vector3D lookDir;

      if (fitPlane) {
        // fit a plane to the local area
        System.arraycopy(p, 0, origin.toArray(), 0, 3);
        lookDir = new Vector3D(sbModel.getNormalAtPoint(p, planeRadius)).normalize();
      } else if (localModel) {
        System.arraycopy(p, 0, origin.toArray(), 0, 3);
        lookDir = new Vector3D(localNormal).normalize();
      } else {
        // use radial vector
        lookDir = new Vector3D(p).normalize();
      }

      Vector3D lookPt = lookDir.scalarMultiply(diagonalLength);
      lookPt = lookPt.add(origin);

      List<Vector3D> intersections = new ArrayList<>();
      for (vtksbCellLocator cellLocator : cellLocators) {
        double[] intersectPoint = new double[3];

        // trace ray from the lookPt to the origin - first intersection is the farthest intersection
        // from the origin
        int result =
            cellLocator.IntersectWithLine(
                lookPt.toArray(),
                origin.toArray(),
                tol,
                t,
                intersectPoint,
                pcoords,
                subId,
                cell_id,
                cell);
        Vector3D intersectVector = new Vector3D(intersectPoint);

        if (fitPlane || localModel) {
          // NOTE: result should return 1 in case of intersection but doesn't sometimes.
          // Use the norm of intersection point to test for intersection instead.

          NavigableMap<Double, Vector3D> pointsMap = new TreeMap<>();
          if (intersectVector.getNorm() > 0) {
            pointsMap.put(origin.subtract(intersectVector).getNorm(), intersectVector);
          }

          lookPt = lookDir.scalarMultiply(-diagonalLength);
          lookPt = lookPt.add(origin);
          result =
              cellLocator.IntersectWithLine(
                  lookPt.toArray(),
                  origin.toArray(),
                  tol,
                  t,
                  intersectPoint,
                  pcoords,
                  subId,
                  cell_id,
                  cell);

          intersectVector = new Vector3D(intersectPoint);
          if (intersectVector.getNorm() > 0) {
            pointsMap.put(origin.subtract(intersectVector).getNorm(), intersectVector);
          }

          if (!pointsMap.isEmpty()) intersections.add(pointsMap.get(pointsMap.firstKey()));
        } else {
          if (result > 0) intersections.add(intersectVector);
        }
      }

      if (intersections.isEmpty()) throw new Exception("Error: no intersections at all");

      Vector3D meanIntersectionPoint = computeMeanPoint(intersections);
      points.SetPoint(i, meanIntersectionPoint.toArray());
    }
  }

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

    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)));

    boolean loadListFromFile = cl.hasOption("filelist");
    double planeRadius = Double.parseDouble(cl.getOptionValue("fit-plane-radius", "-1"));
    boolean localModel = cl.hasOption("local");

    NativeLibraryLoader.loadVtkLibraries();

    String fromfile = cl.getOptionValue("from");
    String tofile = cl.getOptionValue("to");
    String outfile = cl.getOptionValue("output");

    Log4j2Configurator.getInstance();
    logger.info("loading <from-obj-file>: {}", fromfile);
    vtkPolyData frompolydata = PolyDataUtil.loadShapeModelAndComputeNormals(fromfile);

    ArrayList<vtkPolyData> topolydata = new ArrayList<>();
    if (loadListFromFile) {
      List<String> lines = FileUtils.readLines(new File(tofile), Charset.defaultCharset());
      for (String file : lines) {

        // checking length prevents trying to load an empty line, such as the
        // last line of the file.
        if (file.length() > 1) {
          logger.info("loading <to-obj-file>: {}", file);
          topolydata.add(PolyDataUtil.loadShapeModelAndComputeNormals(file));
        }
      }
    } else {
      logger.info("loading <to-obj-file>: {}", tofile);
      topolydata.add(PolyDataUtil.loadShapeModelAndComputeNormals(tofile));
    }

    adjustShapeModelToOtherShapeModel(frompolydata, topolydata, planeRadius, localModel);

    PolyDataUtil.saveShapeModelAsOBJ(frompolydata, outfile);

    logger.info("wrote {}", outfile);
  }
}
