/**
 * Copyright (c) 2008-2021 Bird Dog Games, Inc.
 *
 * This file is part of Ardor3D.
 *
 * Ardor3D is free software: you can redistribute it and/or modify it
 * under the terms of its license which may be found in the accompanying
 * LICENSE file or at <https://git.io/fjRmv>.
 */

package com.ardor3d.bounding;

import java.io.IOException;
import java.nio.FloatBuffer;
import java.util.logging.Level;
import java.util.logging.Logger;

import com.ardor3d.buffer.BufferUtils;
import com.ardor3d.intersection.IntersectionRecord;
import com.ardor3d.math.Plane;
import com.ardor3d.math.Vector3;
import com.ardor3d.math.type.ReadOnlyPlane;
import com.ardor3d.math.type.ReadOnlyPlane.Side;
import com.ardor3d.math.util.MathUtils;
import com.ardor3d.math.type.ReadOnlyRay3;
import com.ardor3d.math.type.ReadOnlyTransform;
import com.ardor3d.math.type.ReadOnlyVector3;
import com.ardor3d.scenegraph.MeshData;
import com.ardor3d.util.export.InputCapsule;
import com.ardor3d.util.export.OutputCapsule;

/**
 * <code>BoundingSphere</code> defines a sphere that defines a container for a group of vertices of
 * a particular piece of geometry. This sphere defines a radius and a center. <br>
 * <br>
 * A typical usage is to allow the class define the center and radius by calling either
 * <code>containAABB</code> or <code>averagePoints</code>. A call to <code>computeFramePoint</code>
 * in turn calls <code>containAABB</code>.
 */
public class BoundingSphere extends BoundingVolume {
  private static final Logger logger = Logger.getLogger(BoundingSphere.class.getName());

  private static final long serialVersionUID = 1L;

  private double _radius;

  static final private double radiusEpsilon = 1 + 0.00001;

  protected final Vector3 _compVect3 = new Vector3();
  protected final Vector3 _compVect4 = new Vector3();

  /**
   * Default constructor instantiates a new <code>BoundingSphere</code> object.
   */
  public BoundingSphere() {}

  /**
   * Constructor instantiates a new <code>BoundingSphere</code> object.
   * 
   * @param r
   *          the radius of the sphere.
   * @param c
   *          the center of the sphere.
   */
  public BoundingSphere(final double r, final ReadOnlyVector3 c) {
    _center.set(c);
    setRadius(r);
  }

  @Override
  public Type getType() { return Type.Sphere; }

  @Override
  public BoundingVolume transform(final ReadOnlyTransform transform, final BoundingVolume store) {
    BoundingSphere sphere;
    if (store == null || store.getType() != BoundingVolume.Type.Sphere) {
      sphere = new BoundingSphere(1, new Vector3(0, 0, 0));
    } else {
      sphere = (BoundingSphere) store;
    }

    transform.applyForward(_center, sphere._center);

    if (!transform.isRotationMatrix()) {
      final Vector3 scale = _compVect3.set(1, 1, 1);
      transform.applyForwardVector(scale);
      sphere.setRadius(Math.abs(maxAxis(scale) * getRadius()) + radiusEpsilon - 1);
    } else {
      final ReadOnlyVector3 scale = transform.getScale();
      sphere.setRadius(Math.abs(maxAxis(scale) * getRadius()) + radiusEpsilon - 1);
    }

    return sphere;
  }

  private double maxAxis(final ReadOnlyVector3 scale) {
    return Math.max(Math.abs(scale.getX()), Math.max(Math.abs(scale.getY()), Math.abs(scale.getZ())));
  }

  /**
   * <code>getRadius</code> returns the radius of the bounding sphere.
   * 
   * @return the radius of the bounding sphere.
   */
  @Override
  public double getRadius() { return _radius; }

  /**
   * <code>setRadius</code> sets the radius of this bounding sphere.
   * 
   * @param radius
   *          the new radius of the bounding sphere.
   */
  public void setRadius(final double radius) { _radius = radius; }

  /**
   * <code>computeFromPoints</code> creates a new Bounding Sphere from a given set of points. It uses
   * the <code>calcWelzl</code> method as default.
   * 
   * @param points
   *          the points to contain.
   */
  @Override
  public void computeFromPoints(final FloatBuffer points) {
    calcWelzl(points);
  }

