""" simplest implementation of a trajectory simulation where the simulated
aircraft is flying a pseudo circle around center position at max load factor

mutex protection occurs when calling replace_message

This program is free software: you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation, either version 3 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with
this program. If not, see <http://www.gnu.org/licenses/>.
"""

import datetime, math
from AbstractTrajectorySimulatorBase import AbstractTrajectorySimulatorBase

class PseudoCircleTrajectorySimulator(AbstractTrajectorySimulatorBase):
    def __init__(self,mutex,broadcast_thread,aircrafinfos):
        super().__init__(mutex,broadcast_thread,aircrafinfos)

        self._lat0 = aircrafinfos.lat_deg
        self._lon0 = aircrafinfos.lon_deg

    def refresh_delay(self):
        return 0.02

    def update_aircraftinfos(self):
        turn_rate = self.getTurnRate()
        R = self.getTurnRadius()
        Rlat = (R/6371000.0)*(180.0/math.pi)
        ta = self._aircraftinfos.track_angle_deg - (turn_rate*self.refresh_delay())*(180.0/math.pi)
        ta = math.fmod(ta,360.0)
        self._aircraftinfos.track_angle_deg = ta # intermediate value and single assignment needed at time because of
                                                 # setter and change detection mecanism in AircraftInfo (somehox shitty)
                                                 # TODO : implement better mecanism
        self._aircraftinfos.lat_deg = self._lat0 - Rlat*math.sin(self._aircraftinfos.track_angle_deg*math.pi/180.0)
        self._aircraftinfos.lon_deg = self._lon0 + Rlat/math.cos(self._aircraftinfos.lat_deg*math.pi/180.0)*math.cos(self._aircraftinfos.track_angle_deg*math.pi/180.0)
        #self._lasttime = now

    def getTurnRate(self):
        tr = (9.81/self._aircraftinfos.speed_mps)*math.sqrt(self._aircraftinfos.maxloadfactor**2.0 - 1.0)
        return tr

    def getTurnRadius(self):
        return self._aircraftinfos.speed_mps/self.getTurnRate()
