void configure() {
		SphericalCoordinates sun(m_sun);
		sun.elevation *= m_stretch;
		m_sunDir = toSphere(sun);

		/* Solid angle covered by the sun */
		m_theta = degToRad(SUN_APP_RADIUS * 0.5f);
		m_solidAngle = 2 * M_PI * (1 - std::cos(m_theta));
		m_radiance = computeSunRadiance(m_sun.elevation, m_turbidity) * m_scale;
	}

/*
 * +ve angles are anticlockwise, angle in degrees
 */
void Turn(float a)
{
  int i = (a < 0) ? -1 : 1;
  setMotorSpeeds(-i*TURN_SPEED, i*TURN_SPEED);
  writeDebugStreamLine("TURN: %f", a);

  // Correction for motor bias
  wait1Msec(i*degToRad(a)*(float)TURN_CONST/(float)TURN_SPEED);
  setMotorSpeeds(0, 0);
}

void leftOneWheelDegree(PidMotion* pidMotion, float angleDegree, float a, float speed, OutputStream* notificationOutputStream) {
    // We multiply by 2, because, only one wheel rotates
    float angleRadius = degToRad(angleDegree) * 2.0f;
    RobotKinematics* robotKinematics = getRobotKinematics();
    float leftWheelLengthForOnePulse = getCoderLeftWheelLengthForOnePulse(robotKinematics);
    float wheelsDistanceFromCenter = getCoderWheelsDistanceFromCenter(robotKinematics);

    float realDistanceRight = (wheelsDistanceFromCenter * angleRadius) / leftWheelLengthForOnePulse;
    gotoSimplePosition(pidMotion, 0.0f, realDistanceRight, a, speed, notificationOutputStream);
}

void idle() {
  //player1->angle = glutGet(GLUT_ELAPSED_TIME) / 1000.0 * 45;  // 45° per second
	timeClock = glutGet(GLUT_ELAPSED_TIME);
	timeDiff = timeClock - timePrev;
	timePrev = timeClock;

	if(WeaTimer < 5000){
		weapon->pos[0] = weapon->pos[0]+(timeDiff*0.02 * sin(degToRad(weapon->angle)));
		weapon->pos[2] = weapon->pos[2]+(timeDiff*0.02 * cos(degToRad(weapon->angle)));
  		WeaTimer ++;
	}
	if(WeaTimer2 < 5000){
		weapon2->pos[0] = weapon2->pos[0]+(timeDiff*0.02 * sin(degToRad(weapon2->angle)));
		weapon2->pos[2] = weapon2->pos[2]+(timeDiff*0.02 * cos(degToRad(weapon2->angle)));
  		WeaTimer2 ++;
	}

  glutPostRedisplay();
}

void draw_separator(t_meter *x,  t_object *view, t_rect *rect)
{
    t_jgraphics *g = jbox_start_layer((t_object *)x, view, hoa_sym_separator_layer, rect->width, rect->height);
    
	if (g)
	{
        double rotateAngle, channelWidth;
        t_jmatrix transform;
        t_jrgba black = rgba_addContrast(x->f_color_mbg, -0.12);
        
		jgraphics_matrix_init(&transform, 1, 0, 0, -1, x->f_center, x->f_center);
		jgraphics_set_matrix(g, &transform);
		
		// skelton separators and leds bg:
		for(int i=0; i < x->f_meter->getNumberOfPlanewaves(); i++)
		{
			channelWidth = radToDeg(x->f_meter->getPlanewaveWidth(i));
            rotateAngle = radToDeg(x->f_meter->getPlanewaveAzimuthMapped(i) + x->f_meter->getPlanewavesRotationX()) - (channelWidth*0.5);
			if (!x->f_rotation)
			{
				rotateAngle += channelWidth;
				rotateAngle *= -1;
			}
			jgraphics_rotate(g, degToRad(rotateAngle));

            // separator
			if (x->f_meter->getNumberOfPlanewaves() > 1)
			{
                jgraphics_set_line_width(g, 1);
                jgraphics_set_source_jrgba(g, &black);
				jgraphics_move_to(g, 0., x->f_rayonInt);
				jgraphics_line_to(g, 0, x->f_rayonExt);
				jgraphics_stroke(g);
			}
			
			jgraphics_rotate(g, degToRad(-rotateAngle));
		}
		
		jbox_end_layer((t_object*)x, view, hoa_sym_separator_layer);
	}
	jbox_paint_layer((t_object *)x, view, hoa_sym_separator_layer, 0., 0.);
}

