main()
{
      double X;

      for (X = 0.0; X <= 360.0; X += 45.0)
            printf("%3.0f degrees = %.12f radians\n", X, deg2rad(X));
      puts("");
      for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 6))
            printf("%.12f radians = %3.0f degrees\n", X, rad2deg(X));
      return 0;
}

int mympu_update() {

	do {
		ret = dmp_read_fifo(gyro,NULL,q._l,NULL,&sensors,&fifoCount);
		/* will return:
			0 - if ok
			1 - no packet available
			2 - if BIT_FIFO_OVERFLOWN is set
			3 - if frame corrupted
		       <0 - if error
		*/

		if (ret!=0) return ret; 
	} while (fifoCount>1);

	q._f.w = (float)q._l[0] / (float)QUAT_SENS;
	q._f.x = (float)q._l[1] / (float)QUAT_SENS;
	q._f.y = (float)q._l[2] / (float)QUAT_SENS;
	q._f.z = (float)q._l[3] / (float)QUAT_SENS;

    mympu.quat.w = q._f.w;
    mympu.quat.x = q._f.x;
    mympu.quat.y = q._f.y;
    mympu.quat.z = q._f.z;

	quaternionToEuler( &q._f, &mympu.ypr[2], &mympu.ypr[1], &mympu.ypr[0] );
	
	/* need to adjust signs and do the wraps depending on the MPU mount orientation */ 
	/* if axis is no centered around 0 but around i.e 90 degree due to mount orientation */
	/* then do:  mympu.ypr[x] = wrap_180(90.f+rad2deg(mympu.ypr[x])); */
	mympu.ypr[0] = rad2deg(mympu.ypr[0]);
	mympu.ypr[1] = rad2deg(mympu.ypr[1]);
	mympu.ypr[2] = rad2deg(mympu.ypr[2]);

	/* need to adjust signs depending on the MPU mount orientation */ 
	mympu.gyro[0] = -(float)gyro[2] / GYRO_SENS;
	mympu.gyro[1] = (float)gyro[1] / GYRO_SENS;
	mympu.gyro[2] = (float)gyro[0] / GYRO_SENS;

	return 0;
}

TCFloat LDExporter::getHFov(void)
{
	if (m_width > m_height)
	{
		return (TCFloat)(2.0 * rad2deg(atan(tan(deg2rad(m_fov / 2.0)) *
			(double)m_width / (double)m_height)));
	}
	else
	{
		return m_fov;
	}
}

/*  Purpose: calculate the difference between true solar time and mean
 *              solar time (minutes)
 */
double calcEquationOfTime(double t) {
    double l0 = deg2rad(calcGeomMeanLongSun(t));
    double e = calcEccentricityEarthOrbit(t);
    double m = deg2rad(calcGeomMeanAnomalySun(t));
    double y = tan(deg2rad(calcObliquityCorrection(t))/2.0);
    double sinm = sin(m);

    y *= y;

    return rad2deg(y*sin(2.0*l0) - 2.0*e*sinm + 4.0*e*y*sinm*cos(2.0*l0)
                    - 0.5*y*y*sin(4.0*l0) - 1.25*e*e*sin(2.0*m))*4.0;
}

void rpi_vector::calc(){
    if (calcd) return;
    if (mode) {
        x = r * sin(deg2rad(lat)) * cos(deg2rad(longit));
        y = r * sin(deg2rad(lat)) * sin(deg2rad(longit));
        z = r * cos(deg2rad(lat)) ;
    }else{

        r = sqrt(x*x+y*y+z*z);
        if (z!=0)
        {
            lat = rad2deg(acos(z/r));
            longit = rad2deg(acos(x/(r * sin(deg2rad(lat)))));
        }else{
            lat=0;
            longit = rad2deg(atan(y/x));
        }

    }
    calcd = true;
}

double sun_hour_angle_at_rise_set(double latitude, double delta_zero, double h0_prime)
{
    double h0             = -99999;
    double latitude_rad   = deg2rad(latitude);
    double delta_zero_rad = deg2rad(delta_zero);
    double argument       = (sin(deg2rad(h0_prime)) - sin(latitude_rad)*sin(delta_zero_rad)) /
                            (cos(latitude_rad)*cos(delta_zero_rad));

    if (fabs(argument) <= 1) h0 = limit_degrees180(rad2deg(acos(argument)));

    return h0;
}