  @Override
  public void computeFromPrimitives(final MeshData data, final int section, final int[] indices, final int start,
      final int end) {
    if (end - start <= 0) {
      return;
    }

    final int vertsPerPrimitive = data.getIndexMode(section).getVertexCount();
    final Vector3[] vertList = new Vector3[(end - start) * vertsPerPrimitive];
    Vector3[] store = new Vector3[vertsPerPrimitive];

    int count = 0;
    for (int i = start; i < end; i++) {
      store = data.getPrimitiveVertices(indices[i], section, store);
      for (int j = 0; j < vertsPerPrimitive; j++) {
        vertList[count++] = Vector3.fetchTempInstance().set(store[j]);
      }
    }

    averagePoints(vertList);
    for (int i = 0; i < vertList.length; i++) {
      Vector3.releaseTempInstance(vertList[i]);
    }
  }

  /**
   * Calculates a minimum bounding sphere for the set of points. The algorithm was originally found at
   * http://www.flipcode.com/cgi-bin/msg.cgi?showThread=COTD-SmallestEnclosingSpheres&forum=cotd&id=-1
   * in C++ and translated to java by Cep21
   * 
   * @param points
   *          The points to calculate the minimum bounds from.
   */
  public void calcWelzl(final FloatBuffer points) {
    final float[] buf = new float[points.limit()];
    points.rewind();
    points.get(buf);
    recurseMini(buf, buf.length / 3, 0, 0);
  }

  /**
   * Used from calcWelzl. This function recurses to calculate a minimum bounding sphere a few points
   * at a time.
   * 
   * @param points
   *          The array of points to look through.
   * @param p
   *          The size of the list to be used.
   * @param pnts
   *          The number of points currently considering to include with the sphere.
   * @param ap
   *          A variable simulating pointer arithmetic from C++, and offset in <code>points</code>.
   */
  private void recurseMini(final float[] points, final int p, final int pnts, final int ap) {
    switch (pnts) {
      case 0:
        setRadius(0);
        _center.set(0, 0, 0);
        break;
      case 1:
        setRadius(1f - radiusEpsilon);
        populateFromBuffer(_center, points, ap - 1);
        break;
      case 2:
        populateFromBuffer(_compVect1, points, ap - 1);
        populateFromBuffer(_compVect2, points, ap - 2);
        setSphere(_compVect1, _compVect2);
        break;
      case 3:
        populateFromBuffer(_compVect1, points, ap - 1);
        populateFromBuffer(_compVect2, points, ap - 2);
        populateFromBuffer(_compVect3, points, ap - 3);
        setSphere(_compVect1, _compVect2, _compVect3);
        break;
      case 4:
        populateFromBuffer(_compVect1, points, ap - 1);
        populateFromBuffer(_compVect2, points, ap - 2);
        populateFromBuffer(_compVect3, points, ap - 3);
        populateFromBuffer(_compVect4, points, ap - 4);
        setSphere(_compVect1, _compVect2, _compVect3, _compVect4);
        return;
    }
    for (int i = 0; i < p; i++) {
      populateFromBuffer(_compVect1, points, i + ap);
      if (_compVect1.distanceSquared(_center) - (getRadius() * getRadius()) > radiusEpsilon - 1f) {
        for (int j = i; j > 0; j--) {
          populateFromBuffer(_compVect2, points, j + ap);
          populateFromBuffer(_compVect3, points, j - 1 + ap);
          setInBuffer(_compVect3, points, j + ap);
          setInBuffer(_compVect2, points, j - 1 + ap);
        }
        recurseMini(points, i, pnts + 1, ap + 1);
      }
    }
  }

  public static void populateFromBuffer(final Vector3 vector, final float[] buf, final int index) {
    vector.setX(buf[index * 3]);
    vector.setY(buf[index * 3 + 1]);
    vector.setZ(buf[index * 3 + 2]);
  }

  public static void setInBuffer(final ReadOnlyVector3 vector, final float[] buf, final int index) {
    if (buf == null) {
      return;
    }
    if (vector == null) {
      buf[index * 3] = 0;
      buf[(index * 3) + 1] = 0;
      buf[(index * 3) + 2] = 0;
    } else {
      buf[index * 3] = vector.getXf();
      buf[(index * 3) + 1] = vector.getYf();
      buf[(index * 3) + 2] = vector.getZf();
    }
  }

