/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Wim Meeussen */

// #include <tf2_ros/transform_listener.h>
// #include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <data_convert/data_convert.h>

tf3::BufferCore* tf_buffer;
tf3::TransformStampedMsg t;
static const double EPS = 1e-3;


TEST(TfGeometry, Frame)
{
  robot_geometry_msgs::PoseStamped v1;
  v1.pose.position.x = 1;
  v1.pose.position.y = 2;
  v1.pose.position.z = 3;
  v1.pose.orientation.x = 1;
  v1.pose.orientation.y = 0;
  v1.pose.orientation.z = 0;
  v1.pose.orientation.w = 0;
  v1.header.stamp = robot::Time(2.0);
  v1.header.frame_id = "A";

  // simple api
  // robot_geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
  // std::cout << "STEP 2" << std::endl;
  robot_geometry_msgs::PoseStamped v_simple = v1;
  // std::cout << "lookupTransform call:" << std::endl;
  
  tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform(
                "B",                        // frame đích
                v1.header.frame_id,         // frame nguồn
                // data_convert::convertTime(v1.header.stamp)
                tf3::Time()
              );
// std::cout << "STEP 3" << std::endl;
  tf3::doTransform(v1, v_simple, tfm);
  // std::cout << "STEP 4" << std::endl;
// std::cout << v_simple.pose.position.x << " , "
// << v_simple.pose.position.y << " , "
// << v_simple.pose.position.z << " , "
// << v_simple.pose.orientation.x << " , "
// << v_simple.pose.orientation.y << " , "
// << v_simple.pose.orientation.z << " , "
// << v_simple.pose.orientation.w << std::endl;

  EXPECT_NEAR(v_simple.pose.position.x, -9, EPS);
  EXPECT_NEAR(v_simple.pose.position.y, 18, EPS);
  EXPECT_NEAR(v_simple.pose.position.z, 27, EPS);
  EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
  
  // advanced api
    // robot_geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
    //                   "A", ros::Duration(3.0));

    tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
                        "B",                      // frame đích
                        "A",                      // frame nguồn
                        data_convert::convertTime(robot::Time(2.0))
                    );

    robot_geometry_msgs::PoseStamped v_advanced = v1;   // hoặc v1
    tf3::doTransform(v1, v_advanced, tfm1);
    // std::cout << v_advanced.pose.position.x << " , "
    // << v_advanced.pose.position.y << " , "
    // << v_advanced.pose.position.z << " , "
    // << v_advanced.pose.orientation.x << " , "
    // << v_advanced.pose.orientation.y << " , "
    // << v_advanced.pose.orientation.z << " , "
    // << v_advanced.pose.orientation.w << std::endl;

  EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS);
  EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS);
  EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS);
  EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS);
}

TEST(TfGeometry, PoseWithCovarianceStamped)
{
  robot_geometry_msgs::PoseWithCovarianceStamped v1;
  v1.pose.pose.position.x = 1;
  v1.pose.pose.position.y = 2;
  v1.pose.pose.position.z = 3;
  v1.pose.pose.orientation.x = 1;
  v1.pose.pose.orientation.y = 0;
  v1.pose.pose.orientation.z = 0;
  v1.pose.pose.orientation.w = 0;
  v1.header.stamp = robot::Time(2);
  v1.header.frame_id = "A";
  for(int i = 0; i < 36; i++) {
    v1.pose.covariance[i] = 0.0;
  }
  v1.pose.covariance[0] = 1;
  v1.pose.covariance[7] = 1;
  v1.pose.covariance[14] = 1;
  v1.pose.covariance[21] = 1;
  v1.pose.covariance[28] = 1;
  v1.pose.covariance[35] = 1;
  
  
  // simple api
  robot_geometry_msgs::PoseWithCovarianceStamped v_simple;
  tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
                        "B",                      // frame đích
                        "A",                      // frame nguồn
                        data_convert::convertTime(robot::Time(2.0))
                    );
  tf3::doTransform(v1, v_simple, tfm1);
  std::cout << v_simple.pose.pose.position.x << " , "