// Checks for collision between avatar and room objects
bool hasObjCollision(const Mesh* a, ComplexObj *cobj[], int sizee){
	VECTOR3D personMin, personMax;
	float rad = degToRad(a->angles.y);
	float xabs = fabs(0.25 * cos(rad) + 0.1 * sin(rad));
	float zabs = fabs(0.25 * sin(rad) + 0.1 * cos(rad));
	personMin.x = a->translation.x - xabs;
	personMin.y = 0.0;
	personMin.z = a->translation.z - zabs;
	personMax.x = a->translation.x + xabs;
	personMax.y = 2.17;
	personMax.z = a->translation.z + zabs;

	for (int i = 0; i < sizee; i++){
		float radobj = degToRad(cobj[i]->angles.y);
		float xabsobj = fabs(cobj[i]->dim.x / 2 * cos(radobj) + cobj[i]->dim.z / 2 * sin(radobj));
		float yabsobj = cobj[i]->dim.y;
		float zabsobj = fabs(cobj[i]->dim.x / 2 * sin(radobj) + cobj[i]->dim.z / 2 * cos(radobj));
		VECTOR3D objMin, objMax;
		objMin.x = cobj[i]->translation.x - xabsobj;
		objMin.y = cobj[i]->translation.y;
		objMin.z = cobj[i]->translation.z - zabsobj;
		objMax.x = cobj[i]->translation.x + xabsobj;
		objMax.y = cobj[i]->translation.y + yabsobj;
		objMax.z = cobj[i]->translation.z + zabsobj;

		if ( (personMin.x < objMax.x && objMin.x < personMax.x) && 
			 (personMin.z < objMax.z && objMin.z < personMax.z) && 
			 (personMin.y < objMax.y && objMin.z < personMax.z) ) 
			return true;

		/*
		if (fabs(a->translation.x - cobj[i]->translation.x) < a->scaleFactor.x + cobj[i]->dim.x / 2){ //check the X-axis
			if (fabs(a->translation.y - cobj[i]->translation.y) < a->scaleFactor.y + cobj[i]->dim.y / 2){ //check the X-axis
				if (fabs(a->translation.z - cobj[i]->translation.z) < a->scaleFactor.z + cobj[i]->dim.z / 2){ //check the X-axis
					return true;
				}
			}
		}
		*/
	}
	return false;
}

void moveThroughDoor()
{
	// common variables
	Ped playerPed = PLAYER::PLAYER_PED_ID();

	if (PED::IS_PED_IN_ANY_VEHICLE(playerPed, 0))
	{
		return;
	}

	Vector3 curLocation = ENTITY::GET_ENTITY_COORDS(playerPed, 0);
	float curHeading = ENTITY::GET_ENTITY_HEADING(playerPed);

	float forwardPush = 0.6;

	float xVect = forwardPush * sin(degToRad(curHeading)) * -1.0f;
	float yVect = forwardPush * cos(degToRad(curHeading));

	ENTITY::SET_ENTITY_COORDS_NO_OFFSET(playerPed, curLocation.x + xVect, curLocation.y + yVect, curLocation.z, 1, 1, 1);
}