  /**
   * Calculates the minimum bounding sphere of 4 points. Used in welzl's algorithm.
   * 
   * @param O
   *          The 1st point inside the sphere.
   * @param A
   *          The 2nd point inside the sphere.
   * @param B
   *          The 3rd point inside the sphere.
   * @param C
   *          The 4th point inside the sphere.
   * @see #calcWelzl(java.nio.FloatBuffer)
   */
  private void setSphere(final Vector3 O, final Vector3 A, final Vector3 B, final Vector3 C) {
    final Vector3 a = A.subtract(O, null);
    final Vector3 b = B.subtract(O, null);
    final Vector3 c = C.subtract(O, null);

    final double Denominator = 2.0 * (a.getX() * (b.getY() * c.getZ() - c.getY() * b.getZ())
        - b.getX() * (a.getY() * c.getZ() - c.getY() * a.getZ())
        + c.getX() * (a.getY() * b.getZ() - b.getY() * a.getZ()));
    if (Denominator == 0) {
      _center.set(0, 0, 0);
      setRadius(0);
    } else {
      final Vector3 o =
          a.cross(b, null).multiplyLocal(c.lengthSquared()).addLocal(c.cross(a, null).multiplyLocal(b.lengthSquared()))
              .addLocal(b.cross(c, null).multiplyLocal(a.lengthSquared())).divideLocal(Denominator);

      setRadius(o.length() * radiusEpsilon);
      O.add(o, _center);
    }
  }

  /**
   * Calculates the minimum bounding sphere of 3 points. Used in welzl's algorithm.
   * 
   * @param O
   *          The 1st point inside the sphere.
   * @param A
   *          The 2nd point inside the sphere.
   * @param B
   *          The 3rd point inside the sphere.
   * @see #calcWelzl(java.nio.FloatBuffer)
   */
  private void setSphere(final Vector3 O, final Vector3 A, final Vector3 B) {
    final Vector3 a = A.subtract(O, null);
    final Vector3 b = B.subtract(O, null);
    final Vector3 acrossB = a.cross(b, null);

    final double Denominator = 2.0 * acrossB.dot(acrossB);

    if (Denominator == 0) {
      _center.set(0, 0, 0);
      setRadius(0);
    } else {

      final Vector3 o = acrossB.cross(a, null).multiplyLocal(b.lengthSquared())
          .addLocal(b.cross(acrossB, null).multiplyLocal(a.lengthSquared())).divideLocal(Denominator);
      setRadius(o.length() * radiusEpsilon);
      O.add(o, _center);
    }
  }

  /**
   * Calculates the minimum bounding sphere of 2 points. Used in welzl's algorithm.
   * 
   * @param O
   *          The 1st point inside the sphere.
   * @param A
   *          The 2nd point inside the sphere.
   * @see #calcWelzl(java.nio.FloatBuffer)
   */
  private void setSphere(final Vector3 O, final Vector3 A) {
    setRadius(Math.sqrt(((A.getX() - O.getX()) * (A.getX() - O.getX()) + (A.getY() - O.getY()) * (A.getY() - O.getY())
        + (A.getZ() - O.getZ()) * (A.getZ() - O.getZ())) / 4f) + radiusEpsilon - 1);
    Vector3.lerp(O, A, .5, _center);
  }

  /**
   * <code>averagePoints</code> selects the sphere center to be the average of the points and the
   * sphere radius to be the smallest value to enclose all points.
   * 
   * @param points
   *          the list of points to contain.
   */
  public void averagePoints(final Vector3[] points) {
    _center.set(points[0]);

    for (int i = 1; i < points.length; i++) {
      _center.addLocal(points[i]);
    }

    final double quantity = 1.0 / points.length;
    _center.multiplyLocal(quantity);

    double maxRadiusSqr = 0;
    for (int i = 0; i < points.length; i++) {
      final Vector3 diff = points[i].subtract(_center, _compVect1);
      final double radiusSqr = diff.lengthSquared();
      if (radiusSqr > maxRadiusSqr) {
        maxRadiusSqr = radiusSqr;
      }
    }

    setRadius(Math.sqrt(maxRadiusSqr) + radiusEpsilon - 1f);

  }

