/*
 * Copyright 2017 Google Inc. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package com.google.ar.core.examples.java.helloar;

import android.opengl.GLES20;
import android.opengl.GLSurfaceView;
import android.os.Bundle;
import android.support.v7.app.AppCompatActivity;
import android.util.Log;
import android.view.MotionEvent;
import android.widget.Toast;
import com.google.ar.core.Anchor;
import com.google.ar.core.ArCoreApk;
import com.google.ar.core.Camera;
import com.google.ar.core.Frame;
import com.google.ar.core.HitResult;
import com.google.ar.core.Plane;
import com.google.ar.core.Point;
import com.google.ar.core.Point.OrientationMode;
import com.google.ar.core.Pose;
import com.google.ar.core.Session;
import com.google.ar.core.Trackable;
import com.google.ar.core.TrackingState;
import com.google.ar.core.examples.java.common.framework.RWT.RWTRenderer;
import com.google.ar.core.examples.java.common.framework.gameMain.OvergroundActor;
import com.google.ar.core.examples.java.common.framework.model3D.CollisionResult;
import com.google.ar.core.examples.java.common.framework.model3D.Object3D;
import com.google.ar.core.examples.java.common.framework.model3D.Position3D;
import com.google.ar.core.examples.java.common.framework.model3D.Quaternion3D;
import com.google.ar.core.examples.java.common.framework.model3D.Universe;
import com.google.ar.core.examples.java.common.framework.physics.Ground;
import com.google.ar.core.examples.java.common.framework.physics.PhysicalSystem;
import com.google.ar.core.examples.java.common.framework.physics.Solid3D;
import com.google.ar.core.examples.java.common.framework.physics.Velocity3D;
import com.google.ar.core.examples.java.common.helpers.CameraPermissionHelper;
import com.google.ar.core.examples.java.common.helpers.DisplayRotationHelper;
import com.google.ar.core.examples.java.common.helpers.FullScreenHelper;
import com.google.ar.core.examples.java.common.helpers.SnackbarHelper;
import com.google.ar.core.examples.java.common.helpers.TapHelper;
import com.google.ar.core.examples.java.common.helpers.TrackingStateHelper;
import com.google.ar.core.examples.java.common.java3d.Appearance;
import com.google.ar.core.examples.java.common.java3d.Material;
import com.google.ar.core.examples.java.common.java3d.Shape3D;
import com.google.ar.core.examples.java.common.java3d.Sphere;
import com.google.ar.core.examples.java.common.java3d.TransparencyAttributes;
import com.google.ar.core.examples.java.common.java3d.TriangleArray;
import com.google.ar.core.examples.java.common.java3d.TriangleFanArray;
import com.google.ar.core.examples.java.common.java3d.Vector3d;
import com.google.ar.core.examples.java.common.rendering.BackgroundRenderer;
import com.google.ar.core.examples.java.common.rendering.ObjectRenderer;
import com.google.ar.core.examples.java.common.rendering.PlaneRenderer;
import com.google.ar.core.examples.java.common.rendering.PointCloudRenderer;
import com.google.ar.core.exceptions.CameraNotAvailableException;
import com.google.ar.core.exceptions.UnavailableApkTooOldException;
import com.google.ar.core.exceptions.UnavailableArcoreNotInstalledException;
import com.google.ar.core.exceptions.UnavailableDeviceNotCompatibleException;
import com.google.ar.core.exceptions.UnavailableSdkTooOldException;
import com.google.ar.core.exceptions.UnavailableUserDeclinedInstallationException;
import java.io.IOException;
import java.nio.FloatBuffer;
import java.util.ArrayList;
import java.util.Collection;

import javax.microedition.khronos.egl.EGLConfig;
import javax.microedition.khronos.opengles.GL10;

/**
 * This is a simple example that shows how to create an augmented reality (AR) application using the
 * ARCore API. The application will display any detected planes and will allow the user to tap on a
 * plane to place a 3d model of the Android robot.
 */
public class HelloArActivity extends AppCompatActivity {
  private static final String TAG = HelloArActivity.class.getSimpleName();

  // Rendering. The Renderers are created here, and initialized when the GL surface is created.
  private GLSurfaceView surfaceView;
  private RWTRenderer renderer;
  private Universe universe;
  private PhysicalSystem physicalSystem;
  private Ground ground = null;
  private Object3D polyObj;