double calcSunEqOfCenter(double t)
{
    double m = calcGeomMeanAnomalySun(t);

    double mrad = degToRad(m);
    double sinm = sin(mrad);
    double sin2m = sin(mrad+mrad);
    double sin3m = sin(mrad+mrad+mrad);

    double C = sinm * (1.914602 - t * (0.004817 + 0.000014 * t)) + sin2m * (0.019993 - 0.000101 * t) + sin3m * 0.000289;
    return C;		// in degrees
}

    void Astronomy::getHorizontalSunPosition (
        double jday,
        double longitude, double latitude,
        double &azimuth, double &altitude)
    {
        // 2451544.5 == Astronomy::getJulianDayFromGregorianDateTime(2000, 1, 1, 0, 0, 0));
        // 2451543.5 == Astronomy::getJulianDayFromGregorianDateTime(1999, 12, 31, 0, 0, 0));
        double d = jday - 2451543.5;

        // Sun's Orbital elements:
        // argument of perihelion
        double w = double (282.9404 + 4.70935E-5 * d);
        // eccentricity (0=circle, 0-1=ellipse, 1=parabola)
        double e = 0.016709 - 1.151E-9 * d;
        // mean anomaly (0 at perihelion; increases uniformly with time)
        double M = double(356.0470 + 0.9856002585 * d);
        // Obliquity of the ecliptic.
        //double oblecl = double (23.4393 - 3.563E-7 * d);

        // Eccentric anomaly
        double E = M + radToDeg(e * sinDeg (M) * (1 + e * cosDeg (M)));

        // Sun's Distance(R) and true longitude(L)
        double xv = cosDeg (E) - e;
        double yv = sinDeg (E) * sqrt (1 - e * e);
        //double r = sqrt (xv * xv + yv * yv);
        double lon = atan2Deg (yv, xv) + w;
        double lat = 0;

        double lambda = degToRad(lon);
        double beta = degToRad(lat);
        double rasc, decl;
        convertEclipticToEquatorialRad (lambda, beta, rasc, decl);
        rasc = radToDeg(rasc);
        decl = radToDeg(decl);

        // Horizontal spherical.
        Astronomy::convertEquatorialToHorizontal (
            jday, longitude, latitude, rasc, decl, azimuth, altitude);
    }

void StatePlaying::initiate()
{
  float farClippingPlane = 2000.0f;

  // Set far clipping plane
  gRenderer.setNearFarClippingPlanes(1.0f,farClippingPlane);    

  m_objects = new Ned3DObjectManager();	
	m_objects->setNumberOfDeadFrames(2);
	m_tetherCamera = new TetherCamera(m_objects);
	
  // Create terrain
  terrain = new Terrain(8,"terrain.xml"); //powers of two for terrain size
  m_objects->spawnTerrain(terrain);
  
  // Load models
  m_objects->setModelManager(gModelManager);
  gModelManager.importXml("models.xml");

  // Loads game objects like the crows, plane, and silo
	resetGame();
  
  // set fog
  gRenderer.setFogEnable(true);
  gRenderer.setFogDistance(farClippingPlane - 1000.0f,farClippingPlane);
  gRenderer.setFogColor(MAKE_ARGB(0,60,180,254));

  // set lights
  gRenderer.setAmbientLightColor(MAKE_ARGB(255,100,100,100));
  gRenderer.setDirectionalLightColor(0XFFFFFFFF);
  Vector3 dir = Vector3(5.0f,-5.0f, 6.0f);
  dir.normalize();
  gRenderer.setDirectionalLightVector(dir);
  	
  // Create water now that we know what camera to use
  float fov = degToRad(gGame.m_currentCam->fov);
  water = new Water(fov, farClippingPlane, "water.xml");
  
  m_objects->spawnWater(water);

	// aquire sounds  
  gSoundManager.setDopplerUnit(1.0f/3.0f); // sound factors
  // get windmill sound
  m_windmillSound = gSoundManager.requestSoundHandle("windmill2.wav");
  m_windmillSoundInstance = gSoundManager.requestInstance(m_windmillSound);  

  // add console commands  
  gConsole.addFunction("camerafollow","",consoleSetFollowCamera);
  gConsole.addFunction("cameratarget","s",consoleSetCameraTarget);
  gConsole.addFunction("godmode","b",consoleGodMode);

}

