// Assumes 0 <= B < T
LossData WCV_tvar::WCV_interval(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
  double time_in = T;
  double time_out = B;

  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  double sz = so.z-si.z;
  double vz = vo.z-vi.z;

  Interval ii = wcv_vertical->vertical_WCV_interval(table.getZTHR(),table.getTCOA(),B,T,sz,vz);

  if (ii.low > ii.up) {
    return LossData(time_in,time_out);
  }
  Vect2 step = v2.ScalAdd(ii.low,s2);
  if (Util::almost_equals(ii.low,ii.up)) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    if (horizontal_WCV(step,v2)) {
      time_in = ii.low;
      time_out = ii.up;
    }
    return LossData(time_in,time_out);
  }
  LossData ld = horizontal_WCV_interval(ii.up-ii.low,step,v2);
  time_in = ld.getTimeIn() + ii.low;
  time_out = ld.getTimeOut() + ii.low;
  return LossData(time_in,time_out);
}

void SolveAccelerationProblems1()
{
  //1. A flower pot falls from a windowsill 25.0 m above the sidewalk.   
  //   How much time does a person below have to move out of the way?  
  //   How fast is the flower pot moving when it strikes the ground? 
  Meter const potfalls( 25. );
  SquareSecond const falloffsquaretime = (potfalls * 2.) / Acceleration3( 9.8 );
  Second const rep1 = sqrt( falloffsquaretime );
  Velocity const rep1_2 = potfalls / rep1;
  Scalar const _rep1 = rep1.GetValue();
  Scalar const _rep1_2 = rep1_2.GetValue();
  Assert( fequal( _rep1, 2.2587697572631282 ) );
  Assert( fequal( _rep1_2, 11.067971810589327  ) );

  //2. A plane, starting from rest at one end of a runway, undergoes a 
  //   constant acceleration of 1.6 m/s2 for a distance of 1600 m before 
  //   takeoff.  What is its speed upon takeoff?  What is the time required 
  //   for takeoff?
  Meter const planedistance( 1600. );
  SquareSecond const squaretime2 = (planedistance * 2.) / Acceleration3( 1.6 );
  Second const rep2 = sqrt( squaretime2 );
  Velocity const rep2_2 = planedistance / rep2;
  Scalar const _rep2 = rep2.GetValue();
  Scalar const _rep2_2 = rep2_2.GetValue(); 
  Assert( fequal( _rep2, 44.721359549995796 ) );
  Assert( fequal( _rep2_2, 35.777087639996637 ) );

}

/**********************************************************
 * SHIP :: interact()
 ***********************************************************/
void Ship :: interact(const Interface & pUI, void * asteroids)
{
   orientation += pUI.isLeft() * TURN_RADIUS;
   orientation -= pUI.isRight() * TURN_RADIUS;

   if (pUI.isUp())
   {
      thrustOn = true;
      Velocity thrust;
      thrust.setDx(THRUST *std::cos(deg2rad(orientation + 90)));
      thrust.setDy(THRUST * std::sin(deg2rad(orientation + 90)));
      v += thrust;
   }
   else
      thrustOn = false;
   
   if (pUI.isSpace())
   {
      Asteroids * pAsteroids = (Asteroids *)asteroids;
      pAsteroids->addItem(new Bullet(v, orientation));
      
   }

   
}

void Receiver1D::removeDirectArrival(const Source& source,
    const Velocity& velocity, float t_width)
{
  float dt = this->getDt();
  int half_len = t_width / dt;

  /// get the velocity average in a range
  int z_min = std::min(source.getDepth(), this->getDepth());
  int z_max = std::max(source.getDepth(), this->getDepth());

  int s[] = {z_min, 0};
  int e[] = {z_max, velocity.getNx()};

  std::vector<int> start(s, s + 2);
  std::vector<int> end(e, e + 2);
  float vel_avg = velocity.getAverageRange(start, end);

  const int nx = this->getGeometryDim()[0];
  for (int ix = 0; ix < nx; ix++) {
    std::vector<int> currRcvLoc, sourceLoc;
    sourceLoc.push_back(source.getDepth());
    sourceLoc.push_back(source.getGeometryOrigin()[0]);
    currRcvLoc.push_back(getDepth());
    currRcvLoc.push_back(getGeometryOrigin()[0] + getGeometryDelta()[0] * ix);
    const float dist2 = variance(sourceLoc, currRcvLoc);

    int tstart = std::sqrt(dist2 * vel_avg);
    int tend = ((tstart + 2 * half_len) > this->getNt()) ? this->getNt() : (tstart + 2 * half_len);

    float *p = &mData[ix * getNt()];
    std::fill(p + tstart, p + tend, 0);
  }

}

/**********************************************************
 * MEDIUMROCK constructor
 ***********************************************************/
MediumRock :: MediumRock(Velocity bigV)
{
   setPoint(bigV.getX(), bigV.getY());
   
   v.setDx(random(-MED_SPEED, MED_SPEED));
   v.setDy(random(-MED_SPEED, MED_SPEED));
   v += bigV;
}