  private boolean installRequested;

  private Session session;
  private final SnackbarHelper messageSnackbarHelper = new SnackbarHelper();
  private DisplayRotationHelper displayRotationHelper;
  private final TrackingStateHelper trackingStateHelper = new TrackingStateHelper(this);
  private TapHelper tapHelper;

  private final BackgroundRenderer backgroundRenderer = new BackgroundRenderer();
  private final ObjectRenderer virtualObject = new ObjectRenderer();
  private final ObjectRenderer virtualObjectShadow = new ObjectRenderer();
  private final PlaneRenderer planeRenderer = new PlaneRenderer();
  private final PointCloudRenderer pointCloudRenderer = new PointCloudRenderer();

  // Temporary matrix allocated here to reduce number of allocations for each frame.
  private final float[] anchorMatrix = new float[16];
  private static final float[] DEFAULT_COLOR = new float[] {0f, 0f, 0f, 0f};

  private static final String SEARCHING_PLANE_MESSAGE = "Searching for surfaces...";

  // Anchors created from taps used for object placing with a given color.
  private static class ColoredAnchor {
    public final Anchor anchor;
    public final float[] color;

    public ColoredAnchor(Anchor a, float[] color4f) {
      this.anchor = a;
      this.color = color4f;
    }
  }

  static class SortablePlane {
    final float distance;
    final Plane plane;

    SortablePlane(float distance, Plane plane) {
      this.distance = distance;
      this.plane = plane;
    }
  }

  private final ArrayList<ColoredAnchor> anchors = new ArrayList<>();

  private Collection<Plane> placedPlanes = new ArrayList<>();
  private Plane placedPlane = null;

  private float ambient = 0.3f;
  private float diffuse = 1.0f;
  private float specular = 1.0f;
  private float specularPower = 6.0f;

  @Override
  protected void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setContentView(R.layout.activity_main);
    surfaceView = findViewById(R.id.surfaceview);
    universe = new Universe();
    renderer = new RWTRenderer(this, backgroundRenderer, universe);
//    renderer.setBackgroundRenderer(backgroundRenderer);
    displayRotationHelper = new DisplayRotationHelper(/*context=*/ this);

    // Set up tap listener.
    tapHelper = new TapHelper(/*context=*/ this);
    surfaceView.setOnTouchListener(tapHelper);

    // Set up renderer.
    surfaceView.setPreserveEGLContextOnPause(true);
//    surfaceView.setEGLContextClientVersion(2);
    surfaceView.setEGLConfigChooser(8, 8, 8, 8, 16, 0); // Alpha used for plane blending.
    surfaceView.setRenderer(renderer);
//    surfaceView.setRenderMode(GLSurfaceView.RENDERMODE_WHEN_DIRTY);
    surfaceView.setRenderMode(GLSurfaceView.RENDERMODE_CONTINUOUSLY);
    surfaceView.setWillNotDraw(false);

    installRequested = false;