  /**
   * <code>whichSide</code> takes a plane (typically provided by a view frustum) to determine which
   * side this bound is on.
   * 
   * @param plane
   *          the plane to check against.
   * @return side
   */
  @Override
  public Side whichSide(final ReadOnlyPlane plane) {
    final double distance = plane.pseudoDistance(_center);

    if (distance <= -getRadius()) {
      return Plane.Side.Inside;
    } else if (distance >= getRadius()) {
      return Plane.Side.Outside;
    } else {
      return Plane.Side.Neither;
    }
  }

  /**
   * <code>merge</code> combines this sphere with a second bounding sphere. This new sphere contains
   * both bounding spheres and is returned.
   * 
   * @param volume
   *          the sphere to combine with this sphere.
   * @return a new sphere
   */
  @Override
  public BoundingVolume merge(final BoundingVolume volume) {
    if (volume == null) {
      return this;
    }

    switch (volume.getType()) {

      case Sphere: {
        final BoundingSphere sphere = (BoundingSphere) volume;
        final double temp_radius = sphere.getRadius();
        final ReadOnlyVector3 tempCenter = sphere.getCenter();
        final BoundingSphere rVal = new BoundingSphere();
        return merge(temp_radius, tempCenter, rVal);
      }

      case AABB: {
        final BoundingBox box = (BoundingBox) volume;
        final Vector3 radVect = new Vector3(box.getXExtent(), box.getYExtent(), box.getZExtent());
        final Vector3 tempCenter = box._center;
        final BoundingSphere rVal = new BoundingSphere();
        return merge(radVect.length(), tempCenter, rVal);
      }

      case OBB: {
        final OrientedBoundingBox box = (OrientedBoundingBox) volume;
        final BoundingSphere rVal = (BoundingSphere) this.clone(null);
        return rVal.mergeLocalOBB(box);
      }

      default:
        return null;

    }
  }

  /**
   * <code>mergeLocal</code> combines this sphere with a second bounding sphere locally. Altering this
   * sphere to contain both the original and the additional sphere volumes;
   * 
   * @param volume
   *          the sphere to combine with this sphere.
   * @return this
   */
  @Override
  public BoundingVolume mergeLocal(final BoundingVolume volume) {
    if (volume == null) {
      return this;
    }

    switch (volume.getType()) {

      case Sphere: {
        final BoundingSphere sphere = (BoundingSphere) volume;
        final double temp_radius = sphere.getRadius();
        final ReadOnlyVector3 temp_center = sphere.getCenter();
        return merge(temp_radius, temp_center, this);
      }

      case AABB: {
        final BoundingBox box = (BoundingBox) volume;
        final Vector3 temp_center = box._center;
        _compVect1.set(box.getXExtent(), box.getYExtent(), box.getZExtent());
        final double radius = _compVect1.length();
        return merge(radius, temp_center, this);
      }

      case OBB: {
        return mergeLocalOBB((OrientedBoundingBox) volume);
      }

      default:
        return null;
    }
  }

  /**
   * Merges this sphere with the given OBB.
   * 
   * @param volume
   *          The OBB to merge.
   * @return This sphere, after merging.
   */
  private BoundingSphere mergeLocalOBB(final OrientedBoundingBox volume) {
    // check for infinite bounds to prevent NaN values... is so, return infinite bounds with center at
    // origin
    if (Double.isInfinite(getRadius()) || Vector3.isInfinite(volume.getExtent())) {
      setCenter(Vector3.ZERO);
      setRadius(Double.POSITIVE_INFINITY);
      return this;
    }

    // compute edge points from the obb
    if (!volume.correctCorners) {
      volume.computeCorners();
    }

    final FloatBuffer mergeBuf = BufferUtils.createFloatBufferOnHeap(8 * 3);

    for (int i = 0; i < 8; i++) {
      mergeBuf.put((float) volume._vectorStore[i].getX());
      mergeBuf.put((float) volume._vectorStore[i].getY());
      mergeBuf.put((float) volume._vectorStore[i].getZ());
    }

    // remember old radius and center
    final double oldRadius = getRadius();
    final double oldCenterX = _center.getX();
    final double oldCenterY = _center.getY();
    final double oldCenterZ = _center.getZ();

    // compute new radius and center from obb points
    computeFromPoints(mergeBuf);

    final double newCenterX = _center.getX();
    final double newCenterY = _center.getY();
    final double newCenterZ = _center.getZ();
    final double newRadius = getRadius();

    // restore old center and radius
    _center.set(oldCenterX, oldCenterY, oldCenterZ);
    setRadius(oldRadius);

    // merge obb points result
    merge(newRadius, _compVect4.set(newCenterX, newCenterY, newCenterZ), this);

    return this;
  }

