// reproject shape
shape_t*
shape_proj(
    const shape_t* shape,
    const char*    from,
    const char*    to   ){

    shape_t* projected = shape_copy(shape);
    projPJ old_prj = pj_init_plus(from);
    projPJ new_prj = pj_init_plus(to);

    for(uint32_t i=0; i<kv_size(projected->points); i++) {
        point_t* p = &kv_A(projected->points, i);
        p->x *= DEG_TO_RAD;
        p->y *= DEG_TO_RAD;
        int32_t err = pj_transform(old_prj, new_prj, 1, 0, &p->x, &p->y, NULL);
        if (err)
            fprintf(stderr, "ERR%d %s\n", err, pj_strerrno(err));
        assert(err == 0);
    }

    for(uint32_t i=0; i<kv_size(projected->hull); i++) {
        point_t* p = &kv_A(projected->hull, i);
        p->x *= DEG_TO_RAD;
        p->y *= DEG_TO_RAD;
        int32_t err = pj_transform(old_prj, new_prj, 1, 0, &p->x, &p->y, NULL);
        if (err)
            fprintf(stderr, "ERR%d %s\n", err, pj_strerrno(err));
        assert(err == 0);
    }

    pj_free(old_prj);
    pj_free(new_prj);

    return projected;
}

bool CMapDEM::getOrigRegion(QVector<qint16>& data,projXY &topLeft, projXY &bottomRight, int& w, int& h)
{
    //memset(data, 0, sizeof(qint16) * h * w);
    data.fill(0);
    if(pjsrc == 0) return false;

    // 1. convert top left and bottom right point into the projection system used by the DEM data
    pj_transform(pjtar, pjsrc, 1, 0, &topLeft.u, &topLeft.v, 0);
    pj_transform(pjtar, pjsrc, 1, 0, &bottomRight.u, &bottomRight.v, 0);

    // 2. get floating point offset of topleft corner
    double xoff1_f = (topLeft.u - xref1) / xscale;
    double yoff1_f = (topLeft.v - yref1) / yscale;

    // 3. truncate floating point offset into integer offset
    int xoff1 = floor(xoff1_f);  //qDebug() << "xoff1:" << xoff1 << xoff1_f;
    int yoff1 = floor(yoff1_f);  //qDebug() << "yoff1:" << yoff1 << yoff1_f;

    // 4. get floating point offset of bottom right corner
    double xoff2_f = (bottomRight.u - xref1) / xscale;
    double yoff2_f = (bottomRight.v - yref1) / yscale;

    // 5. truncate floating point offset into integer offset.
    int xoff2 = ceil(xoff2_f);   //qDebug() << "xoff2:" << xoff2 << xoff2_f;
    int yoff2 = ceil(yoff2_f);   //qDebug() << "yoff2:" << yoff2 << yoff2_f;

    // 6. get width and height to read from file
    qint32 w1 = xoff2 - xoff1 + 1;
                                 //qDebug() << "w1:" << w1 << "h1:" << h1;
    qint32 h1 = yoff2 - yoff1 + 1;

    topLeft.u = xoff1 * xscale + xref1;
    topLeft.v = yoff1 * yscale + yref1;
    bottomRight.u = xoff2 * xscale + xref1;
    bottomRight.v = yoff2 * yscale + yref1;
    pj_transform(pjsrc, pjtar, 1, 0, &topLeft.u, &topLeft.v, 0);
    pj_transform(pjsrc, pjtar, 1, 0, &bottomRight.u, &bottomRight.v, 0);

    // memory sanity check
    if(double(w1) * double(h1) > pow(2.0f,31)) return false;

    if (w1 < w)
    {
        w = w1;
    }
    if (h1 < h)
    {
        h = h1;
    }

    Q_ASSERT(w1 <= w);
    Q_ASSERT(h1 <= h);

    // 7. read DEM data from file
    CPLErr err = dataset->RasterIO(GF_Read, xoff1, yoff1, w1, h1, data.data(), w, h, GDT_Int16, 1, 0, 0, 0, 0);
    return (err != CE_Failure);
}

