/*
 * Copyright 2017 Intermodalics 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 eu.intermodalics.tango_ros_streamer;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

import org.apache.commons.logging.Log;
import org.ros.message.MessageListener;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;
import org.ros.node.topic.Subscriber;

import geometry_msgs.TransformStamped;
import sensor_msgs.Imu;
import tf2_msgs.TFMessage;

/**
 *
 */
public class ImuNode extends AbstractNodeMain implements NodeMain, SensorEventListener {
    private static final String NODE_NAME = "android";

    private ConnectedNode mConnectedNode;
    private Publisher<Imu> mImuPublisher;
    private Imu mImuMessage;
    private Log mLog;
    private Subscriber<TFMessage> mTfListener;
    private Time lastTfTimestamp;


    private SensorManager mSensorManager;
    private Sensor mRotationSensor;
    private Sensor mGyroscopeSensor;
    private Sensor mAccelerometerSensor;
    private boolean mNewRotationData = false;
    private boolean mNewGyroscopeData = false;
    private boolean mNewAccelerometerData = false;

    public ImuNode(Activity activity) {
        mSensorManager = (SensorManager) activity.getSystemService(Context.SENSOR_SERVICE);
        mRotationSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        mGyroscopeSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        mAccelerometerSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
    }

    public GraphName getDefaultNodeName() { return GraphName.of(NODE_NAME); }

    public void onStart(ConnectedNode connectedNode) {
        mConnectedNode = connectedNode;
        mLog = connectedNode.getLog();
        mImuPublisher = connectedNode.newPublisher("android/imu", Imu._TYPE);
        mImuMessage = mConnectedNode.getTopicMessageFactory().newFromType(Imu._TYPE);

        mSensorManager.registerListener(this, mRotationSensor, SensorManager.SENSOR_DELAY_FASTEST);
        mSensorManager.registerListener(this, mGyroscopeSensor, SensorManager.SENSOR_DELAY_FASTEST);
        mSensorManager.registerListener(this, mAccelerometerSensor, SensorManager.SENSOR_DELAY_FASTEST);

        mTfListener = mConnectedNode.newSubscriber("/tf", TFMessage._TYPE);
        mTfListener.addMessageListener(new MessageListener<TFMessage>() {
            @Override
            public void onNewMessage(TFMessage tfMessage) {
                for (TransformStamped transformStamped: tfMessage.getTransforms()) {
                    if (transformStamped.getHeader().getFrameId().equals("start_of_service")
                            && transformStamped.getChildFrameId().equals("device")) {
                        lastTfTimestamp = transformStamped.getHeader().getStamp();
                    }
                }
            }
        });
    }

    @Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
        // Not used.
	}

    @Override
    public void onSensorChanged(SensorEvent event) {
        switch (event.sensor.getType()) {
            case Sensor.TYPE_ROTATION_VECTOR:
                mNewRotationData = true;
                float[] quaternion = new float[4];
                SensorManager.getQuaternionFromVector(quaternion, event.values);
                mImuMessage.getOrientation().setW(quaternion[0]);
                mImuMessage.getOrientation().setX(quaternion[1]);
                mImuMessage.getOrientation().setY(quaternion[2]);
                mImuMessage.getOrientation().setZ(quaternion[3]);
                mImuMessage.setOrientationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
                break;
            case Sensor.TYPE_GYROSCOPE:
                mNewGyroscopeData = true;
                mImuMessage.getAngularVelocity().setX(event.values[0]);
                mImuMessage.getAngularVelocity().setY(event.values[1]);
                mImuMessage.getAngularVelocity().setZ(event.values[2]);
                mImuMessage.setAngularVelocityCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
                break;
            case Sensor.TYPE_ACCELEROMETER:
                mNewAccelerometerData = true;
                mImuMessage.getLinearAcceleration().setX(event.values[0]);
                mImuMessage.getLinearAcceleration().setY(event.values[1]);
                mImuMessage.getLinearAcceleration().setZ(event.values[2]);
                mImuMessage.setLinearAccelerationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
                break;
            default:
                break;
        }

        if (lastTfTimestamp != null && mNewRotationData && mNewGyroscopeData && mNewAccelerometerData) {
            mImuMessage.getHeader().setStamp(lastTfTimestamp);
            mImuMessage.getHeader().setFrameId("imu");
            mImuPublisher.publish(mImuMessage);
            mNewRotationData = false;
            mNewGyroscopeData = false;
            mNewAccelerometerData = false;
        }
    }
}

@@ -917,77 +917,8 @@
 og;%0A
-import org.ros.message.MessageListener;%0Aimport org.ros.message.Time;%0A
 impo
@@ -1093,135 +1093,31 @@
 er;%0A
-import org.ros.node.topic.Subscriber;%0A%0Aimport geometry_msgs.TransformStamped;%0Aimport sensor_msgs.Imu;%0Aimport tf2_msgs.TFMessage
+%0Aimport sensor_msgs.Imu
 ;%0A%0A/
@@ -1410,89 +1410,8 @@
 og;%0A
-    private Subscriber%3CTFMessage%3E mTfListener;%0A    private Time lastTfTimestamp;%0A
 %0A%0A  
@@ -2797,654 +2797,8 @@
 ST);
-%0A%0A        mTfListener = mConnectedNode.newSubscriber(%22/tf%22, TFMessage._TYPE);%0A        mTfListener.addMessageListener(new MessageListener%3CTFMessage%3E() %7B%0A            @Override%0A            public void onNewMessage(TFMessage tfMessage) %7B%0A                for (TransformStamped transformStamped: tfMessage.getTransforms()) %7B%0A                    if (transformStamped.getHeader().getFrameId().equals(%22start_of_service%22)%0A                            && transformStamped.getChildFrameId().equals(%22device%22)) %7B%0A                        lastTfTimestamp = transformStamped.getHeader().getStamp();%0A                    %7D%0A                %7D%0A            %7D%0A        %7D);
 %0A   
@@ -4543,35 +4543,8 @@
 if (
-lastTfTimestamp != null && 
 mNew
@@ -4654,23 +4654,39 @@
 amp(
-lastTfTimestamp
+mConnectedNode.getCurrentTime()
 );%0A 

Set imu timestamp as current ros time