/*
===================================================

Creates an arc shaped polygon. First it creates vertices
for inner side of the arc and the for the outer side.
After creating of an vertex, it's put in to a polygon

===================================================
 */
QPolygonF Sector::createPolygonArc(double radius, double arcWidth,
								   double startAngle, double endAngle) const {
	QPolygonF arc;
	QPointF point;

	double spanAngle = 0.0;
	if(endAngle >= startAngle) {
		spanAngle = endAngle - startAngle;
	} else {
		spanAngle = (endAngle + 360) - startAngle;
	}

	// inner edge of the polygon
	for(double angle = startAngle; angle <= (startAngle + spanAngle); ++angle) {
		point.setX(radius * cos(degreesToRadians(angle)));
		point.setY(radius * sin(degreesToRadians(angle)));
		arc << point;
	}

	// outer edge
	double angle = 0.0;
	if(startAngle + spanAngle > 360) {
		angle = 360.0f + endAngle;
	} else {
		angle = endAngle;
	}

	for( ; angle >= startAngle; --angle) {
		point.setX((arcWidth + radius) * cos(degreesToRadians(angle)));
		point.setY((arcWidth + radius) * sin(degreesToRadians(angle)));
		arc << point;
	}

	return arc;
}

		void testDecomposeRQ3x3()
		{
			try
			{
				srand(1);
				
				// This test produces an incorrect result when the validation check in givensDecomposeRQ3x3 isn't used. 		
				decomposeTest(
					( Eigen::Vector3d() << degreesToRadians( -90. ), degreesToRadians( 24.5916 ), degreesToRadians( -90. ) ).finished(),
					( Eigen::Matrix3d() << 1.94444, 0, 0, 0, 2.1875, 0, 0.0141111, 0.127, 1. ).finished()
				);
				
				for( unsigned int i = 0; i < 1000; ++i )
				{
					decomposeTest(
						( Eigen::Vector3d() << randomNumber( -M_PI*.5, M_PI*.5 ), randomNumber( -M_PI*.5, M_PI*.5 ), randomNumber( -M_PI*.5, M_PI*.5 ) ).finished(),
						( Eigen::Matrix3d() << randomNumber( 0.1, 3. ), 0, 0, 0, randomNumber( 0.1, 3. ), 0, randomNumber( 0.1, 3. ), randomNumber( 0.1, 3. ), 1. ).finished()
					);
				}
			}
			catch ( std::exception &e ) 
			{
				BOOST_WARN( !e.what() );
				BOOST_CHECK( !"Exception thrown during DecomposeRQ3x3Test." );
			}
		}

/**
 * Detects rotation at one edge of the area specified by left, top, right, bottom.
 * Which of the four edges to take depends on whether shiftX or shiftY is non-zero,
 * and what sign this shifting value has.
 */
static double detectEdgeRotation(float deskewScanRange, float deskewScanStep, int deskewScanSize, float deskewScanDepth, int shiftX, int shiftY, int left, int top, int right, int bottom, struct IMAGE* image) {
    // either shiftX or shiftY is 0, the other value is -i|+i
    // depending on shiftX/shiftY the start edge for shifting is determined
    double rangeRad;
    double stepRad;
    double rotation;
    int peak;
    int maxPeak;
    double detectedRotation;
    double m;

    rangeRad = degreesToRadians((double)deskewScanRange);
    stepRad = degreesToRadians((double)deskewScanStep);
    detectedRotation = 0.0;
    maxPeak = 0;    
    // iteratively increase test angle,  alterating between +/- sign while increasing absolute value
    for (rotation = 0.0; rotation <= rangeRad; rotation = (rotation>=0.0) ? -(rotation + stepRad) : -rotation ) {    
        m = tan(rotation);
        peak = detectEdgeRotationPeak(m, deskewScanSize, deskewScanDepth, shiftX, shiftY, left, top, right, bottom, image);
        if (peak > maxPeak) {
            detectedRotation = rotation;
            maxPeak = peak;
        }
    }
    return radiansToDegrees(detectedRotation);
}

