void writeCSVCamera(shared_ptr<ofstream> file, ros::Time stamp)
{
  std::stringstream ss;
  ss << stamp.toNSec() << "," << stamp.toNSec() << ".png";

  *file << ss.str() << endl;
}

void HeaderManipulation::publishMsg(const topic_tools::ShapeShifter &msg, const ros::Time &msg_in_time)
{
    ROS_DEBUG("HeaderManipulation publishMsg Thread started with timestamp %f", msg_in_time.toSec());
    ros::Time end_time(ros::Time::now());
    ros::Time last_time;
    config_mutex_.lock();
    ros::Time time_to_pub(msg_in_time + msg_delay_);
    config_mutex_.unlock();
    do
    {
        last_time = end_time;
        ROS_DEBUG("waiting to publish msg from %f at %f", msg_in_time.toSec(), time_to_pub.toSec());
        if (end_time > time_to_pub)
        {
            boost::mutex::scoped_lock pub_lock(pub_mutex_);
            ROS_DEBUG("publishing msg which should be held back till: %f", time_to_pub.toSec());
            generic_pub_.publish(msg);
            return;
        }
        boost::mutex::scoped_lock config_lock(config_mutex_);
        publish_retry_rate_.sleep();
        time_to_pub = msg_in_time + msg_delay_;
        end_time = ros::Time::now();
    } while (last_time <= end_time);
    ROS_WARN("Detected jump back in time. Dropping msg. last: %f, end %f",
             last_time.toSec(), end_time.toSec());
    return;
}

Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
	// TF ready?
	Transform transform;
	try
	{
		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
		{
			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
			{
				ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
				return transform;
			}
		}

		tf::StampedTransform tmp;
		tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
		transform = rtabmap_ros::transformFromTF(tmp);
	}
	catch(tf::TransformException & ex)
	{
		ROS_WARN("%s",ex.what());
	}
	return transform;
}

/**
 * @brief TimeManipulator::toString
 * @param time
 * @return
 */