int main(){

//  char *pr1[] = {"+proj=latlong", "+ellps=WGS84"};
  char *pr1[] = {"+proj=tmerc",   "+ellps=WGS84", "+lon_0=39",
                 "+scale=1", "+lat_0=0", "+x_0=500000"};
  char *pr2[] = {"+proj=tmerc",   "+ellps=krass", "+lon_0=39",
                 "+scale=1", "+lat_0=0", "+x_0=500000"};


  projPJ P1 = pj_init(6, pr1);
  projPJ P2 = pj_init(6, pr2);


  if (!P1) {
    fprintf(stderr, "can't create projection 1\n");
    exit(1);
  }

  if (!P2) {
    fprintf(stderr, "can't create projection 2\n");
    exit(1);
  }

  double x=39*M_PI/180 + ((double)rand()/RAND_MAX - 0.5) * M_PI/30; // 36..42
  double y=((double)rand()/RAND_MAX - 0.5) * M_PI;         // -90..+90
  double z=((double)rand()/RAND_MAX - 0.5) * 6000;         // +/- 3000m

  printf("%.12f %.12f %f\n",x*180/M_PI,y*180/M_PI,z);

  int i;
  for (i=0; i<1000000; i++){

//    z=0;
    int j = pj_transform(P1,P2, 1, 1, &x, &y, &z);

    if (j) {
      fprintf(stderr, "can't do transform\n");
      exit(1);
    }

//    z=0;
    j = pj_transform(P2,P1, 1, 1, &x, &y, &z);

    if (j) {
      fprintf(stderr, "can't do transform\n");
      exit(1);
    }
  }

  printf("%.12f %.12f %f\n",x*180/M_PI,y*180/M_PI,z);

  pj_free(P1);
  pj_free(P2);

}

void process_projection(const projection_t *prj)
{

    projPJ pj_tgt, pj_latlong;
    const point_t *test_point;
    int i;
    double x,y;
    double x1,y1;

    pj_tgt=pj_init_plus(prj->init_string);
    pj_latlong=pj_init_plus("+proj=latlong +ellps=WGS84");

    if (!pj_latlong) {
        printf("could not init latlong\n");
        exit(1);
    }
    if (!pj_tgt) {
        printf("could not init target projection: %s\n", prj->init_string);
        exit(1);
    }

    printf("\tdescribe '%s'\n", prj->name);
    for (i=0; i<(sizeof(TEST_POINTS) / sizeof(point_t)); i++) {
        test_point=TEST_POINTS+i;
        x=test_point->x * DEG_TO_RAD;
        y=test_point->y * DEG_TO_RAD;

        pj_transform(pj_latlong, pj_tgt, 1, 1, &x, &y, 0);

        printf("\t\tit 'should forward transform (%f, %f) to (%f,%f)'\n", test_point->x, test_point->y, x, y);
        printf("\t\t\tvar prj=new Projections.%s();\n", prj->name);
        printf("\t\t\txy=prj.forward(%.20f, %.20f)\n", test_point->x, test_point->y);
        printf("\t\t\txy[0].should.equal_approximately %.20f\n", x);
        printf("\t\t\txy[1].should.equal_approximately %.20f\n", y);
        printf("\t\tend\n\n");


        x1=x;
        y1=y;
        pj_transform(pj_tgt, pj_latlong, 1, 1, &x1, &y1, 0);
        x1*=RAD_TO_DEG;
        y1*=RAD_TO_DEG;

        printf("\t\tit 'should inverse transform (%f, %f) to (%f,%f)'\n", x, y, x1, y1);
        printf("\t\t\tvar prj=new Projections.%s();\n", prj->name);
        printf("\t\t\txy=prj.inverse(%.20f, %.20f)\n", x, y);
        printf("\t\t\txy[0].should.equal_approximately %.20f\n", x1);
        printf("\t\t\txy[1].should.equal_approximately %.20f\n", y1);
        printf("\t\tend\n\n");
    }

    printf("\tend\n\n");
}