<< v_simple.pose.pose.position.y << " , "
<< v_simple.pose.pose.position.z << " , "
<< v_simple.pose.pose.orientation.x << " , "
<< v_simple.pose.pose.orientation.y << " , "
<< v_simple.pose.pose.orientation.z << " , "
<< v_simple.pose.pose.orientation.w << std::endl;

  EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS);
  EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS);
  EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS);
  EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS);
  EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS);
  
  // no rotation in this transformation, so no change to covariance
  EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS);
  EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS);
  EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS);
  EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS);
  EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS);
  EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
  
  // advanced api
  robot_geometry_msgs::PoseWithCovarianceStamped v_advanced;
  tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
                        "B",                      // frame đích
                        "A",                      // frame nguồn
                        data_convert::convertTime(robot::Time(2.0))
                    );

    tf3::doTransform(v1, v_advanced, tfm2);
  // const robot_geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
	// 						      "A", ros::Duration(3.0));
  std::cout << v_advanced.pose.pose.position.x << " , "
<< v_advanced.pose.pose.position.y << " , "
<< v_advanced.pose.pose.position.z << " , "
<< v_advanced.pose.pose.orientation.x << " , "
<< v_advanced.pose.pose.orientation.y << " , "
<< v_advanced.pose.pose.orientation.z << " , "
<< v_advanced.pose.pose.orientation.w << std::endl;
  EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS);
  EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS);
  
  // no rotation in this transformation, so no change to covariance
  EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS);
  EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS);
  EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS);
  EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS);
  EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS);
  EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS);
  
  /** now add rotation to transform to test the effect on covariance **/
  
  // rotate pi/2 radians about x-axis
  robot_geometry_msgs::TransformStamped t_rot;
  tf3::Quaternion q_rot = tf3::Quaternion(tf3::Vector3(1,0,0), M_PI/2);
  t_rot.transform.rotation.x = q_rot.x();
  t_rot.transform.rotation.y = q_rot.y();
  t_rot.transform.rotation.z = q_rot.z();
  t_rot.transform.rotation.w = q_rot.w();
  t_rot.header.stamp = robot::Time(2.0);
  t_rot.header.frame_id = "A";
  t_rot.child_frame_id = "rotated";
  tf3::TransformStampedMsg t_rot_msg = data_convert::convertToTransformStampedMsg(t_rot);
  tf_buffer->setTransform(t_rot_msg, "rotation_test");
  
  // need to put some covariance in the matrix
  v1.pose.covariance[1] = 1;
  v1.pose.covariance[6] = 1;
  v1.pose.covariance[12] = 1;
  
  // perform rotation
  robot_geometry_msgs::PoseWithCovarianceStamped v_rotated;
  tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform(
                        "rotated",                      // frame đích
                        "A",                      // frame nguồn
                        data_convert::convertTime(robot::Time(2.0))
                    );

    tf3::doTransform(v1, v_rotated, tfm3);

  // the covariance matrix should now be transformed
  EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS);
  EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS);
  
  // set buffer back to original transform
  tf_buffer->setTransform(t, "test");
}


int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);

  // std::vector<int> data = {2,2,7,4,5};
  // for(int i = 0; i < data.size(); i++) {
  //   std::cout << data.size() << std::endl;
  //    std::cout<< i << " " << data[i] << std::endl;
  // }

  tf_buffer = new tf3::BufferCore();
  tf_buffer->setUsingDedicatedThread(true);
  // std::cout << "STEP 1" << std::endl;
  // populate buffer
  t.transform.translation.x = 10;
  t.transform.translation.y = 20;
  t.transform.translation.z = 30;
  t.transform.rotation.x = 1;
  t.transform.rotation.y = 0;
  t.transform.rotation.z = 0;
  t.transform.rotation.w = 0;

  t.header.stamp = data_convert::convertTime(robot::Time(2.0));
  t.header.frame_id = "A";
  t.child_frame_id = "B";
  tf_buffer->setTransform(t, "test");

  int ret = RUN_ALL_TESTS();
  delete tf_buffer;
  return ret;
}





