/*
  * The stack must have three values in this order:
  * 1. x coordinate (a number)
  * 2. y coordinate (a number)
  * 3. z coordinate (a number)
  */
 int LuaSetRelativePosition(lua_State* pt_lua_state) {
    /* Check parameters */
    if(lua_gettop(pt_lua_state) != 3) {
       return luaL_error(pt_lua_state, "robot.quadrotor.set_relative_position() expects 3 arguments");
    }
    luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
    luaL_checktype(pt_lua_state, 2, LUA_TNUMBER);
    luaL_checktype(pt_lua_state, 3, LUA_TNUMBER);
    /* Perform action */
    CLuaUtility::GetDeviceInstance<CCI_QuadRotorPositionActuator>(pt_lua_state, "quadrotor")->
       SetRelativePosition(CVector3(lua_tonumber(pt_lua_state, 1),
                                    lua_tonumber(pt_lua_state, 2),
                                    lua_tonumber(pt_lua_state, 3)));
    return 0;
 }

 bool CRABEquippedEntityGridEntityUpdater::operator()(CRABEquippedEntity& c_entity) {
    try {
       m_cCellUpdater.SetEntity(c_entity);
       m_cGrid.ForCellsInBoxRange(c_entity.GetPosition(),
                                  CVector3(c_entity.GetRange(),
                                           c_entity.GetRange(),
                                           c_entity.GetRange()),
                                  m_cCellUpdater);
       /* Continue with the other entities */
       return true;
    }
    catch(CARGoSException& ex) {
       THROW_ARGOSEXCEPTION_NESTED("While updating the RAB entity grid for RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex);
    }
 }

//-------------------------------------------------------------
//- CalcNorms
//- Calculate the vertex and face normals
//-------------------------------------------------------------
void CalcNorms()
{
	//Calculate face normals first
	for(int x = 0; x < 10; x++)
	{
		CVector3 vTmp = CalcFaceNormal(CVector3(verts[Tris[x][2]]), CVector3(verts[Tris[x][1]]), CVector3(verts[Tris[x][0]]));
		memcpy(facenormals[x], vTmp.Get(), 12);
	}
	//Calculate the vertex normals
	for(x = 0; x < 10; x++)
	{
		int iShared[10];  //Indices of shared faces
		int iNumShared = 0;   //Number of faces that share vertex
		
		//first find out which faces share the vertex
		for(int y = 0; y < 10; y++)
		{
			for(int z = 0; z < 3; z++)
			{
				if(Tris[y][z] == x)
				{
					iShared[iNumShared] = y;
					iNumShared ++;
				}
			}
		}
		//Calculate a normal by averaging the face normals of the shared faces
		CVector3 finalNorm;
		for(y = 0; y < iNumShared; y++)
		{
			finalNorm += facenormals[iShared[y]];
		}
		finalNorm /= (float)iNumShared;
		memcpy(&vertnormals[x], finalNorm.Get(), 12); 
	}
}

// draw function for adding graphics to a foot-bot
void iAnt_qt_user_functions::DrawFood(CFootBotEntity& entity) {
    iAnt_controller& c = dynamic_cast<iAnt_controller&>
                         (entity.GetControllableEntity().GetController());

    UpdateDrawInWorldData(c);

    if(c.IsHoldingFood() == true) {
//#ifdef __APPLE__
        //Edit here for drawing trails
        DrawCylinder(CVector3(0.0f, 0.0f, 0.3f), CQuaternion(), 0.05f, 0.025f, CColor::BLACK);
//#else
//        DrawCylinder(0.05f, 0.025f, CVector3(0.0f, 0.0f, 0.3f), CColor::BLACK);
//#endif
    }
}

void CEyeBotCircle::TakeOff() {
   if(m_eState != STATE_TAKE_OFF) {
      /* State initialization */
      m_eState = STATE_TAKE_OFF;
      m_cCircleCenter = m_pcPosSens->GetReading().Position + CVector3(0.0f, 0.0f, ALTITUDE);
      m_cTargetPos = m_cCircleCenter;
      m_pcPosAct->SetAbsolutePosition(m_cTargetPos);
   }
   else {
      if(Distance(m_cTargetPos, m_pcPosSens->GetReading().Position) < PROXIMITY_TOLERANCE) {
         /* State transition */
         LeaveCircleCenter();
      }
   }
}

_MEMBER_FUNCTION_IMPL(Audio, setPosition)
{
	CAudio * pAudio = script_getinstance<CAudio *>(pVM);

	float fX, fY, fZ, fR;

	script_getfloat ( pVM, 1, &fX );
	script_getfloat ( pVM, 2, &fY );
	script_getfloat ( pVM, 3, &fZ );
	script_getfloat ( pVM, 4, &fR );

	pAudio->SetPosition (CVector3(fX, fY, fZ), fR );

	script_pushbool(pVM, true);
	return 1;
}

// getVehicleAngularVelocity(vehicleid)
CVector3 CVehicleModuleNatives::GetAngularVelocity(EntityId vehicleid)
{
    if(g_pVehicleManager->DoesExist(vehicleid))
    {
        CVehicle * pVehicle = g_pVehicleManager->GetAt(vehicleid);

        if(pVehicle)
        {
            CVector3 vecTurnSpeed;
            pVehicle->GetTurnSpeed(vecTurnSpeed);
            return vecTurnSpeed;
        }
    }

    return CVector3();
}

CNetworkEntity::CNetworkEntity(eEntityType eType)
: m_vecPosition(CVector3()),
m_vecRotation(CVector3()),
m_vecMoveSpeed(CVector3()),
m_vecTurnSpeed(CVector3()),
m_vecDirection(CVector3()),
m_vecRoll(CVector3()),
m_entityId(INVALID_ENTITY),
m_eType(eType)
{
}

   CLightEntity& CLoopFunctions::AddLight(const std::string& str_id,
                                          const CVector3& c_position,
                                          Real f_intensity,
                                          const CColor& c_color) {

      /* Build the XML tree */
      TConfigurationNode tRootNode("light");
      SetNodeAttribute(tRootNode, "id", str_id);
      SetNodeAttribute(tRootNode, "position", c_position);
      SetNodeAttribute(tRootNode, "orientation", CVector3());
      SetNodeAttribute(tRootNode, "intensity", f_intensity);
      SetNodeAttribute(tRootNode, "color", c_color);
      /* Add the entity to the space */
      CLightEntity& cEntity = AddEntityToSpace<CLightEntity>(tRootNode);
      /* Return the entity */
      return cEntity;
   }

CVector3 CSplineInterpolatorVector3::intepolate(S32 index, F32 t){
        if ((index + 1) == nodes.size()){
            return nodes[index];
        }

        if (t == 0.0f){
            return nodes[index];
        }else if(t == 1.0f){
            return nodes[index + 1];
        }

        F32 t2, t3;
        t2 = t * t;
        t3 = t2 * t;
        CVector4 powers(t3, t2, t, 1);

        // ret = powers * mCoeffs * (point1, point2, tangent1, tangent2)
        CVector3& point1 = nodes[index];
        CVector3& point2 = nodes[index+1];
        CVector3& tan1 = mTangents[index];
        CVector3& tan2 = mTangents[index+1];
        CMatrix4 mpt;
        FMatrix4 &pt = mpt.m;

        pt[0][0] = point1.x;
        pt[0][1] = point1.y;
        pt[0][2] = point1.z;
        pt[0][3] = 1.0f;
        pt[1][0] = point2.x;
        pt[1][1] = point2.y;
        pt[1][2] = point2.z;
        pt[1][3] = 1.0f;
        pt[2][0] = tan1.x;
        pt[2][1] = tan1.y;
        pt[2][2] = tan1.z;
        pt[2][3] = 1.0f;
        pt[3][0] = tan2.x;
        pt[3][1] = tan2.y;
        pt[3][2] = tan2.z;
        pt[3][3] = 1.0f;

        CVector4 ret = mCoeffs * mpt * powers;
       
        return CVector3(ret.x, ret.y, ret.z);
}

void CWorldEditor::CreateBaseAxis()
{
	auto sceneRoot = m_overlayViewport->GetSceneRoot();

	static const CVector3 g_arrowScale(0.075f, 0.25f, 0.075f);
	
	{
		auto baseAxisNode = Palleon::CSceneNode::Create();
		baseAxisNode->SetPosition(CVector3(289.2f, 5.00f, -563.f));
		sceneRoot->AppendChild(baseAxisNode);

		{
			auto axisMesh = Palleon::CAxisMesh::Create();
			axisMesh->SetScale(CVector3(1, 1, 1));
			baseAxisNode->AppendChild(axisMesh);
		}

		//X arrow
		{
			auto coneMesh = Palleon::CConeMesh::Create();
			coneMesh->SetPosition(CVector3(1, 0, 0));
			coneMesh->SetRotation(CQuaternion(CVector3(0, 0, 1), M_PI / 2.f));
			coneMesh->SetScale(g_arrowScale);
			coneMesh->GetMaterial()->SetColor(CColor(1, 0, 0, 1));
			baseAxisNode->AppendChild(coneMesh);
		}

		//Y arrow
		{
			auto coneMesh = Palleon::CConeMesh::Create();
			coneMesh->SetPosition(CVector3(0, 1, 0));
			coneMesh->SetScale(g_arrowScale);
			coneMesh->GetMaterial()->SetColor(CColor(0, 1, 0, 1));
			baseAxisNode->AppendChild(coneMesh);
		}

		//Z arrow
		{
			auto coneMesh = Palleon::CConeMesh::Create();
			coneMesh->SetPosition(CVector3(0, 0, 1));
			coneMesh->SetRotation(CQuaternion(CVector3(1, 0, 0), -M_PI / 2.f));
			coneMesh->SetScale(g_arrowScale);
			coneMesh->GetMaterial()->SetColor(CColor(0, 0, 1, 1));
			baseAxisNode->AppendChild(coneMesh);
		}
	}
}

// Ammo constructor intialises ammo-specific data and passes its parameters to the base
// class constructor
CAmmoEntity::CAmmoEntity
(

	CEntityTemplate* entityTemplate,
	TEntityUID       UID,
	const TInt32&	 refillSize,
	const string&    name	/*= ""*/,
	const CVector3&  position /*= CVector3::kOrigin*/,
	const CVector3&  rotation /*= CVector3(0.0f, 0.0f, 0.0f)*/,
	const CVector3&  scale /*= CVector3(1.0f, 1.0f, 1.0f)*/
) : CEntity( entityTemplate, UID, name, position + CVector3(0.0f, 100.0f, 0.0f), rotation, scale )
{
	m_RefillSize = refillSize;
	m_Height = position.y;
	m_FallSpeed = 20.0f;
	landed = false;
	m_SinWave = 0.0f;
}

void CIVTrain::CreateWagons(BYTE iWagons)
{
	// Check if we're a wagon
	if(m_eTrainType == TYPE_WAGON)
		return;

	int iWagonCount = 0;
	while(iWagonCount < iWagons)
	{
		CVehicleEntity * pVehicle = new CVehicleEntity(127, CVector3(), 0.0f, 0, 0, 0, 0);
		m_pWagonEntity[iWagonCount] = new CIVTrain;
		m_pWagonEntity[iWagonCount]->m_eTrainType = TYPE_WAGON;
		m_pWagonEntity[iWagonCount]->m_pVehicle = pVehicle;
		m_pWagonEntity[iWagonCount]->m_pVehicle->SetId(g_pCore->GetGame()->GetVehicleManager()->FindFreeSlot());
		m_pWagonEntity[iWagonCount]->m_pVehicle->Create();
		g_pCore->GetGame()->GetVehicleManager()->Add(m_pWagonEntity[iWagonCount]->m_pVehicle);
		iWagonCount++;
	}
}

CVector3 CQuake3BSP::TraceBox(CVector3 vStart, CVector3 vEnd, CVector3 vMin, CVector3 vMax)
{
    m_traceType = TYPE_BOX;			// Set the trace type to a BOX
    m_vTraceMaxs = vMax;			// Set the max value of our AABB
    m_vTraceMins = vMin;			// Set the min value of our AABB
    m_bCollided = false;			// Reset the collised flag


/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *

    // Here we initialize our variables for a new round of collision checks
    m_bTryStep = false;
    m_bGrounded = false;

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *


    // Grab the extend of our box (the largest size for each x, y, z axis)
    m_vExtents = CVector3(-m_vTraceMins.x > m_vTraceMaxs.x ? -m_vTraceMins.x : m_vTraceMaxs.x,
                          -m_vTraceMins.y > m_vTraceMaxs.y ? -m_vTraceMins.y : m_vTraceMaxs.y,
                          -m_vTraceMins.z > m_vTraceMaxs.z ? -m_vTraceMins.z : m_vTraceMaxs.z);


    // Check if our movement collided with anything, then get back our new position
    CVector3 vNewPosition = Trace(vStart, vEnd);


/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *

    // Let's check to see if we collided with something and we should try to step up
    if(m_bCollided && m_bTryStep)
    {
        // Try and step up what we collided with
        vNewPosition = TryToStep(vNewPosition, vEnd);
    }

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *


    // Return our new position
    return vNewPosition;
}

void COctree::InitOctree()
{
    // Set the subdivided flag to false
    m_bSubDivided = false;

    // Set the dimensions of the box to false
    m_Width = 0;

    // Initialize the triangle count
    m_TriangleCount = 0;

    // Initialize the center of the box to the 0
    m_vCenter = CVector3(0, 0, 0);

    // Set the triangle list to NULL
    m_pVertices = NULL;

    // Set the sub nodes to NULL
    memset(m_pOctreeNodes, 0, sizeof(m_pOctreeNodes));
}

 CEmbodiedEntity::CEmbodiedEntity(CComposableEntity* pc_parent,
                                  const std::string& str_id,
                                  const CVector3& c_position,
                                  const CQuaternion& c_orientation,
                                  bool b_movable) :
    CEntity(pc_parent, str_id),
    m_bMovable(b_movable),
    m_sBoundingBox(NULL),
    m_psOriginAnchor(new SAnchor("origin",
                                 0,
                                 CVector3(),
                                 CQuaternion(),
                                 c_position,
                                 c_orientation)),
    m_cInitOriginPosition(c_position),
    m_cInitOriginOrientation(c_orientation) {
    /* Add anchor to map and enable it */
    m_mapAnchors[m_psOriginAnchor->Id] = m_psOriginAnchor;
    EnableAnchor("origin");
 }

bool CEntity::CheckSphereAgainstOOBB( const CVector3& Point, const float& Radius )
{
	float distX, distZ;

	// First calculate the distance from the X axis
	CVector3 v( Matrix().Position(), Point );
    distX = abs( Dot( v, Normalise( Matrix().XAxis() ) ) );

	// Then calculate the distance from the Z axis
	v  = CVector3( Matrix().Position(), Point );
    distZ = abs( Dot( v, Normalise( Matrix().ZAxis() ) ) );

	if( distX <= ( m_XAxisRadius + Radius ) && distZ <= ( m_ZAxisRadius + Radius ) )
	{
		return true;
	}
	else
	{
		return false;
	}
}

/*****
 * Initialize the controller via the XML configuration file. ARGoS typically
 * wants objects & variables initialized here instead of in the constructor(s).
 *****/
void iAnt_controller::Init(TConfigurationNode& node) {
    /* Shorter names, please. #This_Is_Not_Java */
    typedef CCI_PositioningSensor            CCI_PS;
    typedef CCI_DifferentialSteeringActuator CCI_DSA;
    typedef CCI_FootBotProximitySensor       CCI_FBPS;

    /* Initialize the robot's actuator and sensor objects. */
    motorActuator   = GetActuator<CCI_DSA>("differential_steering");
    compass         = GetSensor<CCI_PS>   ("positioning");
    proximitySensor = GetSensor<CCI_FBPS> ("footbot_proximity");

    TConfigurationNode iAnt_params = GetNode(node, "iAnt_params");
    GetNodeAttribute(iAnt_params, "RobotForwardSpeed", RobotForwardSpeed);
    GetNodeAttribute(iAnt_params, "RobotTurningSpeed", RobotTurningSpeed);
    GetNodeAttribute(iAnt_params, "AngleToleranceInDegrees", angleInDegrees);

    AngleToleranceInRadians.Set(-ToRadians(angleInDegrees),ToRadians(angleInDegrees));

    stepSize = 0.1; /* Assigns the robot's stepSize */
    startPosition = CVector3(0.0, 0.0, 0.0);
}

//--------------------------------------------------------------------------------------------------------------------
void RDX11RenderHelper::RenderBox(XMMATRIX& mtWorld, CVector3& min, CVector3& max, DWORD color)
{
	SetWorldTM(mtWorld);

	BOX_MAKE_PARAM param;
	param.min = min;
	param.max = max;
	param.offset = CVector3(0, 0, 0);

	CVertexPC* pVertices = NULL;
	CGEOMETRY_CONSTRUCTOR::CreateBoxLine( param, &pVertices);

	for (int i =0 ; i< 24; i++)
	{
		pVertices[i].color = color;
		m_LineVertices.Add(pVertices[i]);
	}
	SAFE_DELETE_ARRAY(pVertices);

	GLOBAL::ShaderMgr()->Begin(SHADER_COLOR_VS, SHADER_COLOR_PS);
	GLOBAL::RenderStateMgr()->SetDepthStancil(DEPTH_ON_STENCIL_OFF);
	DrawLine();
}

// setup any theme related resources
void ThemeDesert::SetupTheme(void)
{
	// Desert Theme Specific Settings
	m_sThemeName = "Desert";

	// still let the base class do default setup
	ThemeModel::SetupTheme();

	//
	m_vFloorColor = CVector3(183, 140, 105);

	
	// setup sounds
	//////////////////////////////////////////////////////////////////////////
//	SokobanSoundManager &SoundCache = SokobanSoundManager::Instance();

/*	string SoundStepFileName = GetFullAssetDirectory();
	SoundStepFileName += "audio\\move_forward.au";*/

//	SoundCache.LoadSampleFromFile(MOVEEVENT_STEP, ".\\audio\\sample.wav");


}