// Takes the scalar multiplicationof a dVector v with a scalar k
dVector RKF45::scalar_multiplication(double k, const dVector& v) const {
  dVector output;
  for (dVector::const_iterator it=v.begin(); it != v.end(); ++it) {
    output.push_back(k * (*it));
  }
  return output;
}

dVector ortogonalizacao(const dVector u, const dVector v)
{
    double escalar = produtoEscalar(u, v) / produtoEscalar(v, v);
    dVector vetor(u.size());
    for(int i = 0;i< u.size();i++)vetor[i]=u[i] - escalar*v[i];
    return vetor;
}

void Toolbox::calculateGlobalMeanAndStd(DataSet &X,dVector& mean,dVector& stdDev)
{
	calculateGlobalMean(X,mean);
	int nbElements = 0;

	//Calculate standard deviation
	stdDev.set(0);
	for(int i = 0;i < (int)X.size() ;i++)
	{
		double* pData = X.at(i)->getPrecomputedFeatures()->get();
		int Width = X.at(i)->getPrecomputedFeatures()->getWidth();
		int Height = X.at(i)->getPrecomputedFeatures()->getHeight();

		for(int col=0; col < Width; col++)
		{
			double* pStdDev = stdDev.get();
			double* pMean = mean.get();
			for(int row = 0; row < Height;row++)
			{
				*pStdDev += (*pData-*pMean) * (*pData-*pMean);
				pStdDev++;
				pData++;
				pMean++;
			}
		}
		nbElements+=Width;
	}
	stdDev.multiply(1.0/(double)nbElements);
	stdDev.eltSqrt();
}

// Adds the dVector b to the dVector a. a is modified in-place.
void RKF45::sum_in_place(dVector& a, const dVector& b) const {
    assert ( a.size() == b.size()
	   && "To sum two vectors, they must be the same size.");
    for (unsigned int i = 0; i < a.size(); i++) {
      a[i] += b[i];
    }
}

// Calculates the 2-norm of the dVector v
double RKF45::norm(const dVector& v) const {
  double output = 0;
  for (dVector::const_iterator it=v.begin(); it != v.end(); ++it) {
    output += (*it) * (*it);
  }
  return sqrt(output);
}

void BLASInterface::Madd(dVector& v,const dVector& x,double a)
{
  integer n=x.n;
  integer vinc = v.stride;
  integer xinc = x.stride;
  daxpy_(&n,&a,x.getStart(),&xinc,v.getStart(),&vinc);
}

double BLASInterface::Dot(const dVector& x,const dVector& y)
{
  Assert(x.n == y.n);
  integer n=x.n;
  integer xinc = x.stride;
  integer yinc = y.stride;
  return ddot_(&n,x.getStart(),&xinc,y.getStart(),&yinc);
}

double GradientDD::computeGradient(dVector& vecGradrient, Model* m,DataSequence*)
{
	dVector tmpVec;
	vecGradrient = *(m->getWeights());
	vecGradrient.add(mu);
	tmpVec = vecGradrient;
	tmpVec.transpose();
	tmpVec.multiply(vecGradrient);
	double f = exp(-0.5*tmpVec[0]);
	vecGradrient.multiply(f);
	return f;
}

void Toolbox::calculateGlobalMean(DataSet &X,dVector& mean)
{
	dVector seqSum;
	int nbElements = 0;

	//Calculate mean
	for(int i = 0;i < (int)X.size() ;i++)
	{
		X.at(i)->getPrecomputedFeatures()->rowSum(seqSum);
		mean.add(seqSum);
		nbElements+=X.at(i)->getPrecomputedFeatures()->getWidth();
	}
	mean.multiply(1.0/(double)nbElements);

}

void dCustomBallAndSocket::SubmitConstraintTwistLimits(const dMatrix& matrix0, const dMatrix& matrix1, const dVector& relOmega, dFloat timestep)
{
	dFloat jointOmega = relOmega.DotProduct3(matrix0.m_front);
	dFloat twistAngle = m_twistAngle.GetAngle() + jointOmega * timestep;
	if (twistAngle < m_minTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_minTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
	} else if (twistAngle > m_maxTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_maxTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

	} else if (m_twistFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

		dFloat accel = NewtonUserJointCalculateRowZeroAccelaration(m_joint);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
	}
}

// ----------------------------------------------------------------------
double derivative(int i, const dVector& v, double lattice_spacing) {
  // A local "num points" variable. Used for simplicity.
  int num_points = get_num_points(v);
  assert (0 <= i && i < (int)v.size()
	  && "The ith elemennt must be in the vector");

  if ( i % num_points == 0 ) { // We're at the lower boundary of the grid.
    if (DEBUGGING) {
      cout << "\tUsing forward difference." << endl;
    }
    return forward_difference(i,v,lattice_spacing);
  }
  else if (i % num_points == num_points - 1) {
    // We're at the upper boundary of the grid.
    if (DEBUGGING) {
      cout << "\tUsing backward difference." << endl;
    }
    return backward_difference(i,v,lattice_spacing);
  }
  else { // We're in the middle of the grid.
    if (DEBUGGING) {
      cout << "\tUsing centered difference." << endl;
    }
    return centered_difference(i,v,lattice_spacing);
  }
}

void dComplentaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
{
	dVector accel (force.Scale (m_invMass));
	dVector alpha (m_invInertia.RotateVector(torque));
	m_veloc += accel.Scale (timestep);
	m_omega += alpha.Scale (timestep);
}

dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
	dMatrix matrix;
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	dVector updir (matrix.RotateVector(m_upVector));
	dVector frontDir (matrix.RotateVector(m_frontVector));
	dVector rightDir (frontDir * updir);

	dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
	if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
		// plane is supported by a ground plane, apply the player input velocity
		if ((m_groundPlane % updir) >= m_maxSlope) {
			// player is in a legal slope, he is in full control of his movement
			dVector bodyVeloc;
			NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
			veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
			veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));

			dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
			dFloat speedMag2 = veloc % veloc;
			if (speedMag2 > speedLimitMag2) {
				veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
			}

			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		} else {
			// player is in an illegal ramp, he slides down hill an loses control of his movement 
			NewtonBodyGetVelocity(m_body, &veloc[0]);
			veloc += updir.Scale(verticalSpeed);
			veloc += gravity.Scale (timestep);
			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		}
	} else {
		// player is on free fall, only apply the gravity
		NewtonBodyGetVelocity(m_body, &veloc[0]);
		veloc += updir.Scale(verticalSpeed);
		veloc += gravity.Scale (timestep);
	}
	return veloc;
}

void Quaternion::set_v(const dVector & v)
{
	if(v.size() == 3) {
		v_ = v;
	}
	else {
		assert (0 && "Quaternion::set_v: input has a wrong size.");
	}
}

void OutputASCIIShade(ostream& out,const dVector& x,double scale)
{
  if(scale == 0) scale = x.maxAbsElement();
  out<<scale<<" x ";
  if(scale == 0) scale = 1;
  out<<'[';
  for(int i=0;i<x.n;i++)
    out<<ASCIIShade(x(i)/scale);
  out<<']';
}

//Compute gradient
void UnconstrainedOptimizer::G()
{
	dVector dgrad(n);
	memcpy(vecGradient.get(),x,n*sizeof(double));
	currentModel->setWeights(vecGradient);
	if(currentModel->getDebugLevel() >= 2)
		std::cout << "Compute gradient..." << std::endl;
	currentGradient->computeGradient(dgrad, currentModel,currentDataset);
	memcpy(g,dgrad.get(),n*sizeof(double));
}

void InitCamera (const dVector& eyePoint, const dVector& dir)
{
	gCameraEyepoint = eyePoint;

	gCurrCameraDir = dir.Scale (1.0f / sqrt (dir % dir));
	gRollAngle = dAsin (gCurrCameraDir.m_y);
	gPrevRollAngle = gYawAngle;

	gYawAngle = dAtan2 (-gCurrCameraDir.m_z, gCurrCameraDir.m_x);
	gPrevYawAngle = gYawAngle;
}

Quaternion::Quaternion(const double angle, const dVector & axis)
{
	if(axis.size() != 3) {
		assert (0 && "Quaternion::Quaternion, size of axis != 3");
		return;
	}

	// make sure axis is a unit vector
	v_ = sin(angle/2) * axis/Norm2(axis);
	s_ = cos(angle/2);
}

dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
	// this is correct
	dQuaternion rotation (*this);
	dFloat omegaMag2 = omega % omega;
	const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
	const dFloat errAngle2 = errAngle * errAngle;
	if (omegaMag2 > errAngle2) {
		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
		dVector omegaAxis (omega.Scale (invOmegaMag));
		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
		dQuaternion deltaRotation (omegaAxis, omegaAngle);
		rotation = rotation * deltaRotation;
		rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
	}
	return rotation;
}

static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dVector com;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	// apply the thew body forces
	if (chainForceCallback) {
		chainForceCallback (body, timestep, threadIndex);
	}

	// add the mouse pick penalty force and torque
	NewtonBodyGetVelocity(body, &veloc[0]);

	NewtonBodyGetOmega(body, &omega[0]);
	NewtonBodyGetVelocity(body, &veloc[0]);
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

	dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
	dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
	force -= dampForce;

	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodyGetCentreOfMass (body, &com[0]);
	
	// calculate local point relative to center of mass
	dVector point (matrix.RotateVector (attachmentPoint - com));
	dVector torque (point * force);

	dVector torqueDamp (omega.Scale (mass * 0.1f));

	NewtonBodyAddForce (body, &force.m_x);
	NewtonBodyAddTorque (body, &torque.m_x);

	// make sure the body is unfrozen, if it is picked
	NewtonBodySetFreezeState (body, 0);
}