void coordconverter::convert(const std::vector<latlong>& latlongs, std::vector<coord>& coords)
{
  projPJ pjMerc = pj_init_plus("+proj=tmerc +lat_0=49 +lon_0=-2 +k=0.9996012717 +x_0=400000 +y_0=-100000 +ellps=airy +datum=OSGB36 +units=m +no_defs");
  if(pjMerc == NULL) throw hdsrvexception("pj_init_plus merc failed"); 

  projPJ pjLatLon = pj_init_plus("+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs");
  if(pjLatLon == NULL) 
	{
		pj_free(pjMerc);
		throw hdsrvexception("pj_init_plus proj failed");
	}

	  
  for(std::vector<latlong>::const_iterator it = latlongs.begin(); it != latlongs.end(); it++)
  {
    double x = it->getLonDouble() * DEG_TO_RAD;
    double y = it->getLatDouble() * DEG_TO_RAD;

    if(pj_transform(pjLatLon, pjMerc, 1, 1, &x, &y, NULL) != 0)
    {
			pj_free(pjMerc);
			pj_free(pjLatLon);
			throw hdsrvexception("pj_transform failed");
    }
    coord c;
    c.setNDouble(y);
    c.setEDouble(x);
    coords.push_back(c);
  }
  
  pj_free(pjMerc);
  pj_free(pjLatLon);

    
}

P4Coordinate P4Singleton::transform(P4Coordinate p4c, P4Projection from, P4Projection to)
{
    if (!p4c.isValid() || !from.isValid() || !to.isValid())
        return P4Coordinate();

    double x = p4c.x();
    double y = p4c.y();
    double z = p4c.z();

    if (from.pj()->is_latlong)
    {
        x *= DEG_TO_RAD;
        y *= DEG_TO_RAD;
    }

    int result = pj_transform(from.pj(),to.pj(),1,1,&x,&y,&z);

    if (result != 0)
        return P4Coordinate();

    if (to.pj()->is_latlong)
    {
        x *= RAD_TO_DEG;
        y *= RAD_TO_DEG;
    }

    return P4Coordinate(x,y,z);
}

static GhtErr
l2g_coordinate_reproject(const Las2GhtState *state, GhtCoordinate *coord)
{

    int *pj_errno_ref;
    GhtCoordinate origcoord;

    /* Make a copy of the input point so we can report the original should an error occur */
    origcoord = *coord;

    if (pj_is_latlong(state->pj_input)) l2g_coordinate_to_rad(coord);

    /* Perform the transform */
    pj_transform(state->pj_input, state->pj_output, 1, 0, &(coord->x), &(coord->y), NULL);

    /* For NAD grid-shift errors, display an error message with an additional hint */
    pj_errno_ref = pj_get_errno_ref();

    if (*pj_errno_ref != 0)
    {
        if (*pj_errno_ref == -38)
        {
            ght_warn("No no grid shift files were found, or point out of range.");
        }
        ght_error("%s: could not project point (%g %g): %s (%d)", 
                  __func__, 
                  origcoord.x, origcoord.y,
                  pj_strerrno(*pj_errno_ref), *pj_errno_ref
                  );
        return GHT_ERROR;
    }

    if (pj_is_latlong(state->pj_output)) l2g_coordinate_to_dec(coord);
    return GHT_OK;
}

 /**
  * Transform coordinates from one CRS into another. Wraps the same
  * function of the proj library.
  *
  * Coordinates have to be in radians and are produced in radians.
  *
  * @throws osmmium::projection_error if the projection fails
  */
 inline Coordinates transform(const CRS& src, const CRS& dest, Coordinates c) {
     int result = pj_transform(src.get(), dest.get(), 1, 1, &c.x, &c.y, nullptr);
     if (result != 0) {
         throw osmium::projection_error(std::string("projection failed: ") + pj_strerrno(result));
     }
     return c;
 }

int pj_init_and_transform( const char *from_projection, const char *to_projection, \
   long point_count, double *x, double *y) {

	 projPJ pj_from, pj_to;
	 int i;
	 int p;

    if (!(pj_from = pj_init_plus(from_projection)) ) {
	    printf("\nPROJ4 ERROR: There was a problem creating a PROJ4 object; "
	      "something is\nwrong with the PROJ4 string you supplied."
	      "\nOffending string: '%s'",from_projection);
        exit(1);
	}

    if (!(pj_to = pj_init_plus(to_projection)) ) {
	    printf("\nPROJ4 ERROR: There was a problem creating a PROJ4 object; "
	      "something is\nwrong with the PROJ4 string you supplied."
	      "\nOffending string: '%s'",to_projection);
        exit(1);
	}

    p = pj_transform(pj_from, pj_to, point_count, 1, x, y, NULL );

    pj_free(pj_to);
    pj_free(pj_from);

	return(p);
}

