long long pn(long long n, long long x)
{
    long long s;
    if(n==0)
        s=1;
    else if(n==1)
        s=x;
    else
        s=((2*n-1)*x*pn(n-1,x)-(n-1)*pn(n-2,x))/n;
    return s;
}

void vis_brd::_drw_fld(int i, int j, bool hlght)
{
	assert(0 <= i && 0 <= j);
	assert(i < snpsht_.get_wdth() && j < snpsht_.get_hght());
	plr_clr p = snpsht_.get_cell(i + j * snpsht_.get_wdth());
	static const int spc = fld_sz / 8;

	QPainter pnt(this);

	/**
	 * Step 1: Draw the bounding rectangle.
	 */
	if (hlght == false)
	{
		QPen pn(Qt::black);
		pn.setWidth(wdt);
		pnt.setBrush(QColor(250, 250, 200));
		pnt.setPen(pn);
	}
	else
	{
		QPen pn(Qt::red);
		pn.setWidth(wdt);
		pnt.setBrush(QColor(250, 250, 225));
		pnt.setPen(pn);
	}

	int fr_x = i * fld_sz + wdt / 2;
	int fr_y = j * fld_sz + wdt / 2;

	pnt.drawRect(fr_x, fr_y, fld_sz, fld_sz);

	/**
	 * Step 2: Draw the circle.
	 */
	{
		QPen pn(Qt::black);
		pn.setWidth(wdt);
		if (p == pc_wht)
			pnt.setBrush(QBrush(Qt::red));
		else if (p == pc_blc)
			pnt.setBrush(QBrush(Qt::black));
		pnt.setPen(pn);
	}

	if (p != pc_free)
		pnt.drawEllipse(fr_x + spc, fr_y + spc, fld_sz - 2 * spc, fld_sz - 2 * spc);
}

static void pn (const te_expr *n, int depth) {
    printf("%*s", depth, "");

    if (n->bound) {
        printf("bound %p\n", n->bound);
    } else if (n->left == 0 && n->right == 0) {
        printf("%f\n", n->value);
    } else if (n->left && n->right == 0) {
        printf("f1 %p\n", n->left);
        pn(n->left, depth+1);
    } else {
        printf("f2 %p %p\n", n->left, n->right);
        pn(n->left, depth+1);
        pn(n->right, depth+1);
    }
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "USB2_F_7001_node");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	
	rx_pub = n.advertise<can_msgs::CANFrame>("/can_bus_rx", 10);
    ros::Subscriber tx_sub = n.subscribe<can_msgs::CANFrame>("/can_bus_tx", 10, CANFrameToSend);
	
	std::string port;
	//pn.param<std::string>("port", port, "/dev/ttyUSB0");
	pn.param<std::string>("port", port, "/dev/serial/by-id/usb-LAWICEL_CANUSB_LWVPMVC4-if00-port0"); 
    int bit_rate;
	pn.param("bit_rate", bit_rate, 5);
    
    can_usb_adapter = new USB2_F_7001(&port, &CANFrameReceived);
    
    ROS_INFO("USB2_F_7001 -- Opening CAN bus...");
    if( !can_usb_adapter->openCANBus((USB2_F_7001_BitRate)bit_rate) )
    {
        ROS_FATAL("USB2_F_7001 -- Failed to open the CAN bus!");
		ROS_BREAK();
    }
    ROS_INFO("USB2_F_7001 -- The CAN bus is now open!");
    
	ros::spin();
    
    delete can_usb_adapter;
    
  	return(0);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "kinect_simulated_tilt");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");

	ros::Duration(0.5).sleep();
	ros::Publisher tilt_pub = n.advertise<std_msgs::Float64>("cur_tilt_angle", 10);
	ros::Subscriber sub_tilt_angle = n.subscribe("tilt_angle", 10, setTiltAngle);
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		// tilt angle
		std_msgs::Float64 angle_msg;
		angle_msg.data = pitch * 180.0 / M_PI;

		//send the joint state and transform
		tilt_pub.publish(angle_msg);

		ros::spinOnce();

		// This will adjust as needed per iteration
		loop_rate.sleep();
	}

	return 0;
}