  private BoundingVolume merge(final double otherRadius, final ReadOnlyVector3 otherCenter,
      final BoundingSphere store) {
    // check for infinite bounds... is so, return infinite bounds with center at origin
    if (Double.isInfinite(otherRadius) || Double.isInfinite(getRadius())) {
      store.setCenter(Vector3.ZERO);
      store.setRadius(Double.POSITIVE_INFINITY);
      return store;
    }

    final Vector3 diff = otherCenter.subtract(_center, _compVect1);
    final double lengthSquared = diff.lengthSquared();
    final double radiusDiff = otherRadius - getRadius();
    final double radiusDiffSqr = radiusDiff * radiusDiff;

    // if one sphere wholly contains the other
    if (radiusDiffSqr >= lengthSquared) {
      // if we contain the other
      if (radiusDiff <= 0.0) {
        store.setCenter(_center);
        store.setRadius(_radius);
        return store;
      }
      // else the other contains us
      else {
        store.setCenter(otherCenter);
        store.setRadius(otherRadius);
        return store;
      }
    }

    // distance between sphere centers
    final double length = Math.sqrt(lengthSquared);

    // init a center var using our center
    final Vector3 rCenter = _compVect2;
    rCenter.set(_center);

    // if our centers are at least a tiny amount apart from each other...
    if (length > MathUtils.EPSILON) {
      // place us between the two centers, weighted by radii
      final double coeff = (length + radiusDiff) / (2.0 * length);
      rCenter.addLocal(diff.multiplyLocal(coeff));
    }

    // set center on our resulting bounds
    store.setCenter(rCenter);

    // Set radius
    store.setRadius(0.5 * (length + getRadius() + otherRadius));
    return store;
  }

  @Override
  public BoundingVolume asType(final Type newType) {
    if (newType == null) {
      return null;
    }

    switch (newType) {
      case AABB: {
        final BoundingBox box = new BoundingBox(_center, 0, 0, 0);
        return box.merge(this);
      }

      case Sphere: {
        return this.clone(null);
      }

      case OBB: {
        final OrientedBoundingBox obb = new OrientedBoundingBox();
        obb.setCenter(_center);
        return obb.merge(this);
      }

      default:
        return null;
    }
  }

  /**
   * <code>clone</code> creates a new BoundingSphere object containing the same data as this one.
   * 
   * @param store
   *          where to store the cloned information. if null or wrong class, a new store is created.
   * @return the new BoundingSphere
   */
  @Override
  public BoundingVolume clone(final BoundingVolume store) {
    if (store != null && store.getType() == Type.Sphere) {
      final BoundingSphere rVal = (BoundingSphere) store;
      rVal._center.set(_center);
      rVal.setRadius(_radius);
      rVal._checkPlane = _checkPlane;
      return rVal;
    }

    return new BoundingSphere(getRadius(), _center);
  }

  @Override
  public String toString() {
    return "com.ardor3d.scene.BoundingSphere [Radius: " + getRadius() + " Center: " + _center + "]";
  }

  @Override
  public boolean intersects(final BoundingVolume bv) {
    if (bv == null) {
      return false;
    }

    return bv.intersectsSphere(this);
  }

  @Override
  public boolean intersectsSphere(final BoundingSphere bs) {
    if (!Vector3.isFinite(_center) || !Vector3.isFinite(bs._center)) {
      return false;
    }

    final Vector3 diff = _compVect1.set(getCenter()).subtractLocal(bs.getCenter());
    final double rsum = getRadius() + bs.getRadius();
    return (diff.dot(diff) <= rsum * rsum);
  }