void* thread_main(void* unused)
{
    projCtx p_proj_ctxt;
    projPJ p_WGS84_proj;
    projPJ p_OSGB36_proj;
    
    p_proj_ctxt=pj_ctx_alloc();
    p_WGS84_proj=pj_init_plus_ctx(p_proj_ctxt,"+proj=longlat "
            "+ellps=WGS84 +datum=WGS84 +no_defs");
    p_OSGB36_proj=pj_init_plus_ctx(p_proj_ctxt,
            "+proj=longlat +ellps=airy +datum=OSGB36 +nadgrids=OSTN02_NTv2.gsb "
            "+no_defs");
    
    while(go_on)
    {
        double x, y;
        int proj_ret;

        x = -5.2*DEG_TO_RAD;
        y = 50*DEG_TO_RAD;
        proj_ret = pj_transform(p_WGS84_proj,
                                p_OSGB36_proj, 1, 1, &x, &y, NULL );
        x *= RAD_TO_DEG;
        y *= RAD_TO_DEG;
        /*printf("%.18f %.18f\n", x, y); */
        assert(proj_ret == 0);
        assert(fabs(x - -5.198965360936369962) < 1e-15);
        assert(fabs(y - 49.999396034285531698) < 1e-15);
    }

    return NULL;
}

void Converter::convertLatLonToXY(float lat, float lon, float& x, float&y){
    double tmp_x = lon * DEG_TO_RAD;
    double tmp_y = lat * DEG_TO_RAD;
    pj_transform(src_proj_, dst_proj_, 1, 1, &tmp_x, &tmp_y, NULL);
    x = tmp_x;
    y = tmp_y;
}

Point CartesianInterpolator::project(const Point &src_xy)
{
    Point dest_xy;
    projLP xy;

    xy.u = src_xy.x;
    xy.v = src_xy.y;

    int status = pj_transform(m_src_proj, m_dest_proj, 1, 1, &xy.u, &xy.v, NULL);
    if (status == -14 || status == -20)
    {
        // -14 => "latitude or longitude exceeded limits"
        // -20 => "tolerance condition error"
        xy.u = xy.v = HUGE_VAL;
    }
    else if (status != 0)
    {
        // TODO: Raise a Python exception instead
        std::cerr << "*******************" << std::endl;
        std::cerr << status << std::endl;
        std::cerr << pj_strerrno(status) << std::endl;
        exit(2);
    }

    dest_xy.x = xy.u;
    dest_xy.y = xy.v;
    return dest_xy;
}

void ProjectionSettings::transform(double &x, double &y, double &z)
{
    pj_transform(pj_from, pj_to, 1, 1, &x, &y, &z);
    x += XOffset;
    y += YOffset;
    z += ZOffset;
}

/*!
 * \brief
 * executes reprojection 
 * 
 * JNI informations:
 * Class:     org_proj4_Projections
 * Method:    transform
 * Signature: ([D[D[DLjava/lang/String;Ljava/lang/String;JI)V
 * 
 *
 * \param env - parameter used by jni (see JNI specification)
 * \param parent - parameter used by jni (see JNI specification)
 * \param firstcoord - array of x coordinates
 * \param secondcoord - array of y coordinates
 * \param values - array of z coordinates
 * \param src - definition of the source projection
 * \param dest - definition of the destination projection
 * \param pcount
 * \param poffset
*/
JNIEXPORT void JNICALL Java_org_proj4_Projections_transform
  (JNIEnv * env, jobject parent, jdoubleArray firstcoord, jdoubleArray secondcoord, jdoubleArray values, jstring src, jstring dest, jlong pcount, jint poffset)
{
	int i;
	projPJ src_pj, dst_pj;
	char * srcproj_def = (char *) (*env)->GetStringUTFChars (env, src, 0); 
	char * destproj_def = (char *) (*env)->GetStringUTFChars (env, dest, 0);

	if (!(src_pj = pj_init_plus(srcproj_def)))
		exit(1);
	if (!(dst_pj = pj_init_plus(destproj_def)))
		exit(1);

	double *xcoord = (* env)-> GetDoubleArrayElements(env, firstcoord, NULL); 
	double *ycoord = (* env) -> GetDoubleArrayElements(env, secondcoord, NULL); 
	double *zcoord = (* env) -> GetDoubleArrayElements(env, values, NULL); 

        pj_transform( src_pj, dst_pj, pcount,poffset, xcoord, ycoord, zcoord);

	(* env)->ReleaseDoubleArrayElements(env,firstcoord,(jdouble *) xcoord,JNI_COMMIT);
	(* env)->ReleaseDoubleArrayElements(env,secondcoord,(jdouble *) ycoord,JNI_COMMIT);
	(* env)->ReleaseDoubleArrayElements(env,values,(jdouble *) zcoord,JNI_COMMIT);

	pj_free( src_pj );
	pj_free( dst_pj );
}