int main(int argc, char** argv)
{
  	ros::init(argc, argv, "roomba_tf_setup");
  	ros::NodeHandle n;
  	ros::NodeHandle pn("~");
  	
  	std::string base_frame_id;
	std::string laser_frame_id;
	std::string nose_frame_id;
	pn.param<std::string>("base_frame_id", base_frame_id, "base_link");
	pn.param<std::string>("laser_frame_id", laser_frame_id, "base_laser");
	pn.param<std::string>("nose_frame_id", nose_frame_id, "base_nose");

  	ros::Rate r(50);

  	tf::TransformBroadcaster broadcaster;

  	while(n.ok())
	{
    		broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.065, 0.000, 0.240)),
        	ros::Time::now(), base_frame_id.c_str(), laser_frame_id.c_str()));
        	
        	broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.120, 0.000, 0.160)),
        	ros::Time::now(), base_frame_id.c_str(), nose_frame_id.c_str()));
		
    		r.sleep();
  	}
}

SickTimCommon::SickTimCommon(AbstractParser* parser) :
    diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser)
    // FIXME All Tims have 15Hz?
{
  dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f;
  f = boost::bind(&sick_tim::SickTimCommon::update_config, this, _1, _2);
  dynamic_reconfigure_server_.setCallback(f);

  // datagram publisher (only for debug)
  ros::NodeHandle pn("~");
  pn.param<bool>("publish_datagram", publish_datagram_, false);
  if (publish_datagram_)
    datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);

  // scan publisher
  pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);

  diagnostics_.setHardwareID("none");   // set from device after connection
  diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(pub_, diagnostics_,
          // frequency should be target +- 10%.
          diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10),
          // timestamp delta can be from 0.0 to 1.3x what it ideally is.
          diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_));
  ROS_ASSERT(diagnosticPub_ != NULL);
}

void QDeclarativeRectangle::generateRoundedRect()
{
    Q_D(QDeclarativeRectangle);
    if (d->rectImage.isNull()) {
        const int pw = d->pen && d->pen->isValid() ? d->pen->width() : 0;
        const int radius = qCeil(d->radius);    //ensure odd numbered width/height so we get 1-pixel center

        QString key = QLatin1String("q_") % QString::number(pw) % d->color.name() % QString::number(d->color.alpha(), 16) % QLatin1Char('_') % QString::number(radius);
        if (d->pen && d->pen->isValid())
            key += d->pen->color().name() % QString::number(d->pen->color().alpha(), 16);

        if (!QPixmapCache::find(key, &d->rectImage)) {
            d->rectImage = QPixmap(radius*2 + 3 + pw*2, radius*2 + 3 + pw*2);
            d->rectImage.fill(Qt::transparent);
            QPainter p(&(d->rectImage));
            p.setRenderHint(QPainter::Antialiasing);
            if (d->pen && d->pen->isValid()) {
                QPen pn(QColor(d->pen->color()), d->pen->width());
                p.setPen(pn);
            } else {
                p.setPen(Qt::NoPen);
            }
            p.setBrush(d->color);
            if (pw%2)
                p.drawRoundedRect(QRectF(qreal(pw)/2+1, qreal(pw)/2+1, d->rectImage.width()-(pw+1), d->rectImage.height()-(pw+1)), d->radius, d->radius);
            else
                p.drawRoundedRect(QRectF(qreal(pw)/2, qreal(pw)/2, d->rectImage.width()-pw, d->rectImage.height()-pw), d->radius, d->radius);

            // end painting before inserting pixmap
            // to pixmap cache to avoid a deep copy
            p.end();
            QPixmapCache::insert(key, d->rectImage);
        }
    }
}

/**
 * Prints an integer value
 * @param input The unsigned value.
 */