//----------------------------------------------------------------------------------------------------------------------    
// Environment  
//----------------------------------------------------------------------------------------------------------------------  
void SMCEnvironment::fromXml(const ci::XmlTree& xml)
{
  for(ci::XmlTree::ConstIter elem = xml.begin(); elem != xml.end(); ++elem)
  {
    const ci::XmlTree& obj = *elem;
    ci::Vec2f pos = ci::Vec2f(obj["PosX"].as<float>(), obj["PosY"].as<float>());
    
    if(obj.getTag() == "Line")
    {
      Line* l = new Line (pos, degreesToRadians(obj["Angle"].as<float>()), obj["Length"].as<float>());
      m_objects.push_back(l);
    }
    else if (obj.getTag() == "Circle")
    {
      Circle* c = new Circle (pos, obj["Radius"].as<float>());
      m_objects.push_back(c);      
    }
    else if (obj.getTag() == "Torus")
    {
      Torus* t = new Torus (pos, obj["Radius"].as<float>(), obj["Width"].as<float>());
      t->setPeak(obj.getAttributeValue<float>("Peak", 1.0f));
      m_objects.push_back(t);
    }
    else if (obj.getTag() == "Gradient")
    {
      Gradient* c = new Gradient (pos, obj["Radius"].as<float>());
      m_objects.push_back(c);
    }
    else if (obj.getTag() == "Triangle")
    {
      Triangle* t = new Triangle (pos, obj["Size"].as<float>());
      m_objects.push_back(t);      
    }
    else if (obj.getTag() == "Gaussian")
    {
      Gaussian* g = new Gaussian (pos, obj["Width"].as<float>(), obj["Height"].as<float>(), ci::Vec2f(obj["DirX"].as<float>(), obj["DirY"].as<float>()));
      m_objects.push_back(g);      
    }    
    else 
    {
      // Not recognized as supported object
      continue;
    }

    bool visibility = obj.getAttributeValue<bool>("Visible", true);    
    m_objects[m_objects.size() - 1]->setVisibility(visibility);
    
    float posVarX = obj.getAttributeValue<float>("PosVarX", 0.0);
    float posVarY = obj.getAttributeValue<float>("PosVarY", 0.0);
    m_objects[m_objects.size() - 1]->setPositionVariance(ci::Vec2f(posVarX, posVarY));
    
    float angVar = degreesToRadians(obj.getAttributeValue<float>("AngleVar", 0.0));
    m_objects[m_objects.size() - 1]->setAngleVariance(angVar);

  }
}

    Matrix Matrix::createRotationZ(float angle)
    {
        Matrix result = identityMatrix;

        result.elements[0] = cos(degreesToRadians(angle));
        result.elements[4] = -sin(degreesToRadians(angle));
        result.elements[1] = sin(degreesToRadians(angle));
        result.elements[5] = cos(degreesToRadians(angle));

        return result;
    }

void Bullet::fireBullet(GLfloat xPos, GLfloat yPos, GLfloat angle)
{
	m_isActive = true;

	m_collisionBox.xPos = xPos;
	m_collisionBox.yPos = yPos;
	m_collisionBox.height = BULLET_HEIGHT;
	m_collisionBox.width = BULLET_WIDTH;

	m_xVelocity = JCos(degreesToRadians(angle + 90));
	m_yVelocity = JSin(degreesToRadians(angle + 90));
}

/** Returns a rotated copy of the object by a given azimuth and elevation.
 */
CPanelGroup CSailDisp::dispObject() const
{
    const CVector3d center = baseRect.center();

    CMatrix4x4 matrix;
    matrix.translate(center);
    matrix.rotate(degreesToRadians(m_elevation), CVector3d(1, 0, 0));
    matrix.rotate(degreesToRadians(m_azimuth), CVector3d(0, 1, 0));
    matrix.translate(-center);

    return baseObject.transformed(matrix);
}