void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
    tf::StampedTransform odomToBase;
    tf::Vector3 pos;
    tf::Quaternion quat = lastQuat;
    double y = msg->latitude  / 180.0 * M_PI;
    double x = msg->longitude / 180.0 * M_PI;
    
    pj_transform(pjLatLon, pjUTM, 1, 1, &x, &y, NULL);
    
    pos.setX(x - baseX);
    pos.setY(y - baseY);
    
     try
    {
        tfListener->waitForTransform("/odom", "/base_footprint", msg->header.stamp, ros::Duration(1.0));
        tfListener->lookupTransform("/odom", "/base_footprint", msg->header.stamp, odomToBase);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
    }
    
    quat = quat * odomToBase.getRotation().inverse();
    pos = pos - odomToBase.getOrigin();
    
    tf::Transform mapToOdom(quat, pos);
    
    tfBroadcaster->sendTransform(tf::StampedTransform(mapToOdom, lastOdom.header.stamp, "/map", "/odom"));
}

static VALUE cmethod_proj4_transform(VALUE module, VALUE from, VALUE to, VALUE x, VALUE y, VALUE z)
{
    VALUE result;
    projPJ from_pj;
    projPJ to_pj;
    double xval, yval, zval;
    int err;

    result = Qnil;
    from_pj = RGEO_PROJ4_DATA_PTR(from)->pj;
    to_pj = RGEO_PROJ4_DATA_PTR(to)->pj;
    if (from_pj && to_pj) {
        xval = rb_num2dbl(x);
        yval = rb_num2dbl(y);
        zval = 0.0;
        if (!NIL_P(z)) {
            zval = rb_num2dbl(z);
        }
        err = pj_transform(from_pj, to_pj, 1, 1, &xval, &yval, NIL_P(z) ? NULL : &zval);
        if (!err && xval != HUGE_VAL && yval != HUGE_VAL && (NIL_P(z) || zval != HUGE_VAL)) {
            result = rb_ary_new2(NIL_P(z) ? 2 : 3);
            rb_ary_push(result, rb_float_new(xval));
            rb_ary_push(result, rb_float_new(yval));
            if (!NIL_P(z)) {
                rb_ary_push(result, rb_float_new(zval));
            }
        }
    }
    return result;
}

/**
 * Converts from (target) coordinates to tile coordinates.
 *
 * The zoom level for the coordinates is explicitly given in the
 * variable map_width.
 */
