// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{
    // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
    // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
    // A positive X rate is produced by a positive sensor rotation about the X axis
    // A positive Y rate is produced by a positive sensor rotation about the Y axis
    // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
    // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
    flowMeaTime_ms = imuSampleTime_ms;
    // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
    // reset the accumulated body delta angle and time
    // don't do the calculation if not enough time lapsed for a reliable body rate measurement
    if (delTimeOF > 0.01f) {
        flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
        flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
        delAngBodyOF.zero();
        delTimeOF = 0.0f;
    }
    // check for takeoff if relying on optical flow and zero measurements until takeoff detected
    // if we haven't taken off - constrain position and velocity states
    if (frontend->_fusionModeGPS == 3) {
        detectOptFlowTakeoff();
    }
    // calculate rotation matrices at mid sample time for flow observations
    stateStruct.quat.rotation_matrix(Tbn_flow);
    Tnb_flow = Tbn_flow.transposed();
    // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
    if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
        // correct flow sensor rates for bias
        omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
        omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
        // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
        // note correction for different axis and sign conventions used by the px4flow sensor
        ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
        // write flow rate measurements corrected for body rates
        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
        // record time last observation was received so we can detect loss of data elsewhere
        flowValidMeaTime_ms = imuSampleTime_ms;
        // estimate sample time of the measurement
        ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
        // Correct for the average intersampling delay due to the filter updaterate
        ofDataNew.time_ms -= localFilterTimeStep_ms/2;
        // Prevent time delay exceeding age of oldest IMU data in the buffer
        ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
        // Save data to buffer
        storedOF.push(ofDataNew);
        // Check for data at the fusion time horizon
        flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
    }
}

//----------------------------------------------------------------------------
void BSplineCurveExamples::OnDisplay ()
{
    ClearScreen();

    ColorRGB lightGray(224, 224, 224);
    ColorRGB mediumGray(192, 192, 192);
    ColorRGB darkGray(128, 128, 128);
    ColorRGB black(0, 0, 0);

    // draw axes
    int i;
    for (i = mSize/16; i < mSize; ++i)
    {
        SetPixel(mSize/16, mSize - 1 - i, lightGray);
        SetPixel(i, mSize - 1 - mSize/16, lightGray);
    }

    // draw control points
    int imax = mSpline->GetNumCtrlPoints();
    int x, y;
    for (i = 0; i < imax; ++i)
    {
        const Vector2f& ctrl = mSpline->GetControlPoint(i);
        x = (int)(ctrl.X() + 0.5f);
        y = mSize - 1 - (int)(ctrl.Y() + 0.5f);
        SetThickPixel(x, y, 2, darkGray);
    }

    // draw spline
    imax = 2048;
    for (i = 0; i <= imax; ++i)
    {
        // draw point
        float u = i/(float)imax;
        Vector2f pos = mSpline->GetPosition(u);
        x = (int)(pos.X() + 0.5f);
        y = mSize - 1 - (int)(pos.Y() + 0.5f);

        if (mModified
        &&  mLocCtrlMin[mCurveType] <= u && u <= mLocCtrlMax[mCurveType])
        {
            SetPixel(x, y, mediumGray);
        }
        else
        {
            SetPixel(x, y, black);
        }
    }

    WindowApplication2::OnDisplay();
}

void Camera::rotate2D(float x, float y) {
    Vector2f rot = Vector2f(x, y);

    AngleAxis<float> rotX(rot.x(), Vector3f::UnitY()); // look left right
    AngleAxis<float> rotY(rot.y(), Vector3f::UnitX()); // look up down

    rotation_future_ = (rotX * rotY) * rotation_current_;

    if(roll_correction_){
        rotation_future_ = rollcorrect(rotation_future_);
    }

    emit modified();
}

float fractalsum(const Vector2f& p, float freq)
{
    float t;
    float vec[2];

    for (t = 0.0f; freq >= 1.0f; freq *= 0.5f)
    {
    vec[0] = freq * p.x();
    vec[1] = freq * p.y();
	t += noise2(vec) / freq;
    }

    return t;
}

void Game::OnInput()
{
	if(_input->IsKeyDown(Keys::KEY_A))
		printf("Button %d pressed! \n", Keys::KEY_A);
	if(_input->IsKeyDown(Keys::KEY_B))
		printf("Button %d pressed! \n", Keys::KEY_B);
	if(_input->IsMouseClicked())
	{
		Vector2f mousePosition = _input->GetMousePosition();
		int x = mousePosition.GetX();
		int y = mousePosition.GetY();
		printf("Mouse clicked on x : %d, y : %d \n", x, y);	
	}
}

float turbulence(const Vector2f& p, float freq)
{
    float t;
    float vec[2];

    for (t = 0.0f; freq >= 1.0f; freq *= 0.5f)
    {
    vec[0] = freq * p.x();
    vec[1] = freq * p.y();
	t += (float) fabs(noise2(vec)) / freq;
    }

    return t;
}