void FirstPersonCamera::use()
{
	// Obtain the reference point determined by the angles
	float eyeX, eyeY, eyeZ;
	getEyePosition(eyeX, eyeY, eyeZ);
	float centerX = eyeX + cos(degreesToRadians(xzAngle))*cos(degreesToRadians(yAngle));
	float centerY = eyeY + sin(degreesToRadians(yAngle));
	float centerZ = eyeZ + sin(degreesToRadians(xzAngle))*cos(degreesToRadians(yAngle));
	setReferencePoint(centerX, centerY, centerZ);
	
	// Use the camera
	Camera::use();
}

EllipsePath::EllipsePath(Point center, MCFixed width, MCFixed height,
                         double startAngle, double endAngle) {
  startAngle = degreesToRadians(startAngle);
  endAngle = degreesToRadians(endAngle);
  int pointCount = 20;
  double angleDelta = (endAngle - startAngle) / pointCount;
  for (int i = 0; i < pointCount; i++) {
    double angle = startAngle + (angleDelta * i);
    emplace_back(Point(center.X + width * MCFixed(cos(angle)),
                       center.Y - height * MCFixed(sin(angle))));
  }
  emplace_back(Point(center.X + width * MCFixed(cos(endAngle)),
                     center.Y - height * MCFixed(sin(endAngle))));
}

/* Rotating */
void FreeCamera::rotate(float angle)
{
	float eyeX, eyeY, eyeZ;
	float centerX, centerY, centerZ;
	getEyePosition(eyeX, eyeY, eyeZ);
	getReferencePoint(centerX, centerY, centerZ);

	float distance = distance2D(eyeX, eyeZ, centerX, centerZ);
	centerX += distance*(cos(degreesToRadians(xzAngle + angle)) - cos(degreesToRadians(xzAngle)));
	centerZ += distance*(sin(degreesToRadians(xzAngle + angle)) - sin(degreesToRadians(xzAngle)));
	xzAngle += angle;

	setReferencePoint(centerX, centerY, centerZ);
}

Gosu::Transform
Gosu::rotate(double angle, double aroundX, double aroundY)
{
    double c = std::cos(degreesToRadians(angle));
    double s = std::sin(degreesToRadians(angle));
    Gosu::Transform result = {
        +c, +s, 0, 0,
        -s, +c, 0, 0,
        0,  0,  1, 0,
        0,  0,  0, 1
    };
    if (aroundX != 0 || aroundY != 0)
        result = multiply(multiply(translate(-aroundX, -aroundY), result), translate(aroundX, aroundY));
    return result;
}

// Descr: test degrees to radians and radians to degrees
// Implementation details: See gtest/samples for GTest syntax and usage
TEST(GeometryTest, D2RandR2D)
{
    //test converting Degrees to Radians
    // I know this 'should' be EXPECT_DOUBLE_EQ, but even though the values were good to the 5th or 6th decimal place, it was failing.
    // So, there you go.
    EXPECT_FLOAT_EQ(degreesToRadians(1),0.0174532925 );
    EXPECT_FLOAT_EQ(degreesToRadians(254),4.4331363 );
    EXPECT_FLOAT_EQ(degreesToRadians(360),6.283185307 );
    EXPECT_FLOAT_EQ(degreesToRadians(15),0.261799388 );
    EXPECT_FLOAT_EQ(radiansToDegrees(3.14),179.908747671 );
    EXPECT_FLOAT_EQ(radiansToDegrees(0.174532925),10 );
    EXPECT_FLOAT_EQ(radiansToDegrees(1.745329252),100 );
    EXPECT_FLOAT_EQ(radiansToDegrees(.1234567),7.073547863 );
    EXPECT_FLOAT_EQ(radiansToDegrees(6.195918845),355 );
}