/*
   VA == 0
     A = undefined
   VB == 0
     A = W
             /   2    2    2 \
         -1 |  VA + VB - VW   |
   A = cos  |  ------------   |
             \   2 VA VB     /

        -1 / VW      \
  A = sin |  -- sin W |
           \ VA      /

*/
double Polar::DirectionApparentWind(double VA, double VB, double W, double VW)
{
  if(VA == 0) /* apparent wind direction is not defined */
    return 0;
  if(VB == 0) /* trig identity breaks down, but if we aren't */
    return W; /*   moving, apparent wind is true wind */
  double cosA = (VA*VA + VB*VB - VW*VW) / (2*VA*VB);
  if(cosA > 1) cosA = 1; else if(cosA < -1) cosA = -1; /* slight arithmatic errors */
  double ac = acos(cosA);
  while(W > 180)  W-=360;
  while(W < -180) W+=360;
  return rad2deg(W > 0 ? ac : -ac);
}

/* request geotagged wikipedia entries for current map view */
void maep_geonames_entry_request(coord_t *pt1, coord_t *pt2,
                                 MaepGeonamesRequestCallback cb, gpointer obj)
{
  request_cb_t *context;
  /* create ascii (dot decimal point) strings */
  char str[4][16];
  g_ascii_formatd(str[0], sizeof(str[0]), "%.07f", rad2deg(pt1->rlat));
  g_ascii_formatd(str[1], sizeof(str[1]), "%.07f", rad2deg(pt2->rlat));
  g_ascii_formatd(str[2], sizeof(str[2]), "%.07f", rad2deg(pt1->rlon));
  g_ascii_formatd(str[3], sizeof(str[3]), "%.07f", rad2deg(pt2->rlon));

  gchar *locale, lang[3] = { 0,0,0 };
  gchar *lang_avail[] = {"de", "en", "es", "fr", "it", "nl", "pl", "pt", "ru", "zh", NULL};
  int i;
  locale = setlocale (LC_MESSAGES, NULL);
  g_utf8_strncpy (lang, locale, 2);

  /* currently only "de" and "en" are supported by geonames.org */
  /* force to "en" in any other case */
  g_message("Look for entries in %s", lang);
  for (i = 0; lang_avail[i] && strcasecmp(lang, lang_avail[i]); i++);
  if (!lang_avail[i]) 
    strncpy(lang, "en", 2);
      
  /* build complete url for request */
  char *url = g_strdup_printf(
      GEONAMES "wikipediaBoundingBox?"
      "north=%s&south=%s&west=%s&east=%s&lang=%s&maxRows=%u&username=" PACKAGE, 
      str[0], str[1], str[2], str[3], lang, MAX_RESULT);

  /* start download in background */
  g_message("start asynchronous geonames download.");
  context = g_malloc0(sizeof(request_cb_t));
  context->cb = cb;
  context->obj = obj;
  net_io_download_async(url, geonames_request_cb, context);

  g_free(url);
}

void
computeCorr( logtools_rpos2_t pos, logtools_rpos2_t map, logtools_rpos2_t *corr )
{
  logtools_rpos2_t map2, pos2;

  map2   = map;
  map2.o = rad2deg( map.o );

  pos2   = pos;
  pos2.o = 90.0 - pos.o;

  compute_correction_parameters( pos2, map2, corr );
}

void TfraEmitter::GetInfoNext(const PS::SEmitterDef& E){
    pcEmitterType->ActivePageIndex	= E.m_EmitterType;
	// cone
	seConeDirH->ObjNextInit			(rad2deg(E.m_ConeHPB.x));
    seConeDirP->ObjNextInit			(rad2deg(E.m_ConeHPB.y));
    seConeDirB->ObjNextInit			(rad2deg(E.m_ConeHPB.z));
    seConeAngle->ObjNextInit		(rad2deg(E.m_ConeAngle));
    // sphere
    seSphereRadius->ObjNextInit		(E.m_SphereRadius);
    // box
    seBoxSizeX->ObjNextInit			(E.m_BoxSize.x);
    seBoxSizeY->ObjNextInit			(E.m_BoxSize.y);
    seBoxSizeZ->ObjNextInit			(E.m_BoxSize.z);
    // birth
    seBirthRate->ObjNextInit		(E.m_fBirthRate);
    ebBirthFunc->Down				= E.m_Flags.is(PS_EM_BIRTHFUNC);
    seParticleLimit->ObjNextInit	(E.m_ParticleLimit);
	// burst
	cbBurst->ObjNextInit			((E.m_Flags.is(PS_EM_BURST))?cbChecked:cbUnchecked);
    // play once
    cbPlayOnce->ObjNextInit			((E.m_Flags.is(PS_EM_PLAY_ONCE))?cbChecked:cbUnchecked);
}