//----------------------------------------------------------------------------
void MapTextureToQuad::SelectVertex (const Vector2f& rkPos)
{
    // identify vertex within 5 pixels of mouse click
    const float fPixels = 5.0f;
    m_iSelected = -1;
    for (int i = 0; i < 4; i++)
    {
        Vector2f kDiff = rkPos - m_akVertex[i];
        if ( kDiff.Length() <= fPixels )
        {
            m_iSelected = i;
            break;
        }
    }
}

float Attitude::getDip() const
{

    Vector2f north = Vector2f::UnitX(); // parallel to x axis
    Vector2f dip = getDipDirectionVector().head(
        2); // he third element is 0 (working on horizontal plane)

    float angle = acos(north.dot(dip)) * 180 / M_PI;

    if (dip(1)
        > 0.0) // if y component is postive we are in the 180 to 360 half-space
        angle = 360 - angle;

    return angle;
}

//----------------------------------------------------------------------------
void MapTextureToQuad::SelectVertex (const Vector2f& position)
{
    // Identify vertex within 5 pixels of mouse click.
    const float pixelRange = 5.0f;
    mSelected = -1;
    for (int i = 0; i < 4; ++i)
    {
        Vector2f diff = position - mVertex[i];
        if (diff.Length() <= pixelRange)
        {
            mSelected = i;
            break;
        }
    }
}

Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const
{
  const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
  const Quaternionf& torsoToWorld = theInertialData.orientation2D;
  const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f);
  const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass;
  const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin;

  PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z());

  const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>();
  const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square());

  return (Vector4f() << com.head<2>(), zmp).finished();
}

bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
{
	if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
		v.normalize();
		// To find yaw: take dot product of x = (1,0) and v
		// and multiply by the sign given of cross product of x and v.
		// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
		// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
		heading =  math::sign(v(1)) * wrap_pi(acosf(v(0)));
		return true;
	}

	// heading unknown and therefore do not change heading
	return false;
}

float AP_Landing_Deepstall::update_steering()
{
    Location current_loc;
    if ((!landing.ahrs.get_position(current_loc) || !landing.ahrs.healthy()) && !hold_level) {
        // panic if no position source is available
        // continue the stall but target just holding the wings held level as deepstall should be a minimal
        // energy configuration on the aircraft, and if a position isn't available aborting would be worse
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
        hold_level = true;
    }

    float desired_change = 0.0f;

    if (!hold_level) {
        uint32_t time = AP_HAL::millis();
        float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
        last_time = time;

        Vector2f ab = location_diff(arc_exit, extended_approach);
        ab.normalize();
        Vector2f a_air = location_diff(arc_exit, current_loc);

        crosstrack_error = a_air % ab;
        float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
        float nu1 = asinf(sine_nu1);

        if (L1_i > 0) {
            L1_xtrack_i += nu1 * L1_i / dt;
            L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
            nu1 += L1_xtrack_i;
        }
        desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw) / time_constant;
    }

    float yaw_rate = landing.ahrs.get_gyro().z;
    float yaw_rate_limit_rps = radians(yaw_rate_limit);
    float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);

#ifdef DEBUG_PRINTS
    gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
                                    (double)crosstrack_error,
                                    (double)error,
                                    (double)degrees(yaw_rate),
                                    (double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS

    return ds_PID.get_pid(error);
}

// intersect lines with plane
void Slicer::CalcCuttingPlane(float where, CuttingPlane &plane, const Matrix4f &T)
{
#if CUTTING_PLANE_DEBUG
	cout << "intersect with z " << where << "\n";
#endif
	plane.Clear();
	plane.SetZ(where);

	Vector3f min = T*Min;
	Vector3f max = T*Max;

	plane.Min.x = min.x;
	plane.Min.y = min.y;
	plane.Max.x = max.x;
	plane.Max.y = max.y;

	Vector2f lineStart;
	Vector2f lineEnd;

	bool foundOne = false;
	int num_cutpoints;
	for (size_t i = 0; i < triangles.size(); i++)
	{
		foundOne=false;
		CuttingPlane::Segment line(-1,-1);

		num_cutpoints = triangles[i].CutWithPlane(where, T, lineStart, lineEnd);
		if (num_cutpoints>0)
		  line.start = plane.RegisterPoint(lineStart);
		if (num_cutpoints>1)
		  line.end = plane.RegisterPoint(lineEnd);

		// Check segment normal against triangle normal. Flip segment, as needed.
		if (line.start != -1 && line.end != -1 && line.end != line.start)	
		  // if we found a intersecting triangle
		{
			Vector3f Norm = triangles[i].Normal;
			Vector2f triangleNormal = Vector2f(Norm.x, Norm.y);
			Vector2f segmentNormal = (lineEnd - lineStart).normal();
			triangleNormal.normalise();
			segmentNormal.normalise();
			if( (triangleNormal-segmentNormal).lengthSquared() > 0.2f)
			  // if normals does not align, flip the segment
				line.Swap();
			plane.AddLine(line);
		}
	}
}

