bundles/core/org.eclipse.apogy.core.ui/src/org/eclipse/apogy/core/ui/composites/DeltaPoseComposite.java

/*******************************************************************************
 * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency 
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     Pierre Allard,
 *     Regent L'Archeveque - initial API and implementation
 *  
 * SPDX-License-Identifier: EPL-1.0
 *******************************************************************************/
package org.eclipse.apogy.core.ui.composites;

import javax.vecmath.Point3d;

import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
import org.eclipse.apogy.common.math.Matrix4x4;
import org.eclipse.apogy.common.math.Tuple3d;
import org.eclipse.apogy.common.math.ui.composites.Tuple3dComposite;
import org.eclipse.apogy.core.ApogyCorePackage;
import org.eclipse.apogy.core.ApogySystemApiAdapter;
import org.eclipse.apogy.core.PoseProvider;
import org.eclipse.emf.common.notify.Adapter;
import org.eclipse.emf.common.notify.Notification;
import org.eclipse.emf.common.notify.impl.AdapterImpl;
import org.eclipse.swt.SWT;
import org.eclipse.swt.events.DisposeEvent;
import org.eclipse.swt.events.DisposeListener;
import org.eclipse.swt.layout.GridData;
import org.eclipse.swt.layout.GridLayout;
import org.eclipse.swt.widgets.Composite;
import org.eclipse.swt.widgets.Label;
import org.eclipse.swt.widgets.Text;

import com.ibm.icu.text.DecimalFormat;

public class DeltaPoseComposite extends Composite {
	private Adapter poseAdapter;
	private ApogySystemApiAdapter variable1ApogySystemApiAdapter;
	private ApogySystemApiAdapter variable2ApogySystemApiAdapter;

	private final Text textDistance;
	private final Tuple3dComposite deltaDistanceComposite;
	private final Tuple3dComposite deltaOrientationComposite;

	private final DecimalFormat distanceFormat = new DecimalFormat("0.000");