double distance(double lat1, double lon1)
{

  double theta, dist;
fprintf(stderr, "lastlat %f lastlon %f\n", lastlat, lastlon);


  theta = lon1 - lastlon;
  dist = sin(deg2rad(lat1)) * sin(deg2rad(lastlat)) + cos(deg2rad(lat1)) * cos(deg2rad(lastlat)) * cos(deg2rad(theta));
  dist = acos(dist);
  dist = rad2deg(dist);
  dist = dist * 60 * 1.1515;
  dist = dist * (1.609344);
dist=dist*1000;
c.distance[i]=dist;
double dLon = deg2rad(theta);
double y = sin(dLon) * cos(lat1);
double x = cos(lastlat)*sin(lat1) -
        sin(lastlat)*cos(lat1)*cos(dLon);
double brng =atan2(y,x);
c.angle[i]=brng;
fprintf(stderr,"brng %f\n",brng);
lastlat =lat1;
lastlon=lon1;
c.header.frame_id="cmd";
c.header.stamp = ros::Time::now();
c.num_of_waypoints=i+1;
//c.distance[]={0,0,0,0,0};
//c.angle[]={0,0,0,0,0};
//temp1[i]=dist;
//m++;
//if(m==5)
//{
//for(i=0;i=5;i++)
//c.distance[i]=temp1[i];
//}
if(i==5)

{
pub_cmd.publish(c);
}
i++;
for(int j=0;j<=5;j++)
{

fprintf(stderr,"temp %f\n",c.distance[j]);

}
fprintf(stderr,"m %d\n",m);
  return (dist);
}

void DashboardInstrument_Compass::DrawCompassRose(wxBufferedDC* dc)
{
      wxPoint TextPoint, points[3];
      wxString Value;
      int width, height;
      wxString CompassArray[] = {_("N"),_("NE"),_("E"),_("SE"),_("S"),_("SW"),_("W"),_("NW"),_("N")};

      int tmpradius = m_radius * 0.75;

      dc->SetFont(*g_pFontSmall);

      wxColour cl;
      wxPen pen;
      pen.SetStyle(wxSOLID);
      GetGlobalColor(_T("BLUE1"), &cl);
      pen.SetColour(cl);
      dc->SetPen(pen);
      dc->SetTextForeground(cl);
      //dc->SetPen(*wxTRANSPARENT_PEN);

      int offset = 0;
      for(double tmpangle = m_AngleStart - ANGLE_OFFSET;
                        tmpangle <= m_AngleStart + 360 - ANGLE_OFFSET; tmpangle+=45)
      {
            Value = CompassArray[offset];
            dc->GetTextExtent(Value, &width, &height, 0, 0, g_pFontSmall);
            double x = width/2;
            long double anglefortext = asin((x/tmpradius));
            anglefortext = tmpangle - rad2deg(anglefortext);
            TextPoint.x = m_cx + tmpradius * cos(deg2rad(anglefortext));
            TextPoint.y = m_cy + tmpradius * sin(deg2rad(anglefortext));
            dc->DrawRotatedText(Value, TextPoint.x,
                                                TextPoint.y, -90 - tmpangle);

            dc->SetBrush(*wxTRANSPARENT_BRUSH);
            points[0].x = m_cx;
            points[0].y = m_cy;
            points[1].x = m_cx + tmpradius * 0.1 * cos(deg2rad(tmpangle-45));
            points[1].y = m_cy + tmpradius * 0.1 * sin(deg2rad(tmpangle-45));
            double size = (offset % 2 ? 0.50 : 0.80);
            points[2].x = m_cx + tmpradius * size * cos(deg2rad(tmpangle));
            points[2].y = m_cy + tmpradius * size * sin(deg2rad(tmpangle));
            dc->DrawPolygon(3, points, 0, 0);

            points[1].x = m_cx + tmpradius * 0.1 * cos(deg2rad(tmpangle+45));
            points[1].y = m_cy + tmpradius * 0.1 * sin(deg2rad(tmpangle+45));
            dc->SetBrush(cl);
            dc->DrawPolygon(3, points, 0, 0);
            offset++;
      }
}