/**********************************************************
 * SMALLROCK constructor
 ***********************************************************/
SmallRock :: SmallRock(Velocity biggerV)
{
   setPoint(biggerV.getX(), biggerV.getY());

   v.setDx(random(-SM_SPEED, SM_SPEED));
   v.setDy(random(-SM_SPEED, SM_SPEED));
   v += biggerV;
}

XYZ principia__BubbleVelocityCorrection(Plugin const* const plugin,
                                        int const reference_body_index) {
  journal::Method<journal::BubbleVelocityCorrection> m({plugin,
                                                        reference_body_index});
  CHECK_NOTNULL(plugin);
  Velocity<World> const result =
      plugin->BubbleVelocityCorrection(reference_body_index);
  return m.Return(ToXYZ(result.coordinates() / (Metre / Second)));
}

bool KinematicIntegerBands::conflict(Detection3D* det, const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi,
    double B, double T) {
  if (Util::almost_equals(B,T)) {
    Vect3 sot = vo.ScalAdd(B,so);
    Vect3 sit = vi.ScalAdd(B,si);
    return det->violation(sot,vo,sit,vi);
  }
  return det->conflict(so,vo,si,vi,B,T);
}

std::pair<LatLonAlt,Velocity> KinematicsLatLon::vsAccel(const LatLonAlt& so, const Velocity& vo,  double t, double a) {
	double dist = vo.gs()*t;
	double currentTrk = vo.trk();
	LatLonAlt sn = GreatCircle::linear_initial(so, currentTrk, dist);
	double nsz = so.alt() + vo.z*t + 0.5*a*t*t;
	sn = sn.mkAlt(nsz);
	Velocity  vn = vo.mkVs(vo.z + a*t);
	return std::pair<LatLonAlt,Velocity>(sn,vn);
}

bool WCV_tvar::violation(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) const {
  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  return horizontal_WCV(s2,v2) &&
      wcv_vertical->vertical_WCV(table.getZTHR(),table.getTCOA(),so.z-si.z,vo.z-vi.z);
}

Velocity PlanningProblem::getControl(uint i)
{
    Velocity c;
    if(i < trajec.length()) {
        Station st = trajec.getStation(i);
        c = st.getVelocity();
    }
    else
        c.setZero();
    return c;
}

void Item::move()
{
   Velocity     v = this->getVelocity();
   Point    slope = v.getSlope();
   Point location = this->getLocation();

   location.addX(slope.getX());
   location.addY(slope.getY());

   this->setLocation(location);
}

    Velocity Solver::getRandomVelocity()
    {
        Velocity velocity;

        for (int i = 0; i < currentProblem->n; ++i) {
            double randomVariable = getRandomDoubleValue(0.0, 1.0);

            velocity.push_back(randomVariable);
        }

        return velocity;
    }

    void Solver::findSolution()
    {
        for (auto &i : swarm.getParticles()) {
            Velocity newVelocity;
            Solution newPosition;

            /**
             * Iterate through all dimensions of a Solution/position
             * to calculate the new velocity and update the position.
             *
             * j = dimension
             */
            for (int j = 0; j < currentProblem->n; ++j) {
                int currentPositionD = i.getPosition().at(j);
                double currentVelocityD = i.getVelocity().at(j);

                int pBestD = i.getBestPosition().at(j);
                int gBestD = swarm.getBestPosition().at(j);

                int randomParticleNumber = getRandomIntegerValue(0, 1);
                int randomGlobalNumber = getRandomIntegerValue(0, 1);

                double newVelocityD = parameters.getInertiaWeight() * currentVelocityD +
                                      parameters.getConstant1() * randomParticleNumber * (pBestD - currentPositionD) +
                                      parameters.getConstant2() * randomGlobalNumber * (gBestD - currentPositionD);

                if (newVelocityD > parameters.getVMax())
                    newVelocityD = parameters.getVMax();
                else if (newVelocityD < -parameters.getVMax())
                    newVelocityD = -parameters.getVMax();

                int newPositionD = updateStrategy->updatePosition(currentPositionD, newVelocityD);

                newVelocity.push_back(newVelocityD);
                newPosition.push_back(newPositionD);
            }

            i.setVelocity(newVelocity);
            i.setPosition(newPosition);

            int pBestTmp = calculateProfit(i.getPosition());
            pBestTmp -= calculatePenalty(i.getPosition(), pBestTmp);

            // Update pBest and gBest position/solution
            if (pBestTmp > i.getBestValue()) {
                i.setBestPositionAndValue(i.getPosition(), pBestTmp);

                if (pBestTmp > swarm.getBestValue())
                    swarm.setBestPositionAndValue(i.getPosition(), pBestTmp);
            }
        }
    }