	public DeltaPoseComposite(Composite parent, int style) {
		super(parent, style);
		setLayout(new GridLayout(2, false));

		Label lblDistancem = new Label(this, SWT.NONE);
		lblDistancem.setAlignment(SWT.RIGHT);
		lblDistancem.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
		lblDistancem.setText("Distance (m):");

		this.textDistance = new Text(this, SWT.BORDER);
		this.textDistance.setToolTipText("Distance between the variables, in meters.");
		this.textDistance.setEditable(false);
		this.textDistance.setLayoutData(new GridData(SWT.FILL, SWT.CENTER, true, false, 1, 1));

		Label lblPositionDistancem = new Label(this, SWT.NONE);
		lblPositionDistancem.setAlignment(SWT.RIGHT);
		lblPositionDistancem.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
		lblPositionDistancem.setText("Position Delta (m):");

		this.deltaDistanceComposite = new Tuple3dComposite(this, SWT.NONE);
		this.deltaDistanceComposite.setLayoutData(new GridData(SWT.FILL, SWT.CENTER, false, false, 1, 1));
		this.deltaDistanceComposite.setEnableEditing(false);

		Label lblOrientationDeltadeg = new Label(this, SWT.NONE);
		lblOrientationDeltadeg.setAlignment(SWT.RIGHT);
		lblOrientationDeltadeg.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
		lblOrientationDeltadeg.setText("Orientation Delta (deg):");

		this.deltaOrientationComposite = new Tuple3dComposite(this, SWT.NONE, "0.000");
		this.deltaOrientationComposite.setLayoutData(new GridData(SWT.FILL, SWT.CENTER, false, false, 1, 1));
		this.deltaOrientationComposite.setEnableEditing(false);

		setDisplaysEnabled(false);

		addDisposeListener(new DisposeListener() {

			@Override
			public void widgetDisposed(DisposeEvent e) {
				if (DeltaPoseComposite.this.variable1ApogySystemApiAdapter != null)
					DeltaPoseComposite.this.variable1ApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());
				if (DeltaPoseComposite.this.variable2ApogySystemApiAdapter != null)
					DeltaPoseComposite.this.variable2ApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());
			}
		});
	}

	public ApogySystemApiAdapter getVariable1ApogySystemApiAdapter() {
		return this.variable1ApogySystemApiAdapter;
	}

	public void setVariable1ApogySystemApiAdapter(ApogySystemApiAdapter apogySystemApiAdapter) {
		if (this.variable1ApogySystemApiAdapter != null)
			this.variable1ApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());

		this.variable1ApogySystemApiAdapter = apogySystemApiAdapter;

		if (this.variable1ApogySystemApiAdapter != null)
			this.variable1ApogySystemApiAdapter.eAdapters().add(getPoseAdapter());

		updatePose();
	}

	public ApogySystemApiAdapter getVariable2ApogySystemApiAdapter() {
		return this.variable2ApogySystemApiAdapter;
	}

	public void setVariable2ApogySystemApiAdapter(ApogySystemApiAdapter apogySystemApiAdapter) {
		if (this.variable2ApogySystemApiAdapter != null)
			this.variable2ApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());

		this.variable2ApogySystemApiAdapter = apogySystemApiAdapter;

		if (this.variable2ApogySystemApiAdapter != null)
			this.variable2ApogySystemApiAdapter.eAdapters().add(getPoseAdapter());

		updatePose();
	}

	protected void setDisplaysEnabled(boolean enabled) {
		if (this.textDistance != null && !this.textDistance.isDisposed())
			this.textDistance.setEnabled(enabled);
		if (this.deltaDistanceComposite != null && !this.deltaDistanceComposite.isDisposed())
			this.deltaDistanceComposite.setEnabled(enabled);
		if (this.deltaOrientationComposite != null && !this.deltaOrientationComposite.isDisposed())
			this.deltaOrientationComposite.setEnabled(enabled);
	}

	protected void updatePose() {
		if (!isDisposed()) {
			// Do this on UI Thread.
			getDisplay().asyncExec(new Runnable() {
				@Override
				public void run() {
					setDisplaysEnabled(DeltaPoseComposite.this.variable1ApogySystemApiAdapter != null
							&& DeltaPoseComposite.this.variable2ApogySystemApiAdapter != null);

					if (DeltaPoseComposite.this.variable1ApogySystemApiAdapter != null
							&& DeltaPoseComposite.this.variable2ApogySystemApiAdapter != null) {
						Matrix4x4 variable1Pose = DeltaPoseComposite.this.variable1ApogySystemApiAdapter
								.getPoseTransform();
						if (variable1Pose == null)
							variable1Pose = ApogyCommonMathFacade.INSTANCE.createIdentityMatrix4x4();

						Matrix4x4 variable2Pose = DeltaPoseComposite.this.variable2ApogySystemApiAdapter
								.getPoseTransform();
						if (variable2Pose == null)
							variable2Pose = ApogyCommonMathFacade.INSTANCE.createIdentityMatrix4x4();

						Tuple3d variable1Position = ApogyCommonMathFacade.INSTANCE.extractPosition(variable1Pose);
						Point3d p1 = new Point3d(variable1Position.asTuple3d());

						Tuple3d variable2Position = ApogyCommonMathFacade.INSTANCE.extractPosition(variable2Pose);
						Point3d p2 = new Point3d(variable2Position.asTuple3d());

						// Updates Distance.
						double distance = p1.distance(p2);
						if (DeltaPoseComposite.this.textDistance != null
								&& !DeltaPoseComposite.this.textDistance.isDisposed())
							DeltaPoseComposite.this.textDistance
									.setText(DeltaPoseComposite.this.distanceFormat.format(distance));

						double deltaX = p1.x - p2.x;
						double deltaY = p1.y - p2.y;
						double deltaZ = p1.z - p2.z;

						// Updates Delta Position.
						Tuple3d deltaPosition = ApogyCommonMathFacade.INSTANCE.createTuple3d(deltaX, deltaY, deltaZ);
						if (DeltaPoseComposite.this.deltaDistanceComposite != null
								&& !DeltaPoseComposite.this.deltaDistanceComposite.isDisposed())
							DeltaPoseComposite.this.deltaDistanceComposite.setTuple3d(deltaPosition);

						// TODO Updates Delta Orientation.

					}
				}
			});
		}
	}

	protected Adapter getPoseAdapter() {
		if (this.poseAdapter == null) {
			this.poseAdapter = new AdapterImpl() {
				@Override
				public void notifyChanged(Notification msg) {
					if (msg.getNotifier() instanceof PoseProvider) {
						int featureID = msg.getFeatureID(PoseProvider.class);
						switch (featureID) {
						case ApogyCorePackage.POSE_PROVIDER__POSE_TRANSFORM:
							if (msg.getNewValue() instanceof Matrix4x4) {
								updatePose();
							}
							break;

						default:
							break;
						}
					}
				}
			};
		}

		return this.poseAdapter;
	}
}