kinematicConstraint::kinematicConstraint(const std::string& _name, int id, tjoint jType, vector3d location, 
										 int ip, vector3d fi, vector3d gi,
										 int jp, vector3d fj, vector3d gj)
	: i(0)
	, j(0)
	, lm(NULL)
	, reactionForce(NULL)
	, sRow(0)
	, iCol(0)
	, jCol(0)
	, nConst(0)
	, max_nnz(0)
	, principal_axis(0)
{
	type = jType;
	name = _name;
	//iName = ip;
	//jName = jp;
	i = ip;
	j = jp;
	ID = id;
	axis = 0.0;

	sp_i = 0.0;
	sp_j = 0.0;
	f_i = fi;
	f_j = fj;
	g_i = gi;
	g_j = gj;
	h_i = fi.cross(gi);
	h_j = fj.cross(gj);
	loc = location;

	//setCoordinates();
}

// returns direction in ship's frame from this ship to target lead position
vector3d Ship::AIGetLeadDir(const Body *target, const vector3d& targaccel, int gunindex)
{
	assert(target);
	if (m_equipment.Get(Equip::SLOT_LASER) == Equip::NONE)
		return target->GetPositionRelTo(this).Normalized();

	const vector3d targpos = target->GetPositionRelTo(this);
	const vector3d targvel = target->GetVelocityRelTo(this);
	// todo: should adjust targpos for gunmount offset

	const int laser = Equip::types[m_equipment.Get(Equip::SLOT_LASER, gunindex)].tableIndex;
	const double projspeed = Equip::lasers[laser].speed;

	vector3d leadpos;
	// avoid a divide-by-zero floating point exception (very nearly zero is ok)
	if( !is_zero_exact(projspeed) ) {
		// first attempt
		double projtime = targpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;

		// second pass
		projtime = leadpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;
	} else {
		// default
		leadpos = targpos;
	}

	return leadpos.Normalized();
}

// returns direction in ship's frame from this ship to target lead position
vector3d Ship::AIGetLeadDir(const Body *target, const vector3d& targaccel, int gunindex)
{
	assert(target);
	if (ScopedTable(m_equipSet).CallMethod<int>("OccupiedSpace", "laser_front") == 0)
		return target->GetPositionRelTo(this).Normalized();

	const vector3d targpos = target->GetPositionRelTo(this);
	const vector3d targvel = target->GetVelocityRelTo(this);
	// todo: should adjust targpos for gunmount offset

	double projspeed = 0;
	Properties().Get(gunindex?"laser_rear_speed":"laser_front_speed", projspeed);

	vector3d leadpos;
	// avoid a divide-by-zero floating point exception (very nearly zero is ok)
	if( !is_zero_exact(projspeed) ) {
		// first attempt
		double projtime = targpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;

		// second pass
		projtime = leadpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;
	} else {
		// default
		leadpos = targpos;
	}

	return leadpos.Normalized();
}

//member function to initialize a state with nondimensional values
void primVars::NondimensionalInitialize(const idealGas &eos, const vector3d<double> &vel){
  data[0] = 1.0;
  data[4] = 1.0/eos.Gamma();
  data[1] = vel.X() / eos.GetSoS(data[4], data[0]);
  data[2] = vel.Y() / eos.GetSoS(data[4], data[0]);
  data[3] = vel.Z() / eos.GetSoS(data[4], data[0]);
}