void EnemyShip::createEngagementRangeShape() {
  const float kSpread = 45.f;
  const int kSteps = 9;
  static const sf::Color color{255, 0, 0, 127};

  m_engagementRangeShape.setPrimitiveType(sf::TrianglesFan);
  m_engagementRangeShape.resize(2 + kSteps);

  m_engagementRangeShape[0].position = sf::Vector2f(75.f, 0.f);
  m_engagementRangeShape[0].color = color;

  size_t i = 1;
  const float spreadStep = kSpread / static_cast<float>(kSteps);
  const float half = kSpread / -2.f;
  for (float degrees = 0.f; degrees <= kSpread; degrees += spreadStep, ++i) {
    m_engagementRangeShape[i].position.x =
        std::cos(degToRad(half + degrees)) * kMaxEngagementRange;
    m_engagementRangeShape[i].position.y =
        std::sin(degToRad(half + degrees)) * kMaxEngagementRange;
    m_engagementRangeShape[i].color = color;
  }
}

void PlayerBase::draw()
{
	glBindTexture(GL_TEXTURE_2D, texture);
	glColor3f(color);
	glPushMatrix();
	glTranslatef(frame.position[0], frame.position[1] + 0.2*sin(degToRad(flyingAngle)), frame.position[2]);
	glMultMatrixf(glm::value_ptr(frame.frame));

	mesh->render();
//	drawGeometry(aabb);

	glPopMatrix();
}

    Quaternion ParseEulerAngles(const std::string& text)
    {
        Quaternion quat;
        
        StringVector components = SplitString(text, ' ');
        if (components.size() == 3)
        {
            try
            {
                float xrad = degToRad(ParseString<float>(components[0]));
                float yrad = degToRad(ParseString<float>(components[1]));
                float zrad = degToRad(ParseString<float>(components[2]));
                
                float angle = yrad * 0.5;
                double cx = cos(angle);
                double sx = sin(angle);

                angle = zrad * 0.5;
                double cy = cos(angle);
                double sy = sin(angle);

                angle = xrad * 0.5;
                double cz = cos(angle);
                double sz = sin(angle);

                quat.x = sx * sy * cz + cx * cy * sz;
                quat.y = sx * cy * cz + cx * sy * sz;
                quat.z = cx * sy * cz - sx * cy * sz;
                quat.w = cx * cy * cz - sx * sy * sz;
                
                quat.normalize();
            }
            catch (boost::bad_lexical_cast)
            {
            }
        }
        
        return quat;
    }

    Camera()
    {
        right      = Vector3(1.0f, 0.0f, 0.0f);
        up         = Vector3(0.0f, 1.0f, 0.0f);
        forward    = Vector3(0.0f, 0.0f, 1.0f);
        eye        = Vector3(0.0f, 0.0f, 0.0f);
        viewMatrix = Matrix4::identity();
        vpMatrix   = Matrix4::identity();

        const float fovY   = degToRad(60.0f);
        const float aspect = static_cast<float>(windowWidth) / static_cast<float>(windowHeight);
        projMatrix = Matrix4::perspective(fovY, aspect, 0.1f, 1000.0f);
    }

/**
 * @brief
 *
 * @param nominal
 * @param actual
 * @return double
 */
double NewRoboControl::getSpeedP(double nominal, double actual) // Drehgeschwindigkeit ruhig
{

    double diff = getDiffAngle(nominal, actual);
    // if (std::fabs(diff) < 1.57)
    {
        if (std::fabs(diff) < degToRad(19) || std::fabs(diff) > degToRad(161))
        {
            //return 0.45 * diff;
            return 0.4 * diff;
        }
        else if (std::fabs(diff) < degToRad(30) || std::fabs(diff) > degToRad(150))
        {
            //return 0.28 * diff;
            return 0.28 * diff;
        }
        else
        {
            return 0.19 * diff;
        }
    }
}

