#ifndef IPIN_SCALER_H
#define IPIN_SCALER_H

#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>

struct IPIN {
	double lat;
	double lon;
	double floorNr;
	IPIN(const double lat, const double lon, const double floorNr) : lat(lat), lon(lon), floorNr(floorNr) {;}
};

class IPINScaler {

private:

	int w;
	int h;
	double cenLat;
	double cenLon;
	double rotDeg;
	double mPerPx;

	Point2 mapCenter_m;

	double refLat;
	double m_per_deg_lat;
	double m_per_deg_lon;

	const float fixedFloorHeight = 4.0f;

public:


	/** image width, image height, map-center (lat/lon), map-rotation, meter<->pixel conversion factor */
	IPINScaler(int w, int h, double cenLat, double cenLon, double rotDeg, double mPerPx) : w(w), h(h), cenLat(cenLat), cenLon(cenLon), rotDeg(rotDeg), mPerPx(mPerPx) {

		mapCenter_m = Point2( (w*mPerPx/2), (h*mPerPx/2) );

		refLat = cenLat / 180.0 * M_PI;
		m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat);
		m_per_deg_lon = 111132.954 * std::cos ( refLat );

	}




	/** convert from (x,y,z) to (lat,lon,floorNr) */
	IPIN toIPIN3(const float x, const float y, const float z) const {

		Point2 pos(x,y);

		// move to (0,0)
		pos -= mapCenter_m;

		// undo the rotation
		pos = pos.rotated(-rotDeg / 180 * M_PI);

		const double lat = cenLat + (pos.y / m_per_deg_lat);
		const double lon = cenLon + (pos.x / m_per_deg_lon);
		const double floorNr = z / fixedFloorHeight;

		return IPIN(lat, lon, floorNr);

	}

	IPIN toIPIN3(const Point3 p) const {
		return toIPIN3(p.x, p.y, p.z);
	}

	Point3 convert3D(const IPIN& ipin) const {
		return convert3D(ipin.lat, ipin.lon, ipin.floorNr);
	}

	Point3 convert3D(double lat, double lon, float floorNr) const {

		Point2 p2 = convert2D(lat, lon);

		return Point3(p2.x, p2.y, floorNr * fixedFloorHeight);

	}

	Point2 convert2D(double lat, double lon) const {

		const double y_m = +(lat-cenLat) * m_per_deg_lat;
		const double x_m = +(lon-cenLon) * m_per_deg_lon;

		// rotate (our map is axis aligned)
		Point2 pos(x_m, y_m);
		pos = pos.rotated(rotDeg / 180 * M_PI);

		// apply movement
		pos += mapCenter_m;

		return pos;

	}


};

#endif // IPIN_SCALER_H