void initBoardAlignment(const boardAlignment_t *boardAlignment)
{
    if (isBoardAlignmentStandard(boardAlignment)) {
        return;
    }

    standardBoardAlignment = false;

    fp_angles_t rotationAngles;
    rotationAngles.angles.roll  = degreesToRadians(boardAlignment->rollDegrees );
    rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
    rotationAngles.angles.yaw   = degreesToRadians(boardAlignment->yawDegrees  );

    buildRotationMatrix(&rotationAngles, boardRotation);
}

/**
* CPotatoChainGunWeapon::fireWeapon
* @date Modified May 5, 2006
*/
bool CPotatoChainGunWeapon::fireWeapon()
{
	if (!m_nAmmoLeft)
		return false;

	if (CTimer::getInstance().getTime() - m_fTimer >= 0.15f)
	{
		CObjectManager::ObjectList loPlayers;
		CObjectManager::getInstance().getObjects(OBJ_PLAYER, &loPlayers);
		((CPlayer*)(loPlayers.front()))->m_oStats.addShotsFired();

		// use the player's position as the initial position of the potato
		D3DXVECTOR3 vPotatoPos;
		if (!getPlayer()->getWeaponActorPos(&vPotatoPos))
			vPotatoPos = m_poCharacter->getBV().centerPt;

		vPotatoPos += m_poCharacter->getOrientation() * 5.0f;
		
		vPotatoPos.x += cosf(degreesToRadians(m_nAngles[m_nBarrelIndex]));
		vPotatoPos.z += sinf(degreesToRadians(m_nAngles[m_nBarrelIndex]));
		// loop back to zero if we go over
		++m_nBarrelIndex %= 5;

		vPotatoPos.x *= BARREL_RADIUS;
		vPotatoPos.z *= BARREL_RADIUS;

		CPotato *pPotato = (CPotato*)CObjectManager::getInstance().createObject(OBJ_WEAPONPROJ_POTATO);

		pPotato->m_pMesh = m_nPotatoMesh;
		pPotato->m_pMesh->addRef();
		pPotato->setPosition(vPotatoPos + m_poCharacter->getOrientation() * 5);
		pPotato->m_oBV.centerPt = pPotato->getPosition();
		pPotato->m_oBV.fRadius = 1.0f;
		pPotato->setOrientation(m_poCharacter->getOrientation());
		pPotato->m_vVelocity = pPotato->getOrientation()* 100.0f;
		pPotato->m_nDamage = m_nDamage;
		pPotato->m_nAreaOfEffect = m_nAreaOfEffect;
		pPotato->setPlayer((CPlayer*)m_poCharacter);
		pPotato->activate();

		m_nAmmoLeft--;
		m_fTimer = CTimer::getInstance().getTime();
		// Play sound
		CSoundManager::getInstance().playSound(SND_EFFECT_POTATOFIRE);
		return true;
	}
	return false;
}

void Player::moveForward(const float speed)
{
    //Vector3 pos = getPosition();
    const float speed_multiplier = 40;

    float cosYaw = cosf(degreesToRadians(m_yaw));
    float sinYaw = sinf(degreesToRadians(m_yaw));
    //pos.x += float(cosYaw)*speed;
    //pos.z += float(sinYaw)*speed;

    btVector3 linearVelocity = getCollider()->getBody()->getLinearVelocity();

    getCollider()->getBody()->setLinearVelocity(btVector3(btScalar(float(cosYaw)*speed*speed_multiplier), linearVelocity.getY(), btScalar(float(sinYaw)*speed*speed_multiplier)));

    //setPosition(pos);
}

