/// <summary>
/// find difference between current heading and course heading
/// </summary>
/// <param name="targetMark"></param>
/// <param name="previousMark"></param>
/// <param name="current"></param>
/// <param name="previous"></param>
/// <returns></returns>
private double RelativeAngleToCourse(Mark targetMark, Mark previousMark, CoordinatePoint current, CoordinatePoint previous)
{
if (previousMark != null && targetMark != null)
{
float courseAngle = (float)AngleUtilities.FindAngle(targetMark.Location.Project(), previousMark.Location.Project());
float boatAngle = (float)AngleUtilities.FindAngle(previous.Project(), current.Project()); ;
return AngleUtilities.AngleDifference(courseAngle, boatAngle);
}
else
{
return 0;
}
}
/// <summary>
/// calculate vmg from marks
/// </summary>
/// <param name="targetMark"></param>
/// <param name="previousMark"></param>
/// <param name="current"></param>
/// <param name="previous"></param>
/// <param name="speed"></param>
/// <returns></returns>
private double VelocityMadeGood(Mark targetMark, Mark previousMark, CoordinatePoint current, CoordinatePoint previous,double speed)
{
return Math.Cos(Math.Abs(RelativeAngleToCourse(targetMark,previousMark,current,previous))) * speed;
}
/// <inheritdoc />
public void SetMarkLocation(MarkType markType)
{
if(_state.Course ==null || !(_state.Course is CourseByMarks))
{
_state.Course = new CourseByMarks();
}
if (_state.Location != null)
{
var course = _state.Course as CourseByMarks;
if (course.Marks == null)
{
course.Marks = new List<Mark>();
}
if (!course.Marks.Any())
{
var mark = new Mark() { MarkType = markType, CaptureMethod = MarkCaptureMethod.Location, Location = _state.Location };
course.Marks.Add(mark);
State.TargetMark = mark;
}
else if (_state.TargetMark != null && _state.TargetMark.MarkType == markType)
{
_state.TargetMark.CaptureMethod = MarkCaptureMethod.Location;
_state.TargetMark.Location = _state.Location;
}
else if (State.TargetMark != null && _state.TargetMark.MarkType != markType)
{
var mark = new Mark() { MarkType = markType, CaptureMethod = MarkCaptureMethod.Location, Location = _state.Location };
course.Marks.Add(mark);
//State.TargetMark = mark;
}
else
{
_logger.Error("User set mark location for " + markType + " but unsure what to do with it");
}
}
}
/// <inheritdoc />
public void ProcessMarkRoundings()
{
if (_state.Course is CourseByMarks) {
var course = _state.Course as CourseByMarks;
//if the race just started, set the line
if (DetectRaceStart ()) {
var line = course.Marks.FirstOrDefault (x => x.MarkType == MarkType.Line);
if (line == null) {
line = new Mark () {
MarkType = MarkType.Line,
CaptureMethod = MarkCaptureMethod.Location,
Location = _state.Location
};
course.Marks.Insert (0, line);
}
_state.PreviousMark = line;
if (course.Marks.Any (x => x.MarkType == MarkType.Windward)) {
State.TargetMark = course.Marks.Where (x => x.MarkType == MarkType.Windward).Last ();
} else {
State.TargetMark = null;
}
} else if (_state.StartTime.HasValue && _state.BestTime > _state.StartTime && _state.TargetMark != null &&
_state.TargetMark.Location != null && _autoRoundMarkDistanceMeters.HasValue) {
var nextMark = GetNextMark (_state.TargetMark);
if (nextMark != null) {
var distanceToMark = CoordinatePoint.HaversineDistance (_state.Location, _state.TargetMark.Location);
if (distanceToMark < _autoRoundMarkDistanceMeters) {
_logger.Info ("Distance to " + _state.TargetMark.MarkType + " is " + string.Format ("{0:0.0}m") +
", advancing to next mark");
NextMark ();
}
}
}
}
else
{
DetectRaceStart ();
}
}
/// <summary>
/// find the mark that is next in the course based on the current mark
/// </summary>
/// <param name="currentTargetMark">the mark that is prior to the one youre looking for</param>
/// <returns>the the mark after the one specified as current</returns>
private Mark GetNextMark(Mark currentTargetMark)
{
if (_state.Course is CourseByMarks)
{
var course = _state.Course as CourseByMarks;
if (currentTargetMark == null)
{
return course.Marks.LastOrDefault();
}
else if (currentTargetMark.MarkType == MarkType.Line)
{
return course.Marks.Where(x => x.MarkType == MarkType.Windward).LastOrDefault();
}
else if (currentTargetMark.MarkType == MarkType.Windward)
{
return course.Marks.Where(x => x.MarkType == MarkType.Leeward).LastOrDefault();
}
else if (currentTargetMark.MarkType == MarkType.Leeward)
{
return course.Marks.Where(x => x.MarkType == MarkType.Windward).LastOrDefault();
}
else
{
throw new InvalidOperationException("Unknown condition selecting next mark");
}
}
else
{
return null;
}
}
/// <inheritdoc />
public void SetMarkBearing(MarkType markType, double bearing, bool magneticBearing)
{
if(markType==MarkType.Course)
{
if (_state.Course == null || !(_state.Course is CourseByAngle))
{
_state.Course = new CourseByAngle();
if(magneticBearing)
{
if(_state.StateValues.ContainsKey(StateValue.MagneticDeviation))
{
(_state.Course as CourseByAngle).CourseAngle = bearing + _state.StateValues[StateValue.MagneticDeviation];
}
else
{
_logger.Error("Cannot set course angle using magnetic bearing without magnetic deviation!");
return;
}
}
else
{
(_state.Course as CourseByAngle).CourseAngle = bearing;
}
}
}
else if (_state.Location != null)
{
if (_state.Course == null || !(_state.Course is CourseByMarks))
{
_state.Course = new CourseByMarks();
}
Bearing fullBearing = new Bearing() { Location = _state.Location, RecordedAt = _state.BestTime, CompassHeading = bearing };
//compensate for magnetic deviation
if (magneticBearing)
{
if (_state.StateValues.ContainsKey(StateValue.MagneticDeviation))
{
fullBearing.CompassHeading = fullBearing.CompassHeading + _state.StateValues[StateValue.MagneticDeviation];
}
else
{
_logger.Error("Cannot calculate mark location using magnetic bearing without magnetic deviation!");
return;
}
}
_logger.Info(string.Format("Received bearing for {0} of {1:0.00} ({2:0.00} true) from {3},{4}, altitude {5}, deviation {6}", markType, bearing, fullBearing.CompassHeading, fullBearing.Location.Latitude.Value, fullBearing.Location.Longitude.Value, _state.StateValues[StateValue.AltitudeInMeters], _state.StateValues[StateValue.MagneticDeviation]));
Mark mark;
var course = _state.Course as CourseByMarks;
if (!course.Marks.Any(x => x.MarkType == markType))
{
mark = new Mark() { MarkType = markType, CaptureMethod = MarkCaptureMethod.Bearing, Location = null };
mark.Bearings = new List<Bearing>();
mark.Bearings.Add(fullBearing);
course.Marks.Add(mark);
State.TargetMark = mark;
}
else
{
mark = course.Marks.Where(x => x.MarkType == markType).Last();
mark.Bearings.Add(fullBearing);
}
if (mark.Bearings.Count > 1 && mark.CaptureMethod == MarkCaptureMethod.Bearing)
{
var bearing1 = mark.Bearings[mark.Bearings.Count - 2];
var bearing2 = mark.Bearings[mark.Bearings.Count - 1];
var location = CoordinatePointUtilities.FindIntersection(bearing1.Location, bearing1.CompassHeading, bearing2.Location, bearing2.CompassHeading);
mark.Location = location;
_logger.Info(string.Format("Calculated new location of {0} via bearings to be {1},{2}", markType, mark.Location.Latitude.Value, mark.Location.Longitude.Value));
//TODO, if there's more than 2, do we average down?
}
}
}