void pn(unsigned int input)
{
	int ldb = input % 10;
	int n = (int) input / 10;
	if (input > 9) pn(n);
	printChar(ldb + 48);
}

    JointTrajectoryGenerator(std::string name) : 
      as_(ros::NodeHandle(), "joint_trajectory_generator",
          boost::bind(&JointTrajectoryGenerator::executeCb, this, _1),
          false),
      ac_("joint_trajectory_action"),
      got_state_(false)
      {
        ros::NodeHandle n;
        state_sub_ = n.subscribe("state", 1, &JointTrajectoryGenerator::jointStateCb, this);

        ros::NodeHandle pn("~");
        pn.param("max_acc", max_acc_, 0.5);
        pn.param("max_vel", max_vel_, 5.0);

        // Load Robot Model
        ROS_DEBUG("Loading robot model");
        std::string xml_string;
        ros::NodeHandle nh_toplevel;
        if (!nh_toplevel.getParam(std::string("/robot_description"), xml_string))
          throw ros::Exception("Could not find paramter robot_description on parameter server");

        if(!robot_model_.initString(xml_string)) 
          throw ros::Exception("Could not parse robot model");

        ros::Rate r(10.0);
        while(!got_state_){
          ros::spinOnce();
          r.sleep();
        }

        ac_.waitForServer();
        as_.start();
        ROS_INFO("%s: Initialized",name.c_str());
      }

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "kml_extractor_node");
    ros::NodeHandle n;
    ros::NodeHandle pn("~");

    signal(SIGINT, sigintHandler);

    std::string header_file_path, footer_file_path;
    pn.param<std::string>("kml_file", kml_filename, "out.kml");
    pn.param<std::string>("coordinates_filename", coordinates_utm_filename, "coordinates_utm.txt");
    pn.param<std::string>("header_file_path", header_file_path, "/home/joao/catkin_ws_isrobotcar/src/kml_extractor/src/header.yaml");
    pn.param<std::string>("footer_file_path", footer_file_path, "/home/joao/catkin_ws_isrobotcar/src/kml_extractor/src/footer.yaml");

    kml_file.open(kml_filename.c_str(), std::ios_base::out | std::ios_base::trunc);
    coordinates_utm_file.open(coordinates_utm_filename.c_str(), std::ios_base::out | std::ios_base::trunc);
    std::ifstream header_kml(header_file_path.c_str());
    footer_kml.open(footer_file_path.c_str(), std::ios_base::in);

    while(header_kml.good())
    {
        char c = header_kml.get();       // get character from file
            if (header_kml.good())
             kml_file << c;
    }
    kml_file << std::endl;

    ros::Subscriber fix_sub = n.subscribe("fix", 30, callbackFix);
    ros::Subscriber odom_sub = n.subscribe("odom", 30, callbackOdom);

    ros::spin();

    return 0;
}

  db_err udb_server::find_dbr_cb(const char *key_str, const char *pn_str,
                                 http_response *rsp)
  {
    std::string key(key_str);
    std::string pn(pn_str);
    db_record *dbr = seeks_proxy::_user_db->find_dbr(key,pn);
    if (!dbr)
      {
        return DB_ERR_NO_REC;
      }
    else
      {
        // serialize dbr.
        std::string str;
        dbr->serialize(str);

        // fill up response.
        size_t body_size = str.length() * sizeof(char);
        if (!rsp->_body)
          rsp->_body = (char*)std::malloc(body_size);
        rsp->_content_length = body_size;
        for (size_t i=0; i<str.length(); i++)
          rsp->_body[i] = str[i];
        delete dbr;
        return SP_ERR_OK;
      }
  }

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "pgr_camera");

  typedef dynamic_reconfigure::Server < pgr_camera_driver::PGRCameraConfig > Server;
  Server server;

  try
  {
    boost::shared_ptr < PGRCameraNode > pn (new PGRCameraNode (ros::NodeHandle(),ros::NodeHandle("~")));

    Server::CallbackType f = boost::bind (&PGRCameraNode::configure, pn, _1, _2);
    server.setCallback (f);

    ros::spin ();

  } catch (std::runtime_error & e)
  {
    ROS_FATAL ("Uncaught exception: '%s', aborting.", e.what ());
    ROS_BREAK ();
  }

  return 0;

}