void pose_esti::vslam_poseCallback(const geometry_msgs::PoseWithCovarianceStampedPtr &msg)
{
	yaw_now=ros::Time::now();
   ptam_pose = *msg;
	if( pick_ptam_yaw_init_time && (vslam_states.data=="PTAM_initialized"))
	{
		//ROS_INFO("%f %f %f\n",ptam_pose.pose.pose.position.x,ptam_pose.pose.pose.position.y,ptam_pose.pose.pose.position.z);
		tf::Quaternion temp;
		//
		tf::Quaternion q(0,0,0,0);
		tf::quaternionMsgToTF(ptam_pose.pose.pose.orientation, temp);
		tfScalar ptam_yaw, ptam_pitch, ptam_roll;
		tf::Matrix3x3(temp).getRPY(ptam_roll, ptam_pitch, ptam_yaw);

      
		double cmd_temp_yaw;
	    double ptam_yaw_temp = rad2deg(-(ptam_yaw+M_PI/2));
		//double ptam_yaw_temp = rad2deg(ptam_yaw);
		PTAM_Yaw=ptam_yaw_temp;
   
		cmd_temp_yaw=pid_yaw.updatePid(ptam_yaw_temp-goal_pose.yaw,yaw_now-yaw_past);
		cout<<"pid output yaw = "<<cmd_temp_yaw<<endl;
		cmd_yaw.linear.x=0;
		cmd_yaw.linear.y=0;
		cmd_yaw.linear.z=0;
		cmd_yaw.angular.x=0;
		cmd_yaw.angular.y=0;
		cmd_yaw.angular.z=cmd_temp_yaw;
		cmd_yaw_pub.publish(cmd_yaw);
		cout<<"imu yaw = "<<rad2deg(yaw_test)<<endl<<
			  "ptam yaw= "<<ptam_yaw_temp<<endl;
	}
	else
	{
		yaw_past = yaw_now;
		pick_ptam_yaw_init_time=1;
	}
}

	// Michael's method
	void Euler_matrix2angles(Matrix2D<DOUBLE> A, DOUBLE *alpha, DOUBLE *beta,
		DOUBLE *gamma)
	{
		DOUBLE abs_sb;

		if (ABS(A(1, 1)) > FLT_EPSILON)
		{
			abs_sb = sqrt((-A(2, 2) * A(1, 2) * A(2, 1) - A(0, 2) * A(2, 0)) / A(1, 1));
		}
		else if (ABS(A(0, 1)) > FLT_EPSILON)
		{
			abs_sb = sqrt((-A(2, 1) * A(2, 2) * A(0, 2) + A(2, 0) * A(1, 2)) / A(0, 1));
		}
		else if (ABS(A(0, 0)) > FLT_EPSILON)
		{
			abs_sb = sqrt((-A(2, 0) * A(2, 2) * A(0, 2) - A(2, 1) * A(1, 2)) / A(0, 0));
		}
		else
			EXIT_ERROR(1, "Don't know how to extract angles");

		if (abs_sb > FLT_EPSILON)
		{
			*beta  = atan2(abs_sb, A(2, 2));
			*alpha = atan2(A(2, 1) / abs_sb, A(2, 0) / abs_sb);
			*gamma = atan2(A(1, 2) / abs_sb, -A(0, 2) / abs_sb);
		}
		else
		{
			*alpha = 0;
			*beta  = 0;
			*gamma = atan2(A(1, 0), A(0, 0));
		}

		*gamma = rad2deg(*gamma);
		*beta  = rad2deg(*beta);
		*alpha = rad2deg(*alpha);
	}

float followStraightPath(float* waypointDirection, float* targetWaypoint, float* position, float heading){ //Heading in degrees (magnetic)
    heading = deg2rad(90 - heading);//90 - heading = magnetic heading to cartesian heading
    float courseAngle = atan2(waypointDirection[1], waypointDirection[0]); // (y,x) format
    while (courseAngle - heading < -PI){
        courseAngle += 2 * PI;
    }
    while (courseAngle - heading > PI){
        courseAngle -= 2 * PI;
    }

    float pathError = -sin(courseAngle) * (position[0] - targetWaypoint[0]) + cos(courseAngle) * (position[1] - targetWaypoint[1]);
    float calcHeading = 90 - rad2deg(courseAngle - MAX_PATH_APPROACH_ANGLE * 2/PI * atan(k_gain[PATH] * pathError)); //Heading in degrees (magnetic)
    return calcHeading;

}