vector3d modeler::rotate_axis(vector3d &size, float degrees, int axis) {
	float angle = PI * degrees / 180.0;
	float cos_angle = cos(angle);
	float sin_angle = sin(angle);
	float posX = size.getX();
	float posY = size.getY();
	float posZ = size.getZ();
	float x, y, z;
	switch (axis) {
	case 0: 
		x = cos_angle * posX - sin_angle * posY;
		y = sin_angle * posX + cos_angle * posY;
		z = posZ;
		break;
	case 1: 
		x = cos_angle * posX + sin_angle * posZ;
		y = posY;
		z = -sin_angle * posX + cos_angle * posZ;
		break;
	case 2: 
		x = posX;
		y = cos_angle * posY - sin_angle * posZ;
		z = sin_angle * posY + cos_angle * posZ;
		break;
	default:
		break;
	}
	return vector3d(x, y, z);
}

	void UpdateVBOs() {
		PROFILE_SCOPED()
		//create buffer and upload data
		Graphics::VertexBufferDesc vbd;
		vbd.attrib[0].semantic = Graphics::ATTRIB_POSITION;
		vbd.attrib[0].format   = Graphics::ATTRIB_FORMAT_FLOAT3;
		vbd.attrib[1].semantic = Graphics::ATTRIB_NORMAL;
		vbd.attrib[1].format   = Graphics::ATTRIB_FORMAT_FLOAT3;
		vbd.numVertices = ctx->NUMVERTICES();
		vbd.usage = Graphics::BUFFER_USAGE_STATIC;
		m_vertexBuffer.reset(Pi::renderer->CreateVertexBuffer(vbd));

		GasPatchContext::VBOVertex* vtxPtr = m_vertexBuffer->Map<GasPatchContext::VBOVertex>(Graphics::BUFFER_MAP_WRITE);
		assert(m_vertexBuffer->GetDesc().stride == sizeof(GasPatchContext::VBOVertex));
		
		const Sint32 edgeLen = ctx->edgeLen;
		const double frac = ctx->frac;
		for (Sint32 y=0; y<edgeLen; y++) {
			for (Sint32 x=0; x<edgeLen; x++) {
				const vector3d p = GetSpherePoint(x*frac, y*frac);
				const vector3d pSubCentroid = p - clipCentroid;
				clipRadius = std::max(clipRadius, p.Length());
				vtxPtr->pos = vector3f(pSubCentroid);
				vtxPtr->norm = vector3f(p);

				++vtxPtr; // next vertex
			}
		}
		m_vertexBuffer->Unmap();
	}