void reprojection::coords_to_tile(double *tilex, double *tiley, double lon, double lat,
                                  int map_width)
{
    double x[1], y[1], z[1];

    x[0] = lon;
    y[0] = lat;
    z[0] = 0;

    if (Proj == PROJ_LATLONG)
    {
        x[0] *= DEG_TO_RAD;
        y[0] *= DEG_TO_RAD;
    }

    /* since pj_tile is always spherical merc, don't bother doing anything if
     *  destination proj is the same. */

    if (Proj != PROJ_SPHERE_MERC)
    {
        pj_transform(pj_target, pj_tile, 1, 1, x, y, z);
        /** FIXME: pj_transform could fail if coordinates are outside +/- 85 degrees latitude */
    }

    /* if ever pj_tile were allowed to be PROJ_LATLONG then results would have to
     *  be divided by DEG_TO_RAD here. */

    *tilex = map_width * (0.5 + x[0] / EARTH_CIRCUMFERENCE);
    *tiley = map_width * (0.5 - y[0] / EARTH_CIRCUMFERENCE);
}

	ERMsg CProjection::Reproject(projPJ src, projPJ dst, long point_count, int point_offset, double *x, double *y, double *z)
	{
		ERMsg msg;

		if (pj_is_latlong(src))
		{
			if (x != NULL)
				*x *= DEG_TO_RAD;
			if (y != NULL)
				*y *= DEG_TO_RAD;
			if (z != NULL)
				*z *= DEG_TO_RAD;
		}

		if (pj_transform(src, dst, point_count, point_offset, x, y, z) == 0)
		{
			msg.ajoute("ERROR: unable to re-projection point (" + ToString(*x) + ", " + ToString(*y) + ((z != NULL) ? ToString(*z) : "") + ")");
		}

		if (pj_is_latlong(dst))
		{
			if (x != NULL)
				*x *= RAD_TO_DEG;
			if (y != NULL)
				*y *= RAD_TO_DEG;
			if (z != NULL)
				*z *= RAD_TO_DEG;
		}

		return msg;
	}

void GridGeometryTest::testGetGeometryHirlam10()
{
	std::ostringstream exp;
	double x0 = 5.75 * DEG_TO_RAD;
	double y0 = -13.25 * DEG_TO_RAD;
	double x1 = (5.75+(247*0.1)) * DEG_TO_RAD;
	double y1 = -13.25 * DEG_TO_RAD;
	double x2 = (5.75+(247*0.1)) * DEG_TO_RAD;
	double y2 = (-13.25+(399*0.1)) * DEG_TO_RAD;
	double x3 = 5.75 * DEG_TO_RAD;
	double y3 = (-13.25+(399*0.1)) * DEG_TO_RAD;
	int error = pj_transform( hirlam10Proj, targetProj, 1, 0, &x0, &y0, NULL );
	if ( error ) {
		std::ostringstream msg;
		msg << "Error during reprojection: " << pj_strerrno(error) << ".";
		CPPUNIT_FAIL( msg.str() );
	}
	error = pj_transform( hirlam10Proj, targetProj, 1, 0, &x1, &y1, NULL );
	if ( error ) {
		std::ostringstream msg;
		msg << "Error during reprojection: " << pj_strerrno(error) << ".";
		CPPUNIT_FAIL( msg.str() );
	}
	error = pj_transform( hirlam10Proj, targetProj, 1, 0, &x2, &y2, NULL );
	if ( error ) {
		std::ostringstream msg;
		msg << "Error during reprojection: " << pj_strerrno(error) << ".";
		CPPUNIT_FAIL( msg.str() );
	}
	error = pj_transform( hirlam10Proj, targetProj, 1, 0, &x3, &y3, NULL );
	if ( error ) {
		std::ostringstream msg;
		msg << "Error during reprojection: " << pj_strerrno(error) << ".";
		CPPUNIT_FAIL( msg.str() );
	}
	exp << "POLYGON((";
	exp << wdb::round(x0 * RAD_TO_DEG, 4) << " " << wdb::round (y0 * RAD_TO_DEG, 4) << ",";
	exp << wdb::round(x1 * RAD_TO_DEG, 4) << " " << wdb::round (y1 * RAD_TO_DEG, 4) << ",";
	exp << wdb::round(x2 * RAD_TO_DEG, 4) << " " << wdb::round (y2 * RAD_TO_DEG, 4) << ",";
	exp << wdb::round(x3 * RAD_TO_DEG, 4) << " " << wdb::round (y3 * RAD_TO_DEG, 4) << ",";
	exp << wdb::round(x0 * RAD_TO_DEG, 4) << " " << wdb::round (y0 * RAD_TO_DEG, 4) << "))";
	const std::string expected = exp.str();
	std::string geometry = grid->wktRepresentation();
	CPPUNIT_ASSERT_EQUAL( expected, geometry);
}

QPointF Projection::transformFrom(Projection *pjOther, QPointF p)
{
  projUV uv;
  double z = 0.0;
  uv.u = p.x() / pjOther->scale;
  uv.v = p.y() / pjOther->scale;
  
  pj_transform(pjOther->pj, pj, 1, 0, &uv.u, &uv.v, &z);
  return QPointF(uv.u, uv.v) * scale;
}