Transform Transform::glPerspective(Float fov, Float clipNear, Float clipFar) {
	Float recip = 1.0f / (clipNear - clipFar);
	Float cot = 1.0f / std::tan(degToRad(fov / 2.0f));

	Matrix4x4 trafo(
		cot,   0,     0,   0,
		0,     cot,   0,   0,
		0,     0,     (clipFar + clipNear) * recip,  2 * clipFar * clipNear * recip,
		0,     0,     -1,   0
	);

	return Transform(trafo);
}

	/**
	 * Configures the position of the sun. This calculation is based on
	 * your position on the world and time of day.
	 * From IES Lighting Handbook pg 361.
	 */
	void configureSunPosition(const Float lat, const Float lon,
			const int stdMrd, const int julDay, const Float timeOfDay) {
		const Float solarTime = timeOfDay
			+ (0.170 * sin(4.0 * M_PI * (julDay - 80.0) / 373.0)
			- 0.129 * sin(2.0 * M_PI * (julDay - 8.0) / 355.0))
			+ (stdMrd - lon) / 15.0;

		const Float solarDeclination = (0.4093 * sin(2 * M_PI * (julDay - 81) / 368));

		const Float solarAltitude = asin(sin(degToRad(lat))
			* sin(solarDeclination) - cos(degToRad(lat))
			* cos(solarDeclination) * cos(M_PI * solarTime / 12.0));

		const Float opp = -cos(solarDeclination) * sin(M_PI * solarTime / 12.0);
		const Float adj = -(cos(degToRad(lat)) * sin(solarDeclination)
			+ sin(degToRad(lat)) * cos(solarDeclination)
			* cos(M_PI * solarTime / 12.0));

		const Float solarAzimuth = atan2(opp,adj);

		m_phiS = -solarAzimuth;
		m_thetaS = M_PI / 2.0 - solarAltitude;
	}

void Polar::bvmgWind(double twaOrtho, double tws, double *twaVMG)
{
    double twa;
    myBvmgWind(degToRad(twaOrtho),tws,&twa);
    twa = fmod(twa, TWO_PI);
    if (twa > PI)
    {
      twa -= TWO_PI;
    } else if (twa < -PI)
    {
      twa += TWO_PI;
    }
    *twaVMG=radToDeg(twa);
}

void OpenGLTools::gluPerspective(GLdouble fovy, GLdouble aspect, GLdouble zNear, GLdouble zFar)
{
    double fovyRad = degToRad(fovy);
    double f = 1.0 / tan(fovyRad / 2);
    double zDepth = zNear - zFar;
    Matrix44 matrix (
        f / aspect, 0 ,             0          ,            0            ,
        0         , f ,             0          ,            0            ,
        0         , 0 , (zFar + zNear) / zDepth, 2 *zFar * zNear / zDepth,
        0         , 0 ,            -1          ,            0
    );

    glMultMatrixMatrix44(matrix);
}

	static quat eulerAnglesToQuaternion(float pitch, float yaw, float roll) {

		float y0 = (float) degToRad(yaw / 2.0f);
		float p0 = (float) degToRad(pitch / 2.0f);
		float r0 = (float) degToRad(roll / 2.0f);

		float cosy = (float) cos(y0);
		float cosp = (float) cos(p0);
		float cosr = (float) cos(r0);

		float siny = (float) sin(y0);
		float sinp = (float) sin(p0);
		float sinr = (float) sin(r0);

		quat q;

		q.x = cosr * sinp * cosy + sinr * cosp * siny;
		q.y = cosr * cosp * siny - sinr * sinp * cosy;
		q.z = sinr * cosp * cosy - cosr * sinp * siny;
		q.w = cosr * cosp * cosy + sinr * sinp * siny;

		return q;
	}