    // For Radish
//    physicalSystem = new PhysicalSystem();
//    try {
//      Object3D andy = ModelFactory.loadModel(getResources(), "models/andy.obj", null).createObject();
//      andy.setPosition(new Position3D(0.0, 0.0, -1.0));
//      universe.place(andy);
//    } catch (IOException e) {
//      e.printStackTrace();
//    } catch (ModelFileFormatException e) {
//      e.printStackTrace();
//    }
  }

  @Override
  protected void onResume() {
    super.onResume();

    if (session == null) {
      Exception exception = null;
      String message = null;
      try {
        switch (ArCoreApk.getInstance().requestInstall(this, !installRequested)) {
          case INSTALL_REQUESTED:
            installRequested = true;
            return;
          case INSTALLED:
            break;
        }

        // ARCore requires camera permissions to operate. If we did not yet obtain runtime
        // permission on Android M and above, now is a good time to ask the user for it.
        if (!CameraPermissionHelper.hasCameraPermission(this)) {
          CameraPermissionHelper.requestCameraPermission(this);
          return;
        }

        // Create the session.
        session = new Session(/* context= */ this);
        renderer.setSession(session);

      } catch (UnavailableArcoreNotInstalledException
              | UnavailableUserDeclinedInstallationException e) {
        message = "Please install ARCore";
        exception = e;
      } catch (UnavailableApkTooOldException e) {
        message = "Please update ARCore";
        exception = e;
      } catch (UnavailableSdkTooOldException e) {
        message = "Please update this app";
        exception = e;
      } catch (UnavailableDeviceNotCompatibleException e) {
        message = "This device does not support AR";
        exception = e;
      } catch (Exception e) {
        message = "Failed to create AR session";
        exception = e;
      }

      if (message != null) {
        messageSnackbarHelper.showError(this, message);
        Log.e(TAG, "Exception creating session", exception);
        return;
      }
    }

    // Note that order matters - see the note in onPause(), the reverse applies here.
    try {
      session.resume();
    } catch (CameraNotAvailableException e) {
      messageSnackbarHelper.showError(this, "Camera not available. Try restarting the app.");
      session = null;
      return;
    }

    surfaceView.onResume();
    displayRotationHelper.onResume();
  }

  @Override
  public void onPause() {
    super.onPause();
    if (session != null) {
      // Note that the order matters - GLSurfaceView is paused first so that it does not try
      // to query the session. If Session is paused before GLSurfaceView, GLSurfaceView may
      // still call session.update() and get a SessionPausedException.
      displayRotationHelper.onPause();
      surfaceView.onPause();
      session.pause();
    }
  }

  @Override
  public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
    if (!CameraPermissionHelper.hasCameraPermission(this)) {
      Toast.makeText(this, "Camera permission is needed to run this application", Toast.LENGTH_LONG)
              .show();
      if (!CameraPermissionHelper.shouldShowRequestPermissionRationale(this)) {
        // Permission denied with checking "Do not ask again".
        CameraPermissionHelper.launchPermissionSettings(this);
      }
      finish();
    }
  }

  @Override
  public void onWindowFocusChanged(boolean hasFocus) {
    super.onWindowFocusChanged(hasFocus);
    FullScreenHelper.setFullScreenOnWindowFocusChanged(this, hasFocus);
  }

  public void onSurfaceCreated(GL10 gl, EGLConfig config) {
    GLES20.glClearColor(0.1f, 0.1f, 0.1f, 1.0f);

    // Prepare the rendering objects. This involves reading shaders, so may throw an IOException.
    try {
      // Create the texture and pass it to ARCore session to be filled during update().
      backgroundRenderer.createOnGlThread(/*context=*/ this);
//      planeRenderer.createOnGlThread(/*context=*/ this, "models/trigrid.png");
//      pointCloudRenderer.createOnGlThread(/*context=*/ this);
//
//      virtualObject.createOnGlThread(/*context=*/ this, "models/andy.obj", "models/andy.png");
//      virtualObject.setMaterialProperties(0.0f, 2.0f, 0.5f, 6.0f);
//
//      virtualObjectShadow.createOnGlThread(
//          /*context=*/ this, "models/andy_shadow.obj", "models/andy_shadow.png");
//      virtualObjectShadow.setBlendMode(BlendMode.Shadow);
//      virtualObjectShadow.setMaterialProperties(1.0f, 0.0f, 0.0f, 1.0f);
//
    } catch (IOException e) {
      Log.e(TAG, "Failed to read an asset file", e);
    }
  }

  public void onSurfaceChanged(GL10 gl, int width, int height) {
    displayRotationHelper.onSurfaceChanged(width, height);
    GLES20.glViewport(0, 0, width, height);
  }

  public void onDrawFrame(GL10 gl, Frame frame, Camera camera) {
    // Clear screen to notify driver it should not load any pixels from previous frame.
//    GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT | GLES20.GL_DEPTH_BUFFER_BIT);

    if (session == null) {
      return;
    }
    // Notify ARCore session that the view size changed so that the perspective matrix and
    // the video background can be properly adjusted.
    displayRotationHelper.updateSessionIfNeeded(session);

    try {
//      session.setCameraTextureName(backgroundRenderer.getTextureId());

      // Obtain the current frame from ARSession. When the configuration is set to
      // UpdateMode.BLOCKING (it is by default), this will throttle the rendering to the
      // camera framerate.
//      Frame frame = session.update();
//      Camera camera = frame.getCamera();

      // Handle one tap per frame.
      handleTap(frame, camera);

      // Physics
      universe.update(10);
//      if (physicalSystem.objects.size() > 0) {
//        physicalSystem.motion(0, 10, Force3D.ZERO, physicalSystem.objects.get(0).getGravityCenter(), ground);
//      }

      // If frame is ready, render camera preview image to the GL surface.
//      backgroundRenderer.draw(frame);

      // Keep the screen unlocked while tracking, but allow it to lock when tracking stops.
      trackingStateHelper.updateKeepScreenOnFlag(camera.getTrackingState());

      // If not tracking, don't draw 3D objects, show tracking failure reason instead.
      if (camera.getTrackingState() == TrackingState.PAUSED) {
        messageSnackbarHelper.showMessage(
                this, TrackingStateHelper.getTrackingFailureReasonString(camera));
        return;
      }

      // Get projection matrix.
      float[] projmtx = new float[16];
      camera.getProjectionMatrix(projmtx, 0, 0.1f, 100.0f);
      // Get camera matrix and draw.
      float[] viewmtx = new float[16];
      camera.getViewMatrix(viewmtx, 0);

      // Compute lighting from average intensity of the image.
      // The first three components are color scaling factors.
      // The last one is the average pixel intensity in gamma space.
      final float[] colorCorrectionRgba = new float[4];
      frame.getLightEstimate().getColorCorrection(colorCorrectionRgba, 0);

//      // Visualize tracked points.
//      // Use try-with-resources to automatically release the point cloud.
//      try (PointCloud pointCloud = frame.acquirePointCloud()) {
//        pointCloudRenderer.update(pointCloud);
//        pointCloudRenderer.draw(viewmtx, projmtx);
//      }

      // No tracking error at this point. If we detected any plane, then hide the
      // message UI, otherwise show searchingPlane message.
      if (hasTrackingPlane()) {
        messageSnackbarHelper.hide(this);
      } else {
        messageSnackbarHelper.showMessage(this, SEARCHING_PLANE_MESSAGE);
      }

      // Visualize planes.
      Collection<Plane> updatedPlanes = frame.getUpdatedTrackables(Plane.class);
      for (Plane plane : updatedPlanes) {
        if (placedPlane == null) {
          placedPlane = plane;
          Appearance ap = new Appearance();
          ap.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.5f));
          FloatBuffer vertices = plane.getPolygon();
          vertices.position(0);
          TriangleFanArray polygon = new TriangleFanArray(vertices.remaining() / 2, TriangleArray.COORDINATES, new int[]{vertices.remaining() / 2});
          for (int i = 0; i < vertices.remaining() / 2; i++) {
            polygon.setCoordinate(i, new float[]{vertices.get(i * 2), 0.0f, vertices.get(i * 2 + 1)});
          }
          polyObj = new Object3D("plane", new Shape3D(polygon, ap));
          Pose p = plane.getCenterPose();
          polyObj.apply(new Position3D(p.tx(), p.ty(), p.tz()), false);
          polyObj.apply(new Quaternion3D((double) p.qx(), (double) p.qy(), (double) p.qz(), (double) p.qw()), false);
          ground = new Ground(polyObj);
          universe.place(ground);
        } else if (plane.equals(placedPlane)) {
          Appearance ap = new Appearance();
          ap.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.5f));
          FloatBuffer vertices = plane.getPolygon();
          vertices.position(0);
          TriangleFanArray polygon = new TriangleFanArray(vertices.remaining() / 2, TriangleArray.COORDINATES, new int[]{vertices.remaining() / 2});
          for (int i = 0; i < vertices.remaining() / 2; i++) {
            polygon.setCoordinate(i, new float[]{vertices.get(i * 2), 0.0f, vertices.get(i * 2 + 1)});
          }
          polyObj = new Object3D("plane", new Shape3D(polygon, ap));
          Pose p = plane.getCenterPose();
          polyObj.apply(new Position3D(p.tx(), p.ty(), p.tz()), false);
          polyObj.apply(new Quaternion3D((double) p.qx(), (double) p.qy(), (double) p.qz(), (double) p.qw()), false);
          universe.displace(ground);
          ground = new Ground(polyObj);
          universe.place(ground);
        }
      }