//----------------------------------------------------------------------------------------------------
Vector2f Vector2f::operator/ ( float fScalar ) const
{
	Vector2f quot;
	if ( fScalar != 0.0f )
	{
		float fInvScalar = 1.0f / fScalar;
		quot.x = x * fInvScalar;
		quot.y = y * fInvScalar;
	}
	else
	{
		quot.MakeZero();
	}

	return( quot );
}

float MT::evaluate(Warp warp)
{
	rect_t rect = window(warp.t);
	float fine_cell = sqrt(rectArea(rect) / cell_n);
	feature.set_cell(fine_cell);
	feature.set_step(1);

	float E = 0.0f;
	for (int i = 0; i < fine_samples.size(); ++i) {
		Vector2f p = warp.transform2(fine_samples[i]);
		Vector32f F;
		feature.descriptor4(p.x(), p.y(), F.data());
		E = E + sigmoid((F - fine_model.col(i)).squaredNorm());
	}
	return E / fine_samples.size();
}

Vector2f LIP3D::requiredVelocity(const Vector2f& pos, float time)
{
  const Array2f sinhkt(std::sinh(k.x() * time), std::sinh(k.y() * time));
  const Array2f coshkt(std::cosh(k.x() * time), std::cosh(k.y() * time));

  return k.array() * (pos.array() - position.array() * coshkt) / sinhkt;
}

void Ball::Bounce(Vector2f normal)
{
	normal.normalize();
	Vector2f normal_component = normal * mVelocity.dotProduct(normal);

	mVelocity -= normal_component * 2;

	//Now constrain angles - two cones of 30 degrees left/right should be invalid
	Vector2f v_norm = mVelocity;
	v_norm.normalize();
	float dot = v_norm.dotProduct(Vector2f(1, 0));
	if(dot > cos(DEG2RAD(30)) || dot < -cos(DEG2RAD(30)))
	{
		float magnitude = mVelocity.length();
		
		if(mVelocity.x < 0 && mVelocity.y < 0)
			mVelocity = Vector2f(-cos(DEG2RAD(30)), -sin(DEG2RAD(30))) * magnitude;
		else if(mVelocity.x > 0 && mVelocity.y < 0)
			mVelocity = Vector2f(cos(DEG2RAD(30)), -sin(DEG2RAD(30))) * magnitude;
		else if(mVelocity.x < 0 && mVelocity.y > 0)
			mVelocity = Vector2f(-cos(DEG2RAD(30)), sin(DEG2RAD(30))) * magnitude;
		else
			mVelocity = Vector2f(cos(DEG2RAD(30)), sin(DEG2RAD(30))) * magnitude;
	}

}

	/* PRIVATE MEMBER FUNCTIONS */
	void FFTSplineControl::expandButton_Dragged(const Vector2f &v)
	{
		assert(modelview != NULL);
		assert(expandButton != NULL);
		assert(exitButton != NULL);

		Point2f p(v.getX(), 0.0f);
		modelview->unScalePoint(p);

		float newWidth = width + p.getX();
		float beatWidth = InterfaceManager::getBeatLength();

		if(newWidth >= (beatWidth * MIN_BEAT_WIDTH) && newWidth <= (beatWidth * MAX_BEAT_WIDTH))
		{
			// expand/shrink the control
			float factor = newWidth / width;
			stretchControlPoints(factor);

			setWidth(newWidth);

			// move the handles
			expandButton->getPosition().setX(width + BORDER_DIM);
			exitButton->getPosition().setX(width + BORDER_DIM);

			dirty = true;
			fftDirty = true;
		}
	}

/*
 * Adjusts the desired velocity for the polygon fence.
 */
void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel)
{
    // exit if the polygon fence is not enabled
    if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
        return;
    }

    // exit if the polygon fence has already been breached
    if ((_fence.get_breaches() & AC_FENCE_TYPE_POLYGON) != 0) {
        return;
    }

    // exit immediately if no desired velocity
    if (desired_vel.is_zero()) {
        return;
    }

    // get polygon boundary
    // Note: first point in list is the return-point (which copter does not use)
    uint16_t num_points;
    Vector2f* boundary = _fence.get_polygon_points(num_points);

    // adjust velocity using polygon
    adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true);
}

// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
    switch (ekf_type()) {
    case EKF_TYPE_NONE:
        // We are not using an EKF so no data
        return false;

#if AP_AHRS_WITH_EKF1
    case EKF_TYPE1:
        // use EKF to get variance
        EKF1.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
        return true;
#endif

    case EKF_TYPE2:
    default:
        // use EKF to get variance
        EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset);
        return true;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        velVar = 0;
        posVar = 0;
        hgtVar = 0;
        magVar.zero();
        tasVar = 0;
        offset.zero();
        return true;
#endif
    }
}