/**
 * Receives the messages from the IMU
 */
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  // get RPY
  double roll, pitch, yaw;

  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);

  tf::Quaternion transformation_q;
  // -90 in y
  transformation_q.setRotation( tf::Vector3(0,1,0), -tfScalar(PI/2));

  tf::Quaternion parsed_orientation = orientation.operator*=(transformation_q);

  geometry_msgs::PoseStamped imuVis;
  // publish the imu quaternion
  imuVis.pose.orientation.x = parsed_orientation.x();
  imuVis.pose.orientation.y = parsed_orientation.y();
  imuVis.pose.orientation.z = parsed_orientation.z();
  imuVis.pose.orientation.w = parsed_orientation.w();
  imuVis.pose.position.x = 0;
  imuVis.pose.position.y = 0;
  imuVis.pose.position.z = 0;
  
  imuVis.header.stamp = imuIn->header.stamp;
  imuVis.header.frame_id = "/camera_init_2";
  pubImuVisPointer->publish(imuVis);

  tf::Matrix3x3(parsed_orientation).getRPY(roll, pitch, yaw);

  int imuPointerBack = imuPointerLast; // initially -1
  imuPointerLast = (imuPointerLast + 1) % imuQueLength; // the array pointer 0 - imuQueueLength (100 in MS case)
  imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); // the time the imu measurement was taken for each of the imuQueueLength (100) points
  double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; // difference in time between the two imu measurement

  // if two consecutive measurement were taken within 0.1 seconds
  if (timeDiff < 0.1) {

    // imuAccuRoll += timeDiff * imuIn->angular_velocity.x;
    // imuAccuPitch += timeDiff * imuIn->angular_velocity.y;
    // imuAccuYaw += timeDiff * imuIn->angular_velocity.z;
    
    imuRoll[imuPointerLast] = yaw;
    imuPitch[imuPointerLast] = roll;
    imuYaw[imuPointerLast] = -pitch;

    // imuRoll[imuPointerLast] = imuAccuRoll;
    // imuPitch[imuPointerLast] = -imuAccuPitch;
    // imuYaw[imuPointerLast] = -imuAccuYaw;

    // if there's monotonic IMU shift- accumulate it
    // imuAccX[imuPointerLast] = -imuIn->linear_acceleration.y;
    // imuAccY[imuPointerLast] = -imuIn->linear_acceleration.z - 9.81;
    // imuAccZ[imuPointerLast] = imuIn->linear_acceleration.x;

    AccumulateIMUShift();
  }
}
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
  float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
  float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

  imuPointerLast = (imuPointerLast + 1) % imuQueLength;

  imuTime[imuPointerLast] = imuIn->header.stamp.toSec() - 0.1068;
  imuRoll[imuPointerLast] = roll;
  imuPitch[imuPointerLast] = pitch;
  imuYaw[imuPointerLast] = yaw;
  imuAccX[imuPointerLast] = accX;
  imuAccY[imuPointerLast] = accY;
  imuAccZ[imuPointerLast] = accZ;

  AccumulateIMUShift();
}