std::string TimeManipulator::str(const ros::Time& time)
{
  double tenths = time.toSec() - floor(time.toSec());
  long minutes_unix = MathManipulator::getUnsignedDivision((long) floor(time.toSec()), 60);
  int seconds = MathManipulator::getUnsignedRest((long) floor(time.toSec()), 60);
  long hours_unix = MathManipulator::getUnsignedDivision(minutes_unix, 60);
  int minutes = MathManipulator::getUnsignedRest(minutes_unix, 60);
  long days_unix = MathManipulator::getUnsignedDivision(hours_unix, 24);
  int hours = MathManipulator::getUnsignedRest(hours_unix, 24);
  long periods_of_400_years = MathManipulator::getUnsignedDivision(days_unix, 146097);
  int days_in_400_years_period = MathManipulator::getUnsignedRest(days_unix, 146097);
  if (days_in_400_years_period >= 32 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  if (days_in_400_years_period >= 57 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  if (days_in_400_years_period >= 82 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  int periods_of_4_years = days_in_400_years_period / 1461;
  int days_in_4_years_period = days_in_400_years_period % 1461;
  if (days_in_4_years_period >= 59)
  {
    days_in_4_years_period++;
  }
  if (days_in_4_years_period >= 425)
  {
    days_in_4_years_period++;
  }
  if (days_in_4_years_period >= 1157)
  {
    days_in_4_years_period++;
  }
  int year_in_4_years_period = days_in_4_years_period / 366;
  int year_days = days_in_4_years_period % 366;
  int year = year_in_4_years_period + periods_of_4_years * 4 + periods_of_400_years * 400 + 1970;
  int months_table[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int month_counter = 0;
  while(year_days > months_table[month_counter])
  {
    year_days -= months_table[month_counter++];
  }
  int month = month_counter + 1;
  int day = year_days + 1;
  std::stringstream ss;
  ss << (month < 10 ? "0" : "") << month << (day < 10 ? "/0" : "/") << day << (year < 10 ? "/0" : "/") << year;
  ss << (hours < 10 ? " 0" : " ") << hours << (minutes < 10 ? ":0" : ":") << minutes;
  if (seconds + tenths > 0)
  {
    ss << (seconds < 10 ? ":0" : ":") << seconds + tenths;
  }
  return ss.str();
}

void printTime()
{
  ros::Duration d = current_time - start_time;

  if (paused)
    printf("\r [PAUSED]   Log Time: %13.6f   Duration: %.6f            \r", current_time.toSec(), d.toSec());
  else
    printf("\r [RUNNING]  Log Time: %13.6f   Duration: %.6f            \r", current_time.toSec(), d.toSec());
  fflush(stdout);
}

void encoderTickCallback(const sek_drive::encoders::ConstPtr& msg)
{
    renc = - msg->right_wheel;
    lenc = msg->left_wheel;
    current_time = ros::Time::now();
    if ((current_time.toSec() - enc_loop_time.toSec())>=0.05)
    {
        calcOdom();
        enc_loop_time_2 = current_time;
    }
}

void currentCallback(const std_msgs::Float32::ConstPtr& current){
	last_time = curr_time;
	curr_time = ros::Time::now(); 
	if(last_time.toSec() != 0.0){
		inst_power = current->data*22.0;	
		float inst_coulombs = current->data*(curr_time.toSec() - last_time.toSec());
		//ROS_INFO("inst_coulombs = %f", inst_coulombs);
		//ROS_INFO("curr coulombs, before addition: %f", curr_coulombs);
		curr_coulombs = curr_coulombs + inst_coulombs;
		//ROS_INFO("curr_coulombs, after addition:  %f", curr_coulombs);
	} //else, ignore the first value
}

	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}

  void callback(const tPointCloud::ConstPtr& rgb_cloud, 
                const tImage::ConstPtr& rgb_image, 
                const tCameraInfo::ConstPtr& rgb_caminfo,
                const tImage::ConstPtr& depth_image, 
                const tPointCloud::ConstPtr& cloud
                )
  {
    if (max_update_rate_ > 0.0)
    {
      NODELET_DEBUG("update set to %f", max_update_rate_);
      if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
      {
        NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
        return;
      }
    }
    else
      NODELET_DEBUG("update_rate unset continuing");

    last_update_ = ros::Time::now();

    rgb_cloud_pub_.publish(rgb_cloud);
    rgb_image_pub_.publish(rgb_image);
    rgb_caminfo_pub_.publish(rgb_caminfo);
    depth_image_pub_.publish(depth_image);
    cloud_pub_.publish(cloud);
  }

void revert_applied_readings_since(const ros::Time& time)
{
    int msec = (int)(time.toNSec()/1000l);

    int k = 0;

    for(std::vector<ras_arduino_msgs::Encoders>::reverse_iterator it = _ringbuffer.rbegin(); it != _ringbuffer.rend(); ++it)
    {
        ras_arduino_msgs::Encoders& enc = *it;
        if (enc.timestamp >= msec)
        {
            ++k;

            double dist_l = (2.0*M_PI*robot::dim::wheel_radius) * (enc.delta_encoder1 / robot::prop::ticks_per_rev);
            double dist_r = (2.0*M_PI*robot::dim::wheel_radius) * (enc.delta_encoder2 / robot::prop::ticks_per_rev);

            _theta += (dist_r - dist_l) / robot::dim::wheel_distance;

            double dist = (dist_r + dist_l) / 2.0;

            _x += dist * cos(_theta);
            _y += dist * sin(_theta);

            //remove reading
            _ringbuffer.erase(--it.base());
        }
    }

    ROS_ERROR("[PoseGenerator::revertReadingsSince] Reverted %d readings.",k);
}

int wait_for_iob_signal()
{
  if (iob_synchronized) {
    //std::cerr << "wait>" << std::endl;
    if (start_robothw) {
      // no wait
    }
    //std::cerr << "wait<" << std::endl;
    return 0;
  } else {
    //
    ros::Time rnow;
    ros::Duration tm = ros::Duration(0, g_period_ns);
    ros::WallDuration wtm = ros::WallDuration(0, 100000); // 0.1 ms
    while ((rnow = ros::Time::now()) < rg_ts) {
      wtm.sleep();
    }

    rg_ts += tm;
    if ((rg_ts - rnow).toSec() <= 0) {
      fprintf(stderr, "iob::overrun (%f[ms]), w:%f -> %f\n",
              (rnow - rg_ts).toSec()*1000, rnow.toSec(), rg_ts.toSec());
      do {
        rg_ts += tm;
      } while ((rg_ts - rnow).toSec() <= 0);
    }

    return 0;
  }

  return 0;
}

int NodeClass::requestBoardStatus() {
	int ret;	
	
	// Request Status of RelayBoard 
	ret = m_SerRelayBoard->sendRequest();
	if(ret != SerRelayBoard::NO_ERROR) {
		ROS_ERROR("Error in sending message to Relayboard over SerialIO, lost bytes during writing");
	}

	ret = m_SerRelayBoard->evalRxBuffer();
	if(ret==SerRelayBoard::NOT_INITIALIZED) {
		ROS_ERROR("Failed to read relayboard data over Serial, the device is not initialized");
		relayboard_online = false;
	} else if(ret==SerRelayBoard::NO_MESSAGES) {
		ROS_ERROR("For a long time, no messages from RelayBoard have been received, check com port!");
		if(time_last_message_received_.toSec() - ros::Time::now().toSec() > relayboard_timeout_) {relayboard_online = false;}
	} else if(ret==SerRelayBoard::TOO_LESS_BYTES_IN_QUEUE) {
		//ROS_ERROR("Relayboard: Too less bytes in queue");
	} else if(ret==SerRelayBoard::CHECKSUM_ERROR) {
		ROS_ERROR("A checksum error occurred while reading from relayboard data");
	} else if(ret==SerRelayBoard::NO_ERROR) {
		relayboard_online = true;
		relayboard_available = true;
		time_last_message_received_ = ros::Time::now();
	}

	return 0;
}

bool disable(Disable::Request& request, Disable::Response& response) {
  controller.enabled = false;
  controller.reset();
  lastTime.fromSec(0.0);

  return fadeToDefaultColor();
}

	/**
	 * @brief Send velocity to FCU velocity controller
	 *
	 * @warning Send only VX VY VZ. ENU frame.
	 */
	void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate)
	{
		/**
		 * Documentation start from bit 1 instead 0;
		 * Ignore position and accel vectors, yaw.
		 */
		uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
		auto vel = [&] () {
			if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
				return ftf::transform_frame_baselink_aircraft(vel_enu);
			} else {
				return ftf::transform_frame_enu_ned(vel_enu);
			}
		} ();

		auto yr = ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate));

		set_position_target_local_ned(stamp.toNSec() / 1000000,
					utils::enum_value(mav_frame),
					ignore_all_except_v_xyz_yr,
					Eigen::Vector3d::Zero(),
					vel,
					Eigen::Vector3d::Zero(),
					0.0, yr.z());
	}