mat4* mat4PerspectiveProjection(mat4* pOut, float fovY,
                                float aspect, float zNear,
                                float zFar) {
    float r = degreesToRadians(fovY / 2);
    float deltaZ = zFar - zNear;
    float s = sinf(r);
    float cotangent = 0;

    if (deltaZ == 0 || s == 0 || aspect == 0) {
        return NULL;
    }

    /*cos(r) / sin(r) = cot(r)*/
    cotangent = cosf(r) / s;

    amat4_identity(pOut);
    pOut->mat[0] = cotangent / aspect;
    pOut->mat[5] = cotangent;
    pOut->mat[10] = -(zFar + zNear) / deltaZ;
    pOut->mat[11] = -1;
    pOut->mat[14] = -2 * zNear * zFar / deltaZ;
    pOut->mat[15] = 0;

    return pOut;
}

Quaternion::Quaternion(float degrees, const Vector3 &axis)
{
	float halfRadians = degreesToRadians(degrees / 2.0f);

	w = std::cos(halfRadians);
	v = axis * std::sin(halfRadians);
}

//
// Behavior moveArcBehavior(target)
// Last modified: 07Nov2009
//
// Moves the robot using the parameterized movement vector,
// returning the appropriate robot behavior.
//
// Returns:     the appropriate robot behavior
// Parameters:
//      target  in/out  the target move of the behavior
//
Behavior Robot::moveArcBehavior(const Vector &target)
{
    GLfloat theta    = target.angle();
	  GLfloat phi      = this->heading.angle();
	  GLfloat delta    = degreesToRadians(theta);
    GLfloat cosDelta = cos(delta);
    GLfloat sinDelta = sin(delta);
	  GLfloat t        = cosDelta * cosDelta * sign(cosDelta);
	  GLfloat r        = sinDelta * sinDelta * sign(sinDelta);
	  behavior         = Behavior(ACTIVE, t, r, maxSpeed());
    if (abs(theta) < 90.0f)
	      behavior.setDiffVel(maxSpeed() * (t + r), maxSpeed() * (t - r));
    else
        behavior.setDiffVel(maxSpeed() * (t - r), maxSpeed() * (t + r));
	  return behavior;
/*
    GLfloat r     = target.magnitude();
    if (r <= threshold()) return moveStop();
    GLfloat theta = degreesToRadians(target.angle());
    if (theta == 0.0f)    return moveForwardBehavior(r);
    else return moveArcBehavior((abs(theta) >
                                degreesToRadians(angThreshold())) ?
                                0.0f :
                                r * theta / sin(theta), getDiameter() * theta);
*/
}   // moveArcBehavior(const Vector &)

Quaternion Quaternion::Slerp(const Quaternion &q1, const Quaternion &q2,
                        float t)
{
    float theta = degreesToRadians(Angle(q1, q2));
    
    return (q1 * sinf((1 - t) * theta) + q2 * sinf(t * theta)) / sinf(theta);
}

void annexCode(void)
{

    int32_t throttleValue;

    // Compute ROLL PITCH and YAW command
    rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
    rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
    rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband);

    //Compute THROTTLE command
    throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
    throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
    rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);

    if (FLIGHT_MODE(HEADFREE_MODE)) {
        const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
        const float cosDiff = cos_approx(radDiff);
        const float sinDiff = sin_approx(radDiff);
        const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
        rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
        rcCommand[PITCH] = rcCommand_PITCH;
    }

    if (ARMING_FLAG(ARMED)) {
        LED0_ON;
    } else {
        if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
            ENABLE_ARMING_FLAG(OK_TO_ARM);
        }

        if (!STATE(SMALL_ANGLE)) {
            DISABLE_ARMING_FLAG(OK_TO_ARM);
        }

        if (isCalibrating() || isSystemOverloaded()) {
            warningLedFlash();
            DISABLE_ARMING_FLAG(OK_TO_ARM);
        }

#if defined(NAV)
        if (naivationBlockArming()) {
            DISABLE_ARMING_FLAG(OK_TO_ARM);
        }
#endif

        if (ARMING_FLAG(OK_TO_ARM)) {
            warningLedDisable();
        } else {
            warningLedFlash();
        }

        warningLedUpdate();
    }

    // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
    if (gyro.temperature)
        gyro.temperature(&telemTemperature1);
}