  @Override
  public boolean intersectsBoundingBox(final BoundingBox bb) {
    if (!Vector3.isFinite(_center) || !Vector3.isFinite(bb._center)) {
      return false;
    }

    if (Math.abs(bb._center.getX() - getCenter().getX()) < getRadius() + bb.getXExtent()
        && Math.abs(bb._center.getY() - getCenter().getY()) < getRadius() + bb.getYExtent()
        && Math.abs(bb._center.getZ() - getCenter().getZ()) < getRadius() + bb.getZExtent()) {
      return true;
    }

    return false;
  }

  @Override
  public boolean intersectsOrientedBoundingBox(final OrientedBoundingBox obb) {
    return obb.intersectsSphere(this);
  }

  @Override
  public boolean intersects(final ReadOnlyRay3 ray) {
    if (!Vector3.isFinite(_center)) {
      return false;
    }

    final Vector3 diff = ray.getOrigin().subtract(getCenter(), _compVect1);
    final double radiusSquared = getRadius() * getRadius();
    final double a = diff.dot(diff) - radiusSquared;
    if (a <= 0.0) {
      // in sphere
      return true;
    }

    // outside sphere
    final Vector3 dir = _compVect2.set(ray.getDirection());
    final double b = dir.dot(diff);
    if (b >= 0.0) {
      return false;
    }
    return b * b >= a;
  }

  @Override
  public IntersectionRecord intersectsWhere(final ReadOnlyRay3 ray) {

    final Vector3 diff = ray.getOrigin().subtract(getCenter(), _compVect1);
    final double a = diff.dot(diff) - (getRadius() * getRadius());
    double a1, discr, root;
    if (a <= 0.0) {
      // inside sphere
      a1 = ray.getDirection().dot(diff);
      discr = (a1 * a1) - a;
      root = Math.sqrt(discr);
      final double[] distances = new double[] {root - a1};
      final Vector3[] points =
          new Vector3[] {ray.getDirection().multiply(distances[0], new Vector3()).addLocal(ray.getOrigin())};
      return new IntersectionRecord(distances, points);
    }

    a1 = ray.getDirection().dot(diff);
    if (a1 >= 0.0) {
      // No intersection
      return null;
    }

    discr = a1 * a1 - a;
    if (discr < 0.0) {
      return null;
    } else if (discr >= MathUtils.ZERO_TOLERANCE) {
      root = Math.sqrt(discr);
      final double[] distances = new double[] {-a1 - root, -a1 + root};
      final Vector3[] points =
          new Vector3[] {ray.getDirection().multiply(distances[0], new Vector3()).addLocal(ray.getOrigin()),
              ray.getDirection().multiply(distances[1], new Vector3()).addLocal(ray.getOrigin())};
      final IntersectionRecord record = new IntersectionRecord(distances, points);
      return record;
    }

    final double[] distances = new double[] {-a1};
    final Vector3[] points =
        new Vector3[] {ray.getDirection().multiply(distances[0], new Vector3()).addLocal(ray.getOrigin())};
    return new IntersectionRecord(distances, points);
  }

  @Override
  public boolean contains(final ReadOnlyVector3 point) {
    return getCenter().distanceSquared(point) < (getRadius() * getRadius());
  }

  @Override
  public double distanceToEdge(final ReadOnlyVector3 point) {
    return _center.distance(point) - getRadius();
  }

  @Override
  public void write(final OutputCapsule capsule) throws IOException {
    super.write(capsule);
    try {
      capsule.write(getRadius(), "radius", 0);
    } catch (final IOException ex) {
      logger.logp(Level.SEVERE, this.getClass().toString(), "write(Ardor3DExporter)", "Exception", ex);
    }
  }

  @Override
  public void read(final InputCapsule capsule) throws IOException {
    super.read(capsule);
    try {
      setRadius(capsule.readDouble("radius", 0));
    } catch (final IOException ex) {
      logger.logp(Level.SEVERE, this.getClass().toString(), "read(Ardor3DImporter)", "Exception", ex);
    }
  }

  @Override
  public double getVolume() { return 4 * MathUtils.ONE_THIRD * MathUtils.PI * getRadius() * getRadius() * getRadius(); }
}
