#include "FMC.h"
#include <Helper.h>
#include <GeoFormulas/GeoFormulas.h>
#include <AerospaceFormulas.h>

using namespace std;

double slantRangeToGroundDist(double slantRangeNM, double verticalDistFt)
{
	return FeetToNm(sqrt(pow(NmToFeet(slantRangeNM), 2) - pow(verticalDistFt, 2)));
}

void FMC::FlightPlan::CalculateLegs()
{
	double dTol = 0.0;
	double weight = fmc->gw != DataEntryType::None ? (double)fmc->gw : 280000.0;

	for (size_t i = 0; i < legs.size(); i++)
	{
		shared_ptr<NavDataObj> leg = legs[i];

		if (leg->usage == ObjectUsage::Origin)
		{
			double dist = 0;
			if (arrApt.GetID() != -1)
				dist = depApt.latlon.Dist(arrApt.latlon);
			leg->vertSegments.emplace_back(VerticalSegment(0.0, fmc->GetPredictedSpeedTarget(depApt.elevation, weight, FLIGHT_PHASE::TAKEOFF, dist, &fmc->actFpln, true), depApt.elevation, SPD_INVALID, ObjectUsagePhase::Climb));
		}
		else if (leg->usage == ObjectUsage::Destination)
		{
			leg->vertSegments.emplace_back(VerticalSegment(0.0, fmc->GetPredictedSpeedTarget(arrApt.elevation, weight, FLIGHT_PHASE::APPROACH, 0, &fmc->actFpln, true), arrApt.elevation, SPD_INVALID, ObjectUsagePhase::Descent));
		}

		//NavLat/Lon -> destLatLon
		//NavDist->distToDest

		//WptLat/Lon -> refLatLon

		//distance->distance

		if (leg->GetType() == NavDataObjType::TerminalLeg)
		{
			shared_ptr<TerminalLeg> tl = dynamic_pointer_cast<TerminalLeg>(leg);

			switch (tl->track_code)
			{
			case TrackCode::TrackFromFixToDistance:
				tl->latlon = tl->refLatLon.Dest(tl->distance, modAngle(tl->course - tl->refLatLon.GetMagVar())); //this DEFINITELY works for CourseFromFixToDMEDistance				
				break;
			case TrackCode::CourseFromFixToDMEDistance:
			{
				//check this, terminal ID 11546 on RW35L transition contains one
				double alt = tl->GetPredictedAltitude();
				if (alt == ALT_INVALID)
					alt = tl->altRestrLower;
				if (alt == ALT_INVALID)
					alt = tl->altRestr;

				double wptHeight = tl->nvd.GetID() != -1 ? tl->nvd.elevation : 0;
				double vertDist = fabs(alt - wptHeight);
				double groundDist = slantRangeToGroundDist(tl->distance, vertDist);
				groundDist = CLAMP(groundDist, (tl->distance * 0.5), (tl->distance * 1.5));				

				tl->latlon = tl->refLatLon.Dest(groundDist, modAngle(tl->course - tl->refLatLon.GetMagVar())); //this DEFINITELY works for CourseFromFixToDMEDistance
				tl->isFlyover = true;
				break;
			}
			case TrackCode::ArcToFix:
				//check this - segment generator must create the arc based on NavDist?
				tl->latlon = tl->refLatLon;
				break;
			case TrackCode::RadiusToFix:
				tl->latlon = tl->refLatLon;
				break;
			case TrackCode::CourseFromFixToAltitude:
			case TrackCode::CourseToAltitude:
			case TrackCode::HeadingToAltitude:
			{
				double lastAlt = 0;
				LatitudeLongitude startLatLon;
				double lastSpeed = 150;

				for (int lastLeg = this->GetPreviousFlyableLeg((int)i, legs); lastLeg != -1; lastLeg = this->GetPreviousFlyableLeg((int)lastLeg, legs))
				{
					startLatLon = FMC::GetBestLatLonForLeg(this, legs[lastLeg], FMC::perf.get(), true); //this is because the segment will indefinitely extend otherwise
					lastAlt = GetBestAltitudeForLeg(legs[lastLeg]);
					lastSpeed = fmc->GetPredictedSpeedTarget(lastAlt, weight, legs[lastLeg]->GetUsagePhase() == ObjectUsagePhase::Climb || legs[lastLeg]->GetUsagePhase() == ObjectUsagePhase::MissedApproach ? FLIGHT_PHASE::CLIMB : FLIGHT_PHASE::CRUISE, 0, this, true);
					break;
				}


				double angle = modAngle(tl->course + startLatLon.GetMagVar());

				double minDist = 0;

				if (tl->track_code == TrackCode::CourseFromFixToAltitude) //this breaks at airports
				{
					if (fabs(diffAngle(angle, tl->refLatLon.BearingTo(startLatLon))) < 10)						
						minDist = tl->refLatLon.Dist(startLatLon);

					startLatLon = tl->refLatLon;
				}
				
				//the heading one should _technically_ account for wind drift here

				double diffAlt = tl->altRestr - lastAlt;

				if (tl->altRestrictionType == AltitudeRestrictionType::Above)
					diffAlt = max(diffAlt, 1);
				else if (tl->altRestrictionType == AltitudeRestrictionType::Below)
					diffAlt = min(diffAlt, -1);

				double fpm = FMC::perf->GetClimbRate(lastAlt, lastSpeed, weight);
				{
					double fpmMax = 0;

					double groundHeight = 0;
					if(tl->usage == ObjectUsage::Origin || tl->usage == ObjectUsage::DepartureLeg)
						groundHeight = depApt.GetID() != -1 ? depApt.elevation : 0;
					else if (tl->usage == ObjectUsage::Destination || tl->usage == ObjectUsage::ArrivalLeg || tl->usage == ObjectUsage::ApproachLeg)
						groundHeight = arrApt.GetID() != -1 ? arrApt.elevation : 0;

					double altAgl = tl->altRestr - groundHeight;
					if (altAgl < 150)
						fpmMax = 400;
					else if (altAgl < 300)
						fpmMax = 800;
					else if (altAgl < 1000)
						fpmMax = 1000;
					else if (altAgl < 1500)
						fpmMax = 1500;

					if (fpmMax > 0)
						fpm = min(fpm, fpmMax);
				}

				double minutes = fabs(diffAlt / fpm);
				double dist = ((minutes / 60.0) * lastSpeed) + minDist;

				tl->latlon = startLatLon.Dest(dist, angle);
				tl->isFlyover = true; //is this right? aren't all of these automatically overfly?
				break;
			}
			case TrackCode::HeadingToDMEDistance:
			case TrackCode::CourseToDMEDistance:
			{
				if (i > 0) //needs to account for DISCOs, etc.
				{
					//DME distance is usually slant range - account for altitude in this?
					LLPoint intcpt1;
					LLPoint intcpt2;

					std::shared_ptr<NavDataObj> ll = legs[i - 1];
					LatitudeLongitude startPoint = ll->GetLatitudeLongitude();
					double desCourse = tl->course;

					double alt = tl->GetPredictedAltitude();
					if (alt == ALT_INVALID)
						alt = tl->altRestrLower;
					if (alt == ALT_INVALID)
						alt = tl->altRestr;

					double wptHeight = tl->nvd.GetID() != -1 ? tl->nvd.elevation : 0;
					double vertDist = alt != ALT_INVALID ? fabs(alt - wptHeight) : 0;
					double groundDist = slantRangeToGroundDist(tl->distance, vertDist);
					groundDist = CLAMP(groundDist, (tl->distance * 0.5), (tl->distance * 1.5));

					//the heading one should _technically_ account for wind drift here

					if (ll->GetLatitudeLongitude().Dist(tl->destLatlon) <= groundDist)
					{
						startPoint = ll->GetLatitudeLongitude().Dest(groundDist * 3.0, modAngle(tl->course + ll->GetLatitudeLongitude().GetMagVar()));
						desCourse = modAngle(desCourse - 180.0);
					}

					if (GeodesicArcIntercept(startPoint, DEGTORAD(modAngle(desCourse + startPoint.GetMagVar())), tl->destLatlon, NmToMeters(groundDist), intcpt1, intcpt2, dTol) > 0)
					{
						bool intcpt1valid = fabs(diffAngle(startPoint.BearingTo(intcpt1), desCourse)) < 90.0;
						bool intcpt2valid = fabs(diffAngle(startPoint.BearingTo(intcpt2), desCourse)) < 90.0;

						if (intcpt1valid == true && intcpt2valid == false)
							tl->latlon = intcpt1;
						else if (intcpt1valid == false && intcpt2valid == true)
							tl->latlon = intcpt2;
						else
						{
							if (startPoint.Dist(intcpt1) < startPoint.Dist(intcpt2))
								tl->latlon = intcpt1;
							else
								tl->latlon = intcpt2;
						}
					}
					else
						tl->latlon = tl->destLatlon;
				}
				else
					tl->latlon = tl->destLatlon;
				
				tl->isFlyover = true;
				break;
			}
			case TrackCode::HoldAtFix:
			case TrackCode::HoldUntilManualTermination:
			case TrackCode::HoldToAltitude:
			{
				if (tl->hold.type == HoldType::Database)
				{
					tl->hold.holdDir = tl->turn_dir == ObjectTurnDirection::Left ? TurnDirection::Left : TurnDirection::Right;
					tl->hold.holdDist = tl->distance != -1.0 ? tl->distance : tl->distToDest; //is this right?
					tl->hold.holdDistManual = tl->hold.holdDist != -1;
					tl->hold.holdInboundCourse = tl->course;
					tl->hold.defaultCourse = tl->course;
					tl->hold.wptIdent = i > 0 ? legs[i-1]->GetIdentifier() : L"";
				}
				break;
			}
			case TrackCode::HeadingUntilManualTermination:
			{
				LatitudeLongitude lastlatLon;
				for (int lastLeg = this->GetPreviousFlyableLeg((int)i, legs); lastLeg != -1; lastLeg = this->GetPreviousFlyableLeg((int)lastLeg, legs))
				{
					lastlatLon = legs[lastLeg]->GetLatitudeLongitude();
					break;
				}
				tl->latlon = lastlatLon.Dest(1000, modAngle(tl->course + lastlatLon.GetMagVar()));
				break;
			}
			case TrackCode::CourseToNextIntercept: //is this right?
			case TrackCode::HeadingToNextLegIntercept:
			{
				//TESTED WITH HEADING

				bool set = false;
				if (i + 1 < legs.size() && i > 0)
				{
					if (legs[i + 1]->GetType() == NavDataObjType::TerminalLeg)
					{
						std::shared_ptr<TerminalLeg> ntl = std::dynamic_pointer_cast<TerminalLeg>(legs[i + 1]);
						double trueTlCourse = modAngle(tl->course + ntl->GetLatitudeLongitude().GetMagVar()); //using ntl MagVar as tl->latlon isn't set yet
						LatitudeLongitude last = legs[i - 1]->GetLatitudeLongitude(); //maybe check this for special cases?
						if (legs[i - 1]->isFlyover == true && legs[i - 1]->bearingFromTo != -1)
						{
							double diffCourse = diffAngleByDir(trueTlCourse, legs[i - 1]->bearingFromTo, (int)tl->turn_dir);
							diffCourse = CLAMP(diffCourse, -180, 180);
							double offset = fmc->GetTurnRadiusNM(legs[i]) * (fabs(diffCourse) / 90.0);
							last = last.Dest(offset, modAngle(legs[i - 1]->bearingFromTo + CLAMP(diffCourse, -90, 90)));
						}

						if (ntl->track_code == TrackCode::ArcToFix)
						{
							LLPoint tan1, tan2;

							if (PointToArcTangents(last, ntl->nvd.GetLatitudeLongitude(), NmToMeters(ntl->refLatLon.Dist(ntl->nvd.GetLatitudeLongitude())), tan1, tan2, dTol) != 0)
							{
								if(ntl->turn_dir == ObjectTurnDirection::Left) //this may not be the correct check
									tl->latlon = tan2;
								else
									tl->latlon = tan1;
								set = true;
							}
						}
						else if(ntl->GetLocationType() != LocationType::Abstract && ntl->course != -1 && ntl->GetLatitudeLongitude() != LatitudeLongitude())
						{
							LLPoint intcpt;

							double trueNtlCourse = modAngle(ntl->course + ntl->GetLatitudeLongitude().GetMagVar());							

							if (CrsIntersect(last, DEGTORAD(trueTlCourse), ntl->GetLatitudeLongitude(), DEGTORAD(modAngle(trueNtlCourse + 180.0)), dTol, intcpt))
								tl->latlon = intcpt;
							else
								tl->latlon = last; //this will cause a bypass because it's the exact same location									
							set = true;
						}						
					}
				}
				if (set == false) //sanity check to ensure it's never in the middle of the ocean
				{
					int lastFlyable = this->GetPreviousFlyableLeg((int)i, legs);
					if (lastFlyable != -1)
						tl->latlon = legs[lastFlyable]->GetLatitudeLongitude();
					else
					{
						int nextFlyable = this->GetNextFlyableLeg((int)i, legs);
						if (nextFlyable != -1)
							tl->latlon = legs[nextFlyable]->GetLatitudeLongitude();
					}
				}
				break;
			}

			case TrackCode::HeadingToRadialTermination:
			case TrackCode::CourseToRadialTermination: //Tested and working on CourseToRadialTermination
			{
				//this leg does NOT do the outbound course from the previous leg, it's the outbound course from the point the plane is able to achieve tl->course
				if (i + 1 < legs.size() && i > 1)
				{
					//needs to account for DISCOs, T/C, T/D, etc.
					std::shared_ptr<NavDataObj> ll = legs[i - 1];

					LatitudeLongitude startPoint = ll->GetLatitudeLongitude();

					double tlCrs = modAngle(tl->course - tl->destLatlon.GetMagVar());

					if (i > 2 && ll->isFlyover == true)
					{
						double lastBrg = legs[i - 2]->bearingFromTo;

						TurnDirection dir = diffAngle(tlCrs, lastBrg) < 0 ? TurnDirection::Left : TurnDirection::Right;
						double turnRad = fmc->GetTurnRadiusNM(ll);

						double angleIn = modAngle(lastBrg + (dir == TurnDirection::Left ? -90 : 90));
						double angleOut = modAngle(180.0 + (angleIn + diffAngle(tlCrs, lastBrg)));

						LatitudeLongitude turnCenter = ll->GetLatitudeLongitude().Dest(turnRad, angleIn);
						startPoint = turnCenter.Dest(turnRad, angleOut);
					}

					LLPoint intcpt;

					if (CrsIntersect(startPoint, DEGTORAD(modAngle(tlCrs)), tl->destLatlon, DEGTORAD(tl->bearToDest), dTol, intcpt)
						//&& fabs(diffAngle(last.BearingTo(intcpt), trueTlCourse)) < 90
						//&& fabs(diffAngle(LatitudeLongitude(intcpt).BearingTo(ntl->GetLatitudeLongitude()), trueNtlCourse)) < 90
						)
					{
						tl->latlon = intcpt;
					}
				}
				break;
			}
			case TrackCode::ProcedureTurnToIntercept: //Test with KLBX ILS 07
				if (i > 0 && i + 1 < legs.size() && legs[i -1]->GetLocationType() == LocationType::Absolute && legs[i + 1]->GetLocationType() == LocationType::Absolute)
				{					
					double crsBetween = legs[i - 1]->GetLatitudeLongitude().BearingTo(legs[i + 1]->GetLatitudeLongitude());
					tl->latlon = legs[i - 1]->GetLatitudeLongitude().Dest(tl->distance, crsBetween);
				}
				else
					tl->latlon = tl->destLatlon; //just set to there is something
				break;
			case TrackCode::CourseFromFixToManualTermination:
				tl->latlon = tl->wpt.GetLatitudeLongitude().Dest(1000, modAngle(tl->course + tl->wpt.GetLatitudeLongitude().GetMagVar()));
				break;
			case TrackCode::TrackToFix:
			case TrackCode::CourseToFix:
			case TrackCode::DirectToFix:
				/*These are pretty standard, segment calculator processes them correctly already based on desired course*/
				break;
			}
		}
	}

}