double PositionCommand::chirp_command ()
{
    double max_freq = 5.0;
    double min_freq = 0.0;
    double max_amp  = 0.3;
    double min_amp  = 0.05;
    double duration = 15.0;
    double Freq, Amp, time, velocity ,f_slope , a_slope;
    ros ::Time curr_time = ros::Time::now();

    f_slope = (max_freq - min_freq  ) / duration;
    a_slope = (max_amp  - min_amp   ) / duration;


    time = curr_time.toSec() - time0_chirp_.toSec();

    Amp  = time * a_slope + min_amp;
    Freq = time * f_slope + min_freq;

    velocity = -Amp * sin ( 2 * PI * Freq * time);

    std::cout   << "f = "           << Freq
                << "\ttime = "       << time
                << "\tvelocity = "   << velocity
                << "\tAmp = "        << Amp
                << std::endl;

    if (Freq >= max_freq)
    {
        chirp_enable_ = false;
        velocity = 0;
    }
    return velocity;
}

bool ElevationMap::update(const grid_map::Matrix& varianceUpdate, const grid_map::Matrix& horizontalVarianceUpdateX,
                          const grid_map::Matrix& horizontalVarianceUpdateY,
                          const grid_map::Matrix& horizontalVarianceUpdateXY, const ros::Time& time)
{
  boost::recursive_mutex::scoped_lock scopedLock(rawMapMutex_);

  const auto& size = rawMap_.getSize();

  if (!((Index(varianceUpdate.rows(), varianceUpdate.cols()) == size).all()
      && (Index(horizontalVarianceUpdateX.rows(), horizontalVarianceUpdateX.cols()) == size).all()
      && (Index(horizontalVarianceUpdateY.rows(), horizontalVarianceUpdateY.cols()) == size).all()
      && (Index(horizontalVarianceUpdateXY.rows(), horizontalVarianceUpdateXY.cols()) == size).all())) {
    ROS_ERROR("The size of the update matrices does not match.");
    return false;
  }

  rawMap_.get("variance") += varianceUpdate;
  rawMap_.get("horizontal_variance_x") += horizontalVarianceUpdateX;
  rawMap_.get("horizontal_variance_y") += horizontalVarianceUpdateY;
  rawMap_.get("horizontal_variance_xy") += horizontalVarianceUpdateXY;
  clean();
  rawMap_.setTimestamp(time.toNSec());

  return true;
}