/////////////////////////////////////////////////////////////////////////////////////
// 以下は複数検出面に対応したコードだが、検出面が2個以上になるとちゃんと動かない?
//
//      Collection<Plane> updatedPlanes = frame.getUpdatedTrackables(Plane.class);
//      if (updatedPlanes != null && updatedPlanes.size() > 0) {
//        if (ground != null) universe.displace(ground);
//        Object3D polygons[] = new Object3D[updatedPlanes.size()];
//        int i = 0;
//        for (Plane plane : updatedPlanes) {
//          Appearance ap = new Appearance();
//          ap.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.5f));
//          FloatBuffer vertices = plane.getPolygon();
//          vertices.position(0);
//          TriangleFanArray polygon = new TriangleFanArray(vertices.remaining() / 2, TriangleArray.COORDINATES, new int[]{vertices.remaining() / 2});
//          for (int j = 0; j < vertices.remaining() / 2; j++) {
//            polygon.setCoordinate(j, new float[]{vertices.get(j * 2), 0.0f, vertices.get(j * 2 + 1)});
//          }
//          Object3D polyObj = new Object3D("plane" + i, new Shape3D(polygon, ap));
//          Pose p = plane.getCenterPose();
//          polyObj.apply(new Position3D(p.tx(), p.ty(), p.tz()), false);
//          polyObj.apply(new Quaternion3D((double) p.qx(), (double) p.qy(), (double) p.qz(), (double) p.qw()), false);
//          polygons[i] = polyObj;
//          i++;
//        }
//        ground = new Ground(new Object3D("ground", polygons));
//        universe.place(ground);
//      }
/////////////////////////////////////////////////////////////////////////////////////