/* This function convert body quaternion to body wind angles, see reference in MathLab DCM->Wind. */
static BOARD_ERROR be_api_body_angle_QuaternionToWindAngles(void)
{
    BOARD_ERROR be_result = BOARD_ERR_OK;
    float q0 = fl_api_body_angle_quaternion[0];
    float q1 = fl_api_body_angle_quaternion[1];
    float q2 = fl_api_body_angle_quaternion[2];
    float q3 = fl_api_body_angle_quaternion[3];  /* short name local variable for readability. */
    float fl_a, fl_b;

    /* TODO: should be added dividing by zero controls. */
    /* Mu angle. */
    fl_a = 2.0f * (q2 * q3 + q0 * q1);
    fl_b = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    fl_api_body_angle_wind_angles[0] = rad2deg((float)atan2((double)fl_a,(double)fl_b));
    /* Gamma angle. */
    fl_a = -2.0f * (q1 * q3 - q0 * q2);
    fl_api_body_angle_wind_angles[1] = rad2deg((float)asin((double)fl_a));
    /* Kzetta angle. */
    fl_a = 2.0f * (q1 * q2 + q0 * q3);
    fl_b = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
    fl_api_body_angle_wind_angles[2] = rad2deg((float)atan2((double)fl_a,(double)fl_b));

    return(be_result);
}

float SVGAngleValue::value() const
{
    switch (m_unitType) {
    case SVG_ANGLETYPE_GRAD:
        return grad2deg(m_valueInSpecifiedUnits);
    case SVG_ANGLETYPE_RAD:
        return rad2deg(m_valueInSpecifiedUnits);
    case SVG_ANGLETYPE_UNSPECIFIED:
    case SVG_ANGLETYPE_UNKNOWN:
    case SVG_ANGLETYPE_DEG:
        return m_valueInSpecifiedUnits;
    }
    ASSERT_NOT_REACHED();
    return 0;
}

void
show_window6()
{
	GladeXML *gladexml;
	GtkWidget *dialog;
	GtkWidget *entry14, *entry15, *combobox2;

	double lat, lon, lat_deg, lon_deg;
	char buf[64];

	gladexml = glade_xml_new (gladefile, "window6", GETTEXT_PACKAGE);
	glade_xml_signal_autoconnect (gladexml);
	dialog = glade_xml_get_widget (gladexml, "window6");
	g_signal_connect_swapped (dialog, "destroy",
				  G_CALLBACK (g_object_unref), gladexml);
	gtk_widget_show(dialog);
	new_dialog = TRUE;
	
	
	lat = pixel2lat(global_zoom, global_y+mouse_y);
	lon = pixel2lon(global_zoom, global_x+mouse_x);
	lat_deg = rad2deg(lat);
	lon_deg = rad2deg(lon);
	entry14 = lookup_widget(dialog, "entry14");
	entry15 = lookup_widget(dialog, "entry15");
	
	g_sprintf(buf, "%f", lat_deg);
	gtk_entry_set_text(GTK_ENTRY(entry14), buf);
	g_sprintf(buf, "%f", lon_deg);
	gtk_entry_set_text(GTK_ENTRY(entry15), buf);
	
	
	combobox2 = lookup_widget(dialog, "combobox2");
	gtk_combo_box_set_active(GTK_COMBO_BOX(combobox2), 0);

}

double PolarMeasurement::W() const
{
//        VB*sin(W)=VA*sin(W-A);
//        sin(W)/(cos(A)*sin(W)-sin(A)*cos(W)) = VA/ VB;
//        W = atan(VA*sin(A) / (VA*cos(A)-VB))
//
//    return rad2deg(atan(VA*sin(deg2rad(A)) / (VA*cos(deg2rad(A))-VB)));
//
//    VA = sqrt(VW^2 + VB^2 + 2*VW*VB*cos(W))
//    VA*VA = VW*VW + VB*VB + 2*VW*VB*cos(W)
//    (VA*VA - VW*VW - VB*VB)/(2*VW*VB) = cos(W)
//    W = acos(VA*VA - VW*VW - VB*VB)/(2*VW*VB))
    double vw = VW();
    return rad2deg(acos((VA*VA - vw*vw - VB*VB)/(2*vw*VB)));
}

//draw a line
void WindowSFML::line(V2d a, V2d b, double thickness)
{
	double rot=-atan2(b.y-a.y, b.x-a.x);
	sf::RectangleShape rectangle(sf::Vector2f(dst(a, b), thickness));
	rectangle.setFillColor(sfmlDrawClr());
	rectangle.setPosition(a.x+sin(rot)*thickness/2, dim.y-a.y-cos(rot)*thickness/2);
	rectangle.setRotation(rad2deg(rot));
	
	windowObj.draw(rectangle);
	
	//circle(a, thickness/2);
	//circle(b, thickness/2);
	
	//circle(a, drawThick/2, ::clr(0, 0, 0), 0.5);
	//circle(b, drawThick/2, ::clr(0, 0, 0), 0.5);
}