vector3d TerrainColorFractal<TerrainColorAsteroid>::GetColor(const vector3d &p, double height, const vector3d &norm) const
{
	double n = m_invMaxHeight*height/2;

	if (n <= 0.02) {
		const double flatness = pow(p.Dot(norm), 6.0);
		const vector3d color_cliffs = m_rockColor[1];

		double equatorial_desert = (2.0)*(-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(2.0)*(1.0-p.y*p.y);

		vector3d col;
		col = interpolate_color(equatorial_desert, m_rockColor[0], m_greyrockColor[3]);
		col = interpolate_color(n, col, vector3d(1.5,1.35,1.3));
		col = interpolate_color(flatness, color_cliffs, col);
		return col;
	} else {
		const double flatness = pow(p.Dot(norm), 6.0);
		const vector3d color_cliffs = m_greyrockColor[1];

		double equatorial_desert = (2.0)*(-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(2.0)*(1.0-p.y*p.y);

		vector3d col;
		col = interpolate_color(equatorial_desert, m_greyrockColor[0], m_greyrockColor[2]);
		col = interpolate_color(n, col, m_rockColor[3]);
		col = interpolate_color(flatness, color_cliffs, col);
		return col;
	}
}

vector3f toFloat(vector3d & pts) {
	vector3f opts(pts.size());
	for (int i=0; i<pts.size();i++) {
		Vector3f vec((float) pts[i].x(), (float) pts[i].y(), (float) pts[i].z());
		opts.push_back(vec);
	}
	return opts;
}

static int GetFlipMode(Ship *ship, const vector3d &relpos, const vector3d &relvel)
{
	double dist = relpos.Length();
	double vel = relvel.Dot(relpos) / dist;
	if (vel > 0.0 && vel*vel > 1.7 * ship->GetAccelRev() * dist) return 2;
	if (dist > 100000000.0) return 1;	// arbitrary
	return 0;
}

vector3d TransferPlanner::GetOffsetVel() const {
	if(m_position.ExactlyEqual(vector3d(0., 0., 0.)))
		return vector3d(0., 0., 0.);

	const vector3d pNormal = m_position.Cross(m_velocity);

	return m_dvPrograde * m_velocity.Normalized() +
		   m_dvNormal   * pNormal.Normalized() +
		   m_dvRadial   * m_position.Normalized();
}

static void DrawAtmosphereSurface(Graphics::Renderer *renderer,
	const matrix4x4d &modelView, const vector3d &campos, float rad, Graphics::Material *mat)
{
	const int LAT_SEGS = 20;
	const int LONG_SEGS = 20;
	vector3d yaxis = campos.Normalized();
	vector3d zaxis = vector3d(1.0,0.0,0.0).Cross(yaxis).Normalized();
	vector3d xaxis = yaxis.Cross(zaxis);
	const matrix4x4d invrot = matrix4x4d::MakeRotMatrix(xaxis, yaxis, zaxis).InverseOf();

	renderer->SetTransform(modelView * matrix4x4d::ScaleMatrix(rad, rad, rad) * invrot);

	// what is this? Well, angle to the horizon is:
	// acos(planetRadius/viewerDistFromSphereCentre)
	// and angle from this tangent on to atmosphere is:
	// acos(planetRadius/atmosphereRadius) ie acos(1.0/1.01244blah)
	double endAng = acos(1.0/campos.Length())+acos(1.0/rad);
	double latDiff = endAng / double(LAT_SEGS);

	double rot = 0.0;
	float sinCosTable[LONG_SEGS+1][2];
	for (int i=0; i<=LONG_SEGS; i++, rot += 2.0*M_PI/double(LONG_SEGS)) {
		sinCosTable[i][0] = float(sin(rot));
		sinCosTable[i][1] = float(cos(rot));
	}

	/* Tri-fan above viewer */
	Graphics::VertexArray va(Graphics::ATTRIB_POSITION);
	va.Add(vector3f(0.f, 1.f, 0.f));
	for (int i=0; i<=LONG_SEGS; i++) {
		va.Add(vector3f(
			sin(latDiff)*sinCosTable[i][0],
			cos(latDiff),
			-sin(latDiff)*sinCosTable[i][1]));
	}
	renderer->DrawTriangles(&va, mat, Graphics::TRIANGLE_FAN);

	/* and wound latitudinal strips */
	double lat = latDiff;
	for (int j=1; j<LAT_SEGS; j++, lat += latDiff) {
		Graphics::VertexArray v(Graphics::ATTRIB_POSITION);
		float cosLat = cos(lat);
		float sinLat = sin(lat);
		float cosLat2 = cos(lat+latDiff);
		float sinLat2 = sin(lat+latDiff);
		for (int i=0; i<=LONG_SEGS; i++) {
			v.Add(vector3f(sinLat*sinCosTable[i][0], cosLat, -sinLat*sinCosTable[i][1]));
			v.Add(vector3f(sinLat2*sinCosTable[i][0], cosLat2, -sinLat2*sinCosTable[i][1]));
		}
		renderer->DrawTriangles(&v, mat, Graphics::TRIANGLE_STRIP);
	}
}

static int l_vector_unit(lua_State *L)
{
	LUA_DEBUG_START(L);
	if (lua_isnumber(L, 1)) {
		double x = luaL_checknumber(L, 1);
		double y = luaL_checknumber(L, 2);
		double z = luaL_checknumber(L, 3);
		const vector3d v = vector3d(x, y, z);
		LuaVector::PushToLua(L, v.NormalizedSafe());
	} else {
		const vector3d *v = LuaVector::CheckFromLua(L, 1);
		LuaVector::PushToLua(L, v->NormalizedSafe());
	}
	LUA_DEBUG_END(L, 1);
	return 1;
}

vector3d TerrainColorFractal<TerrainColorVolcanic>::GetColor(const vector3d &p, double height, const vector3d &norm)
{
	double n = m_invMaxHeight*height;
	const double flatness = pow(p.Dot(norm), 6.0);
	const vector3d color_cliffs = m_rockColor[2];		
	double equatorial_desert = (-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(1.0-p.y*p.y);

	vector3d col;

	if (n > 0.4){
	col = interpolate_color(equatorial_desert, vector3d(.3,.2,0), vector3d(.3, .1, .0));
	col = interpolate_color(n, col, vector3d(.1, .0, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else if (n > 0.2){
	col = interpolate_color(equatorial_desert, vector3d(1.2,1,0), vector3d(.9, .3, .0));
	col = interpolate_color(n, col, vector3d(-1.1, -1, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else if (n > 0.1){
	col = interpolate_color(equatorial_desert, vector3d(.2,.1,0), vector3d(.1, .05, .0));
	col = interpolate_color(n, col, vector3d(2.5, 2, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else {
	col = interpolate_color(equatorial_desert, vector3d(.75,.6,0), vector3d(.75, .2, .0));
	col = interpolate_color(n, col, vector3d(-2, -2.2, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	}
	return col;
}

void HyperspaceCloud::Render(const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	Render::State::UseProgram(Render::simpleShader);
	glDisable(GL_LIGHTING);
	glEnable(GL_BLEND);
	glPushMatrix();
	glTranslatef(float(viewCoords.x), float(viewCoords.y), float(viewCoords.z));
	
	// face the camera dammit
	vector3d zaxis = viewCoords.NormalizedSafe();
	vector3d xaxis = vector3d(0,1,0).Cross(zaxis).Normalized();
	vector3d yaxis = zaxis.Cross(xaxis);
	matrix4x4d rot = matrix4x4d::MakeRotMatrix(xaxis, yaxis, zaxis).InverseOf();
	glMultMatrixd(&rot[0]);
	// precise to the rendered frame (better than PHYSICS_HZ granularity)
	double preciseTime = Pi::game->GetTime() + Pi::GetGameTickAlpha()*Pi::game->GetTimeStep();

	float radius = 1000.0f + 200.0f*float(noise(10.0*preciseTime, 0, 0));
	if (m_isArrival) {
		make_circle_thing(radius, Color(1.0,1.0,1.0,1.0), Color(0.0,0.0,1.0,0.0));
	} else {
		make_circle_thing(radius, Color(1.0,1.0,1.0,1.0), Color(1.0,0.0,0.0,0.0));
	}
	glPopMatrix();
	glDisable(GL_BLEND);
	glEnable(GL_LIGHTING);
}

double Ship::AIFaceOrient(const vector3d &dir, const vector3d &updir)
{
	double timeStep = Pi::GetTimeStep();
	matrix4x4d rot; GetRotMatrix(rot);
	double maxAccel = GetShipType().angThrust / GetAngularInertia();		// should probably be in stats anyway
	double frameAccel = maxAccel * timeStep;
	
	if (dir.Dot(vector3d(rot[8], rot[9], rot[10])) > -0.999999)
		{ AIFaceDirection(dir); return false; }
	
	vector3d uphead = (updir * rot).Normalized();		// create desired object-space updir
	vector3d dav(0.0, 0.0, 0.0);			// desired angular velocity
	double ang = 0.0;
	if (uphead.y < 0.999999)
	{
		ang = acos(Clamp(uphead.y, -1.0, 1.0));		// scalar angle from head to curhead
		double iangvel = sqrt(2.0 * maxAccel * ang);	// ideal angvel at current time

		double frameEndAV = iangvel - frameAccel;
		if (frameEndAV <= 0.0) iangvel = ang / timeStep;	// last frame discrete correction
		else iangvel = (iangvel + frameEndAV) * 0.5;		// discrete overshoot correction
		dav.z = -iangvel;
	}
	vector3d cav = (GetAngVelocity() - GetFrame()->GetAngVelocity()) * rot;				// current obj-rel angvel
//	vector3d cav = GetAngVelocity() * rot;				// current obj-rel angvel
	vector3d diff = (dav - cav) / frameAccel;			// find diff between current & desired angvel

	SetAngThrusterState(diff);
	return ang;
//	if (diff.x*diff.x > 1.0 || diff.y*diff.y > 1.0 || diff.z*diff.z > 1.0) return false;
//	else return true;
}

void SpaceStation::Render(Graphics::Renderer *r, const Camera *camera, const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	Body *b = GetFrame()->GetBody();
	assert(b);

	if (!b->IsType(Object::PLANET)) {
		// orbital spaceport -- don't make city turds or change lighting based on atmosphere
		RenderModel(r, camera, viewCoords, viewTransform);
		r->GetStats().AddToStatCount(Graphics::Stats::STAT_SPACESTATIONS, 1);
	} else {
		// don't render city if too far away
		if (viewCoords.LengthSqr() >= SQRMAXCITYDIST) {
			return;
		}
		std::vector<Graphics::Light> oldLights;
		Color oldAmbient;
		SetLighting(r, camera, oldLights, oldAmbient);

		if (!m_adjacentCity) {
			m_adjacentCity = new CityOnPlanet(static_cast<Planet*>(b), this, m_sbody->GetSeed());
		}
		m_adjacentCity->Render(r, camera->GetContext()->GetFrustum(), this, viewCoords, viewTransform);

		RenderModel(r, camera, viewCoords, viewTransform, false);

		ResetLighting(r, oldLights, oldAmbient);

		r->GetStats().AddToStatCount(Graphics::Stats::STAT_GROUNDSTATIONS, 1);
	}
}

void SpaceStation::Render(const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	LmrObjParams &params = GetLmrObjParams();
	params.label = GetLabel().c_str();
	SetLmrTimeParams();

	for (int i=0; i<MAX_DOCKING_PORTS; i++) {
		params.animStages[ANIM_DOCKING_BAY_1 + i] = m_shipDocking[i].stage;
		params.animValues[ANIM_DOCKING_BAY_1 + i] = m_shipDocking[i].stagePos;
	}

	RenderLmrModel(viewCoords, viewTransform);
	
	/* don't render city if too far away */
	if (viewCoords.Length() > 1000000.0) return;

	// find planet Body*
	Planet *planet;
	{
		Body *_planet = GetFrame()->m_astroBody;
		if ((!_planet) || !_planet->IsType(Object::PLANET)) {
			// orbital spaceport -- don't make city turds
		} else {
			planet = static_cast<Planet*>(_planet);
		
			if (!m_adjacentCity) {
				m_adjacentCity = new CityOnPlanet(planet, this, m_sbody->seed);
			}
			m_adjacentCity->Render(this, viewCoords, viewTransform);
		}
	}
}

// Renders space station and adjacent city if applicable
// For orbital starports: renders as normal
// For surface starports:
//	Lighting: Calculates available light for model and splits light between directly and ambiently lit
//            Lighting is done by manipulating global lights or setting uniforms in atmospheric models shader
void SpaceStation::Render(Graphics::Renderer *r, const Camera *camera, const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	Body *b = GetFrame()->GetBody();
	assert(b);

	if (!b->IsType(Object::PLANET)) {
		// orbital spaceport -- don't make city turds or change lighting based on atmosphere
		RenderModel(r, camera, viewCoords, viewTransform);
	}

	else {
		std::vector<Graphics::Light> oldLights;
		Color oldAmbient;
		SetLighting(r, camera, oldLights, oldAmbient);

		Planet *planet = static_cast<Planet*>(b);
		/* don't render city if too far away */
		if (viewCoords.Length() < 1000000.0){
			if (!m_adjacentCity) {
				m_adjacentCity = new CityOnPlanet(planet, this, m_sbody->seed);
			}
			m_adjacentCity->Render(r, camera, this, viewCoords, viewTransform);
		}

		RenderModel(r, camera, viewCoords, viewTransform, false);

		ResetLighting(r, oldLights, oldAmbient);
	}
}

vector3d TerrainColorFractal<TerrainColorBandedRock>::GetColor(const vector3d &p, double height, const vector3d &norm) const
{
	const double flatness = pow(p.Dot(norm), 6.0);
	double n = fabs(noise(vector3d(height*10000.0,0.0,0.0)));
	vector3d col = interpolate_color(n, m_rockColor[0], m_rockColor[1]);
	return interpolate_color(flatness, col, m_rockColor[2]);
}

/*
 * Function: SpawnShipNear
 *
 * Create a ship and place it in space near the given <Body>.
 *
 * > ship = Space.SpawnShip(type, body, min, max, hyperspace)
 *
 * Parameters:
 *
 *   type - the name of the ship
 *
 *   body - the <Body> near which the ship should be spawned
 *
 *   min - minimum distance from the body to place the ship, in Km
 *
 *   max - maximum distance to place the ship
 *
 *   hyperspace - optional table containing hyperspace entry information. If
 *                this is provided the ship will not spawn directly. Instead,
 *                a hyperspace cloud will be created that the ship will exit
 *                from. The table contains two elements, a <SystemPath> for
 *                the system the ship is travelling from, and the due
 *                date/time that the ship should emerge from the cloud.
 *
 * Return:
 *
 *   ship - a <Ship> object for the new ship
 *
 * Example:
 *
 * > -- spawn a ship 10km from the player
 * > local ship = Ship.SpawnNear("viper_police_craft", Game.player, 10, 10)
 *
 * Availability:
 *
 *   alpha 10
 *
 * Status:
 *
 *   experimental
 */
static int l_space_spawn_ship_near(lua_State *l)
{
    if (!Pi::game)
        luaL_error(l, "Game is not started");

    LUA_DEBUG_START(l);

    const char *type = luaL_checkstring(l, 1);
    if (! ShipType::Get(type))
        luaL_error(l, "Unknown ship type '%s'", type);

    Body *nearbody = LuaObject<Body>::CheckFromLua(2);
    float min_dist = luaL_checknumber(l, 3);
    float max_dist = luaL_checknumber(l, 4);

    SystemPath *path = 0;
    double due = -1;
    _unpack_hyperspace_args(l, 5, path, due);

    Ship *ship = new Ship(type);
    assert(ship);

    Body *thing = _maybe_wrap_ship_with_cloud(ship, path, due);

    // XXX protect against spawning inside the body
    Frame * newframe = nearbody->GetFrame();
    const vector3d newPosition = (MathUtil::RandomPointOnSphere(min_dist, max_dist)* 1000.0) + nearbody->GetPosition();

    // If the frame is rotating and the chosen position is too far, use non-rotating parent.
    // Otherwise the ship will be given a massive initial velocity when it's bumped out of the
    // rotating frame in the next update
    if (newframe->IsRotFrame() && newframe->GetRadius() < newPosition.Length()) {
        assert(newframe->GetParent());
        newframe = newframe->GetParent();
    }

    thing->SetFrame(newframe);;
    thing->SetPosition(newPosition);
    thing->SetVelocity(vector3d(0,0,0));
    Pi::game->GetSpace()->AddBody(thing);

    LuaObject<Ship>::PushToLua(ship);

    LUA_DEBUG_END(l, 1);

    return 1;
}