//      planeRenderer.drawPlanes(
//          session.getAllTrackables(Plane.class), camera.getDisplayOrientedPose(), projmtx);
//
      // Visualize anchors created by touch.
//        float scaleFactor = 1.0f;
//        for (ColoredAnchor coloredAnchor : anchors) {
//          if (coloredAnchor.anchor.getTrackingState() != TrackingState.TRACKING) {
//            continue;
//          }
//        // Get the current pose of an Anchor in world space. The Anchor pose is updated
//        // during calls to session.update() as ARCore refines its estimate of the world.
//        coloredAnchor.anchor.getPose().toMatrix(anchorMatrix, 0);
//
//        // Update and draw the model and its shadow.
//        virtualObject.updateModelMatrix(anchorMatrix, scaleFactor);
//        virtualObjectShadow.updateModelMatrix(anchorMatrix, scaleFactor);
//        virtualObject.draw(viewmtx, projmtx, colorCorrectionRgba, coloredAnchor.color);
//        virtualObjectShadow.draw(viewmtx, projmtx, colorCorrectionRgba, coloredAnchor.color);
//
//      }

    } catch(Throwable t){
      // Avoid crashing the application due to unhandled exceptions.
      Log.e(TAG, "Exception on the OpenGL thread", t);
    }
  }


  // Handle only one tap per frame, as taps are usually low frequency compared to frame rate.
  private void handleTap(Frame frame, Camera camera) {
    MotionEvent tap = tapHelper.poll();
    if (tap != null && camera.getTrackingState() == TrackingState.TRACKING) {
      for (HitResult hit : frame.hitTest(tap)) {
        // Check if any plane was hit, and if it was hit inside the plane polygon
        Trackable trackable = hit.getTrackable();
        // Creates an anchor if a plane or an oriented point was hit.
        if ((trackable instanceof Plane
                && ((Plane) trackable).isPoseInPolygon(hit.getHitPose())
                && (PlaneRenderer.calculateDistanceToPlane(hit.getHitPose(), camera.getPose()) > 0))
                || (trackable instanceof Point
                && ((Point) trackable).getOrientationMode()
                == OrientationMode.ESTIMATED_SURFACE_NORMAL)) {
          // Hits are sorted by depth. Consider only closest hit on a plane or oriented point.
          // Cap the number of objects created. This avoids overloading both the
          // rendering system and ARCore.
          if (anchors.size() >= 20) {
            anchors.get(0).anchor.detach();
            anchors.remove(0);
          }

          // Assign a color to the object for rendering based on the trackable type
          // this anchor attached to. For AR_TRACKABLE_POINT, it's blue color, and
          // for AR_TRACKABLE_PLANE, it's green color.
          float[] objColor;
          if (trackable instanceof Point) {
            objColor = new float[] {66.0f, 133.0f, 244.0f, 255.0f};
          } else if (trackable instanceof Plane) {
            objColor = new float[] {139.0f, 195.0f, 74.0f, 255.0f};
          } else {
            objColor = DEFAULT_COLOR;
          }

          // Adding an Anchor tells ARCore that it should track this position in
          // space. This anchor is created on the Plane to place the 3D model
          // in the correct position relative both to the world and to the plane.
          ColoredAnchor anchor = new ColoredAnchor(hit.createAnchor(), objColor);

//          double px = anchor.anchor.getPose().tx();
//          double py = anchor.anchor.getPose().ty();
//          double pz = anchor.anchor.getPose().tz();

//          try {
//            Object3D andy = ModelFactory.loadModel(getResources(), "models/andy.obj", null).createObject();
          Appearance ap = new Appearance();
          Material m = new Material();
          ap.setMaterial(m);
          Object3D andy = new Object3D("andy", new Sphere(0.05f, ap));
//            andy.setPosition(new Position3D(px, py + 0.2, pz));
            float viewPoint[] = camera.getDisplayOrientedPose().getTranslation();
            andy.setPosition(new Position3D(viewPoint[0], viewPoint[1], viewPoint[2]));
            OvergroundActor andyObj = new OvergroundActor(andy, null) {
              public void onIntersect(CollisionResult cr, long interval) {
                // めり込んだら（めり込んだ面の法線方向に）押し戻す
                Vector3d back = (Vector3d) cr.normal.clone();
                back.scale(cr.length * 2.0);
                body.apply(new Position3D(body.getPosition3D().add(back)), false);

                // 速度の面の法線方向の成分を 0 にする
                Vector3d v = (Vector3d) ((Solid3D)body).getVelocity().getVector3d().clone();
                double d = v.dot(cr.normal);
                v.scaleAdd(-d * 1.5, cr.normal, v);
                body.apply(new Velocity3D(v), false);
              }
            };
            float viewLine[] = camera.getDisplayOrientedPose().getZAxis();
            andyObj.setVelocity(new Velocity3D(-viewLine[0], -viewLine[1], -1.7 * viewLine[2]));
//            Solid3D andyObj = new Solid3D(andy);
            universe.place(andyObj);
//            physicalSystem.add(andyObj);
//          } catch (IOException e) {
//            e.printStackTrace();
//          } catch (ModelFileFormatException e) {
//            e.printStackTrace();
//          }
//            Box b1 = new Box(1.0f, 1.0f, 1.0f, null);
//            Object3D obj1 = new Object3D("box", b1);
//            float viewPoint[] = camera.getDisplayOrientedPose().getTranslation();
//            obj1.setPosition(new Position3D( viewPoint[0], viewPoint[1], viewPoint[2]));
//            Solid3D diceObj = new Solid3D(obj1);
//            diceObj.scale(0.01, 0.01, 0.01);
//            universe.place(diceObj);
//            physicalSystem.add(diceObj);
          anchors.add(anchor);
          break;
        }
      }
    }
  }

  /** Checks if we detected at least one plane. */
  private boolean hasTrackingPlane() {
    for (Plane plane : session.getAllTrackables(Plane.class)) {
      if (plane.getTrackingState() == TrackingState.TRACKING) {
        return true;
      }
    }
    return false;
  }

  public static float calculateDistanceToPlane(Pose planePose, Pose cameraPose) {
    float[] normal = new float[3];
    float cameraX = cameraPose.tx();
    float cameraY = cameraPose.ty();
    float cameraZ = cameraPose.tz();
    // Get transformed Y axis of plane's coordinate system.
    planePose.getTransformedAxis(1, 1.0f, normal, 0);
    // Compute dot product of plane's normal with vector from camera to plane center.
    return (cameraX - planePose.tx()) * normal[0]
            + (cameraY - planePose.ty()) * normal[1]
            + (cameraZ - planePose.tz()) * normal[2];
  }
}
