/*!
 * Two house numbers are considered to be equal if
 * * country, city, postcode, suburb, street, housenumber, name, and shop equal
 * * or housenumber, street, name, and shop equal, and country, city, and postcode do not differ (ignoring empty values),
 *    and lat/lon difference is less than DISTANCE_THRESHOLD
 */
bool HouseNumber::isSameAddress(HouseNumber const& rhs) const {
	if(getName().toLower()!=rhs.getName().toLower() ||
	   getShop().toLower()!=rhs.getShop().toLower() ||
	   getNumber().toLower()!=rhs.getNumber().toLower() ||
	   getStreet().toLower()!=rhs.getStreet().toLower() ||
	   getNumber()=="" || getStreet()=="") {
		return false;
	}
	
	if(getPostcode().toLower()==rhs.getPostcode().toLower() && getPostcode()!="" &&
	   getCity().toLower()==rhs.getCity().toLower() && getCity()!="" &&
	   getSuburb().toLower()==rhs.getSuburb().toLower() &&
	   getCountry().toLower()==rhs.getCountry().toLower() && getCountry()!="") {
		return true;
	}
	
	// consider two house numbers with similar address information and little distance to each other to be equal
	if(myAbs(getLat()-rhs.getLat())>DISTANCE_THRESHOLD ||
	   myAbs(getLon()-rhs.getLon())>DISTANCE_THRESHOLD)
		return false;
	if(getPostcode()!="" && rhs.getPostcode()!="" && getPostcode().toLower()!=rhs.getPostcode().toLower())
		return false;
	if(getCity()!="" && rhs.getCity()!="" && getCity().toLower()!=rhs.getCity().toLower())
		return false;
	if(getSuburb()!="" && rhs.getSuburb()!="" && getSuburb()!=rhs.getSuburb())
		return false;
	if(getCountry()!="" && rhs.getCountry()!="" && getCountry().toLower()!=rhs.getCountry().toLower())
		return false;
	return true;
}
float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const {
    float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2);
    float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2);
    float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
                  next.getLat()) * sinDeltaLon2 * sinDeltaLon2;
    float c = 2 * atan2(sqrt(a), sqrt(1 - a));
    return rEarth * c;
}
float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const {
    float deltaLon = next.getLon() - getLon();
    /*
     Serial.print("Lon: "); Serial.println(getLon());
     Serial.print("nextLon: "); Serial.println(next.getLon());
     Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt);
     */
    float bearing = atan2(
                        sin(deltaLon) * cos(next.getLat()),
                        cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos(
                            next.getLat()) * cos(deltaLon));
    return bearing;
}
int main(int argc, char *argv[])
{
 fprintf(stderr,"Starting Location Service Tester!\n");

 if ( startLocationServices() )
 {
   unsigned int sampleNumber=0;
   while (1)
   {
   if ( pollLocationServices())
   {
     fprintf(stderr,"Sample %u => %0.2f %0.2f \n",sampleNumber++,getLat(),getLon());

   }
     usleep(10000);
     fprintf(stderr,".");
   }
   stopLocationServices();
 }

 fprintf(stderr,"Done..\n\n");
 return 0;
}