std::pair<Position,Velocity> ProjectedKinematics::gsAccel(const Position& so, const Velocity& vo, double t, double a) {
	Vect3 s3 = so.point();
	if (so.isLatLon()) {
		s3 = Projection::createProjection(so.lla().zeroAlt()).project(so);
	}
	Vect3 pres = Kinematics::gsAccelPos(s3,vo,t,a);
	Velocity vres = Velocity::mkTrkGsVs(vo.trk(),vo.gs()+a*t,vo.vs());
	if (so.isLatLon()) {
		return Projection::createProjection(so.lla().zeroAlt()).inverse(pres,vres,true);
	} else {
		return std::pair<Position,Velocity>(Position(pres), vres);
	}
}

/**
 * Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
 * @param s0 starting point of line 1
 * @param v0 direction vector for line 1
 * @param s1 starting point of line 2
 * @param v1 direction vector of line 2
 * @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
 * If the lines are parallel, this returns the pair (0,NaN).
 */
double  VectFuns::timeOfIntersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
	Vect2 so = so3.vect2();
	Vect2 vo = vo3.vect2();
	Vect2 si = si3.vect2();
	Vect2 vi = vi3.vect2();
	Vect2 ds = si.Sub(so);
	if (vo.det(vi) == 0) {
		//f.pln(" $$$ intersection: lines are parallel");
		return NaN;
	}
	double tt = ds.det(vi)/vo.det(vi);
	//f.pln(" $$$ intersection: tt = "+tt);
	return tt;
}

bool KinematicIntegerBands::cd_future_traj(Detection3D* det, double B, double T, bool trajdir, double t,
    const TrafficState& ownship, const TrafficState& ac) const {
  if (t > T || B > T) return false;
  std::pair<Vect3,Velocity> sovot = trajectory(ownship,t,trajdir);
  Vect3 sot = sovot.first;
  Velocity vot = sovot.second;
  Vect3 si = ac.get_s();
  Velocity vi = ac.get_v();
  Vect3 sit = vi.ScalAdd(t,si);
  if (B > t) {
    return conflict(det, sot, vot, sit, vi, B-t, T-t);
  }
  return conflict(det, sot, vot, sit, vi, 0, T-t);
}

	void StateWriter::writeState(const std::string& name, double time, const Position& p, const Velocity& v) {
		if (first_line) {
			latlon = p.isLatLon();
			output.setOutputUnits(display_units);

			// Comments and parameters are handled by SeparatedOutput
			
			output.addHeading("name", "unitless");
			if (display_time) {
				output.addHeading("time", "s");				
			}
			if (latlon) {
				output.addHeading("lat",  "deg");
				output.addHeading("lon",  "deg");
				output.addHeading("alt",  "ft");
			} else {
				output.addHeading("sx",   "NM");
				output.addHeading("sy",   "NM");
				output.addHeading("sz",   "ft");
			}
			if (velocity) {
				if (trkgsvs) {
					output.addHeading("trk", "deg");
					output.addHeading("gs",  "knot");
					output.addHeading("vs",  "fpm");
				} else {
					output.addHeading("vx",  "knot");
					output.addHeading("vy",  "knot");
					output.addHeading("vz",  "fpm");
				}
			}
			
			first_line = false;
		}
		output.addColumn(name);
		if (display_time) {
			output.addColumn(FmPrecision(time,precision));			
		}
		output.addColumn(p.toStringList(precision));
		if (velocity) {
			if (trkgsvs) {
				output.addColumn(v.toStringList(precision));							
			} else {
				output.addColumn(v.toStringXYZList(precision));											
			}
		}
		lines++;
		output.writeLine();		
	}	

XYZ VesselWorldVelocity(Plugin const* const plugin,
                        char const* vessel_guid,
                        XYZ const parent_world_velocity,
                        double const parent_rotation_period) {
  Velocity<World> result = CHECK_NOTNULL(plugin)->VesselWorldVelocity(
      vessel_guid,
      Velocity<World>({parent_world_velocity.x * Metre / Second,
                       parent_world_velocity.y * Metre / Second,
                       parent_world_velocity.z * Metre / Second}),
      parent_rotation_period * Second);
  R3Element<Speed> const& coordinates = result.coordinates();
  return XYZ{coordinates.x / (Metre / Second),
             coordinates.y / (Metre / Second),
             coordinates.z / (Metre / Second)};
}

 std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnOmegaAlt(const LatLonAlt& so, const Velocity& vo, double t, double omega) {
   double currentTrk = vo.trk();
   double perpTrk;
   if (omega > 0.0) {
     perpTrk = currentTrk+M_PI/2;
   } else {
     perpTrk = currentTrk-M_PI/2;
   }
   double radius = turnRadiusByRate(vo.gs(), omega);
   LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
   //f.pln("center="+center);
   LatLonAlt sn = GreatCircle::small_circle_rotation(so,center,omega*t).mkAlt(so.alt()+vo.z*t);
   double finalPerpTrk = GreatCircle::initial_course(sn,center);
   double nTrk = finalPerpTrk - M_PI/2 * Util::sign(omega);
   Velocity vn = vo.mkTrk(nTrk);  
   return std::pair<LatLonAlt,Velocity>(sn,vn);		
 }