void BagFormat::writeData(const ros::Time& time, const QVector< Plot::LinkedBufferIterator* >& data)
{
	plot_msgs::Plot msg;
	msg.header.stamp = time;

	Q_FOREACH(Plot::LinkedBufferIterator* it, data)
	{
		if(!it)
			continue;

		const Plot::DataPoint& p = **it;
		const Plot* plot = it->plot();

		plot_msgs::PlotPoint point;
		point.name = plot->path().toStdString();
		point.value = p.value;
		msg.points.push_back(point);
	}

	try
	{
		m_bag.write("plot", time, msg);
	}
	catch(rosbag::BagException& e)
	{
		fprintf(stderr, "Bag exception: %s\n", e.what());
		fprintf(stderr, "Time was %lf\n", time.toSec());
		throw;
	}
}

int main(int argc, char **argv){

    ros::init(argc, argv, "remaining_memory");
	 ros::NodeHandle n;

    //Publisher part

    ros::Publisher remaining_memory_pub = n.advertise<mavros_msgs::Mavlink>("mavlink/to", 2000);
    ros::Rate loop_rate(10);
    
    FILE *in;
	 char buff[512];
    float remaining_memory = 0.0;

    int count = 0;
    while (ros::ok()) {

       // get the remaining memory thanks to a shell script

	    if(!(in = popen("df -h /home/ | xargs | cut -d ' ' -f 14 | sed 's/%//g'", "r"))){
		     exit(1);
	    }

	    while(fgets(buff, sizeof(buff), in)!=NULL){
		     remaining_memory = (float) atoll(buff);
	    }
	    pclose(in);


        //message generation
        //1. make named value float with type mavlink_message_t
        mavlink::mavlink_message_t msg;
        mavlink::MsgMap map(msg);

	mavlink::common::msg::NAMED_VALUE_FLOAT mem_msg;

	std::array<char, 10> name = {"rem_mem"};

	mem_msg.time_boot_ms = stamp.toNSec()/1000;
	mem_msg.name = name;
	mem_msg.value = remaining_memory;


	mem_msg.serialize(map);

        auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();

        rmsg->header.stamp = ros::Time::now();
        mavros_msgs::mavlink::convert(msg, *rmsg);

		  remaining_memory_pub.publish(rmsg);
		  ros::spinOnce();

		  loop_rate.sleep();
	 	  ++count;
	 }

    return 0;

}

	bool getPoseAt(const ros::Time& time, StateType& pose, ros::Time& stamp)
	{
		std::deque<TimeState> past_poses_copy;

		{
			boost::mutex::scoped_lock past_poses_lock(past_poses_mutex_);
			past_poses_copy = past_poses_;
		}

		if (past_poses_copy.empty())
			return false;

		TimeState result = *(past_poses_copy.rbegin());
		if (result.first <= time)
		{
			typename std::deque<TimeState>::const_reverse_iterator iter =
					past_poses_copy.rbegin()++;
			while(iter != past_poses_copy.rend() && iter->first <= time)
			{
				iter++;
			}
			if (iter != past_poses_copy.rend())
			{
				// check which time is the nearest
				double delta_after = fabs(iter->first.toSec() - time.toSec());
				double delta_before = fabs(
						(iter - 1)->first.toSec() - time.toSec());
				if (delta_before < delta_after)
				{
					result = *(--iter);
				}
				else
				{
					result = *iter;
				}
			}
			else
			{
				result = *(--iter);
			}
		}

		pose = result.second;
		stamp = result.first;
		return true;
	}

	/**
	 * @brief Send vision speed estimate to FCU velocity controller
	 */
	void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp) {
		Eigen::Vector3d vel_;
		tf::vectorMsgToEigen(vel_enu, vel_);
		auto vel = UAS::transform_frame_enu_ned(vel_);

		vision_speed_estimate(stamp.toNSec() / 1000,
				vel.x(), vel.y(), vel.z());
	}