rowvec dF2du(unsigned row, irowvec causes, const DataPairs &data, const gmat &sigma, vec u){

  // Attaining variance covariance matrix etc. (conditional on u)
  vmat cond_sig = sigma(causes);
  vec cond_mean = cond_sig.proj*u;

  rowvec alp = data.alpha_get(row, causes);
  rowvec gam = data.gamma_get(row, causes);
  colvec alpgam = alp.t() - gam.t();

  double cdf = pn(alpgam, cond_mean, cond_sig.vcov);

  vec ll = sqrt(diagvec(cond_sig.vcov));
  mat Lambda = diagmat(ll);
  mat iLambda = diagmat(1/ll);
  mat R = iLambda*cond_sig.vcov*iLambda;
  mat LR = Lambda*R;
  double r = R(0,1);

  vec ytilde = iLambda*(alpgam - cond_mean);
  vecmat D = Dbvn(ytilde(0),ytilde(1),r);
  mat M = -LR*D.V;

  vec dcdfdu = trans(cond_sig.proj)*cond_sig.inv*M;

  rowvec dF2du_1 = data.dpiduMarg_get(row, causes(0), 0)*data.piMarg_get(row, causes(1), 1)*cdf ;
  rowvec dF2du_2 = data.dpiduMarg_get(row, causes(1), 1)*data.piMarg_get(row, causes(0), 0)*cdf;
  vec dF2du_3 = data.piMarg_get(row, causes(0), 0)*data.piMarg_get(row, causes(1), 1)*dcdfdu;

  rowvec dF2du = dF2du_1 + dF2du_2 + dF2du_3.t();
  return(dF2du);
};

TEST_F(PubNubCppTest, PubNubInitDone) {
	{
		PubNub pn("demo", "demo", &cb, NULL);
		EXPECT_TRUE(initCalled);
	}
	EXPECT_TRUE(doneCalled);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "sweep_node");
    ros::NodeHandle n;
    ros::NodeHandle pn("~");

    double min;
    pn.param("min", min, -0.8);
    double max;
    pn.param("max", max, 0.8);
    double height;
    pn.param("height", height, 0.05);
    double speed;
    pn.param("speed", speed, 0.2);
    double acceleration;
    pn.param("acceleration", acceleration, 0.5);

    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("/arm_controller/follow_joint_trajectory", true);
    ROS_INFO("Sweep -- Waiting for the action server to start...");
    ac.waitForServer();
    ROS_INFO("Sweep -- Got it!");

    double position = min;

    while(ros::ok())
    {
        control_msgs::FollowJointTrajectoryGoal goal;
        goal.trajectory.header.stamp = ros::Time::now();
        goal.trajectory.joint_names.resize(2);
        goal.trajectory.points.resize(1);
        goal.trajectory.joint_names[0] = "upper_arm_joint";
        goal.trajectory.points[0].positions.push_back(height);
        goal.trajectory.points[0].velocities.push_back(speed);
        goal.trajectory.points[0].accelerations.push_back(acceleration);
        goal.trajectory.joint_names[1] = "arm_axel_joint";
        goal.trajectory.points[0].positions.push_back(position = position == min ? max : min);
        goal.trajectory.points[0].velocities.push_back(speed);
        goal.trajectory.points[0].accelerations.push_back(acceleration);
        goal.trajectory.points[0].time_from_start = ros::Duration(2.0);
        goal.goal_tolerance.resize(4);
        goal.goal_tolerance[0].name = "upper_arm_joint";
        goal.goal_tolerance[0].position = 0.01;
        goal.goal_tolerance[1].name = "arm_axel_joint";
        goal.goal_tolerance[1].position = 0.01;
        goal.goal_time_tolerance = ros::Duration(0.5);

        ac.sendGoal(goal);

        // Wait for the action to return
        bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

        if(!finished_before_timeout)
        {
            actionlib::SimpleClientGoalState state = ac.getState();
            ROS_ERROR("Sweep -- %s!", state.toString().c_str());
        }
    }

    return 0;
}

/* Conditional on other individual */
double F1(unsigned row, unsigned cause, unsigned indiv, unsigned cond_cause, const DataPairs &data, const gmat &sigma, vec u){

  // Other individual
  unsigned cond_indiv;
  if (indiv==0){
    cond_indiv=1;
  }
  else {
    cond_indiv=0;
  }

  // Alpgam of other individual
  double cond_alp = data.alphaMarg_get(row, cond_cause, cond_indiv);
  double cond_gam = data.gammaMarg_get(row, cond_cause, cond_indiv);
  double cond_alpgam = cond_alp - cond_gam;

  vec vcond_alpgam(1); vcond_alpgam(0) = cond_alpgam;

  // Joining u vector and alpgam from other individual
  vec alpgamu = join_cols(vcond_alpgam, u);

  // Attaining variance covariance matrix etc. (conditional on u and other individual)
  vmat cond_sig = sigma(cause,cond_cause);
  double cond_mean = as_scalar(cond_sig.proj*alpgamu);

  double alp = data.alphaMarg_get(row, cause, indiv);
  double gam = data.gammaMarg_get(row, cause, indiv);
  double alpgam = alp - gam;

  double F1 = data.piMarg_get(row, cause, indiv)*pn(alpgam, cond_mean, as_scalar(cond_sig.vcov));
  return(F1);
};

	void TerrainMeshSource::generateNoise(int x, int y, int w, int h, int numOctaves, float amplitude, float frequency)
	{
		if(w == -1)
			w = getHeightmapWidth();
		if(h == -1)
			h = getHeightmapHeight();

		RECT rc;
		rc.left = x;
		rc.right = x+w;
		rc.top = y;
		rc.bottom = y+h;

		clipRect(rc);

		//
		int cw = rc.right - rc.left;
		int ch = rc.bottom - rc.top;
		if(cw <=0
			|| ch <= 0)
			return;

		Perlin pn(numOctaves, frequency, amplitude, timeGetTime());

		if(m_mapBits == 16)
			copyNoiseToHeightmap(pn, rc, &m_16bitHMap, 65535);
		else
			copyNoiseToHeightmap(pn, rc, &m_8bitHMap, 255);
	}

static
Value *
eval(const Ast *expr) {
	switch(expr->class) {
	case N_CALL:
		return call(expr);
	case N_ASSIGNMENT:
		return assignment(expr);
	case N_IDENTIFIER:
		return identifier(expr);
	case N_NEG:
		return _neg(expr);
	case N_NOT:
		return _not(expr);
	case N_EQ:
		return _eq(expr);
	case N_NEQ:
		return _neq(expr);
	case N_AND:
		return _and(expr);
	case N_IOR:
		return _ior(expr);
	case N_XOR:
		return _neq(expr); // alias
	case N_LT:
		return _lt(expr);
	case N_LE:
		return _le(expr);
	case N_GE:
		return _ge(expr);
	case N_GT:
		return _gt(expr);
	case N_ADD:
		return _add(expr);
	case N_SUB:
		return _sub(expr);
	case N_MUL:
		return _mul(expr);
	case N_DIV:
		return _div(expr);
	case N_POW:
		return _pow(expr);
	case N_MOD:
		return _mod(expr);
	case N_BOOLEAN:
		return _bool(expr);
	case N_INTEGER:
		return _int(expr);
	case N_FLOAT:
		return _float(expr);
	case N_STRING:
		return _string(expr);
	case N_SET:
		return _set(expr);
	case N_R:
		return _relation(expr);
	}
printf("EVALFAIL %d ", expr->class); pn(expr);
	assert(false && "should not be reached");
}

bool FishEyeCamera::gen_ray(double dx, double dy, Vector3D &ray_dir)
{
    Vector3D u = 1.0 * m_properties->m_left_dir;
    Vector3D v = 1.0 * m_properties->m_up_dir;
    Vector3D w = -1.0 * m_properties->m_view_dir;
    u.normalize();
    v.normalize();
    w.normalize();
    Point2D pn(m_properties->m_pixel_size * 2.0 / (m_properties->m_pixel_size * m_properties->m_screen_width) * dx,
               m_properties->m_pixel_size * 2.0 / (m_properties->m_pixel_size * m_properties->m_screen_height) * dy);
	double r_squared = pn[0] * pn[0] + pn[1] * pn[1];
    
	if (r_squared <= 1.0) {
		float r 		= sqrt(r_squared);
		float psi 		= r * m_psi_max * PI_ON_180;
		float sin_psi 	= sin(psi);
		float cos_psi 	= cos(psi);
		float sin_alpha = pn[1] / r;
		float cos_alpha = pn[0] / r;
		ray_dir = sin_psi * cos_alpha * u +  sin_psi * sin_alpha * v - cos_psi * w;
        
		return true;
	}
	return false;
}