#!/usr/bin/env python
'''
useful extra functions for use by mavlink clients

Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
from __future__ import print_function
from __future__ import absolute_import
from builtins import object
from builtins import sum as builtin_sum

from math import *

try:
    # in case numpy isn't installed
    from .quaternion import Quaternion
except:
    pass

try:
    # rotmat doesn't work on Python3.2 yet
    from .rotmat import Vector3, Matrix3
except Exception:
    pass


def kmh(mps):
    '''convert m/s to Km/h'''
    return mps*3.6

def altitude(SCALED_PRESSURE, ground_pressure=None, ground_temp=None):
    '''calculate barometric altitude'''
    from . import mavutil
    self = mavutil.mavfile_global
    if ground_pressure is None:
        if self.param('GND_ABS_PRESS', None) is None:
            return 0
        ground_pressure = self.param('GND_ABS_PRESS', 1)
    if ground_temp is None:
        ground_temp = self.param('GND_TEMP', 0)
    scaling = ground_pressure / (SCALED_PRESSURE.press_abs*100.0)
    temp = ground_temp + 273.15
    return log(scaling) * temp * 29271.267 * 0.001

def altitude2(SCALED_PRESSURE, ground_pressure=None, ground_temp=None):
    '''calculate barometric altitude'''
    from . import mavutil
    self = mavutil.mavfile_global
    if ground_pressure is None:
        if self.param('GND_ABS_PRESS', None) is None:
            return 0
        ground_pressure = self.param('GND_ABS_PRESS', 1)
    if ground_temp is None:
        ground_temp = self.param('GND_TEMP', 0)
    scaling = SCALED_PRESSURE.press_abs*100.0 / ground_pressure
    temp = ground_temp + 273.15
    return 153.8462 * temp * (1.0 - exp(0.190259 * log(scaling)))

def mag_heading(RAW_IMU, ATTITUDE, declination=None, SENSOR_OFFSETS=None, ofs=None):
    '''calculate heading from raw magnetometer'''
    if declination is None:
        from . import mavutil
        declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
    mag_x = RAW_IMU.xmag
    mag_y = RAW_IMU.ymag
    mag_z = RAW_IMU.zmag
    if SENSOR_OFFSETS is not None and ofs is not None:
        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z

    # go via a DCM matrix to match the APM calculation
    dcm_matrix = rotation(ATTITUDE)
    cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
    headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y
    headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z)

    heading = degrees(atan2(-headY,headX)) + declination
    if heading < 0:
        heading += 360
    return heading

def mag_field_df(MAG, ofs=None, diagonals=(1.0,1.0,1.0), offdiagonals=(0.0,0.0,0.0)):
    '''calculate magnetic field strength from raw magnetometer for DF '''
    mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
    if ofs is not None:
        mag += Vector3(ofs[0],ofs[1],ofs[2]) - Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
        diagonals = Vector3(diagonals[0], diagonals[1], diagonals[2])
        offdiagonals = Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2])
        rot = Matrix3(Vector3(diagonals.x,    offdiagonals.x, offdiagonals.y),
                      Vector3(offdiagonals.x, diagonals.y,    offdiagonals.z),
                      Vector3(offdiagonals.y, offdiagonals.z, diagonals.z))
        mag = rot * mag
    return mag.length()

def mag_heading_df(MAG, ATT, declination=None, ofs=None, diagonals=(1.0,1.0,1.0), offdiagonals=(0.0,0.0,0.0)):
    '''calculate heading from raw magnetometer'''
    if declination is None:
        from pymavlink import mavutil
        declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
    mag = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ)
    if ofs is not None:
        mag += Vector3(ofs[0],ofs[1],ofs[2]) - Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
        diagonals = Vector3(diagonals[0], diagonals[1], diagonals[2])
        offdiagonals = Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2])
        rot = Matrix3(Vector3(diagonals.x,    offdiagonals.x, offdiagonals.y),
                      Vector3(offdiagonals.x, diagonals.y,    offdiagonals.z),
                      Vector3(offdiagonals.y, offdiagonals.z, diagonals.z))
        mag = rot * mag

    # go via a DCM matrix to match the APM calculation
    dcm_matrix = rotation_df(ATT)
    cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
    headY = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y
    headX = mag.x * cos_pitch_sq - dcm_matrix.c.x * (mag.y * dcm_matrix.c.y + mag.z * dcm_matrix.c.z)

    heading = degrees(atan2(-headY,headX)) + declination
    if heading < 0:
        heading += 360
    return heading

def gps_time_to_epoch(week, msec):
    '''convert GPS week and TOW to a time in seconds since 1970'''
    epoch = 86400*(10*365 + int((1980-1969)/4) + 1 + 6 - 2)
    return epoch + 86400*7*week + msec*0.001 - 18


def mag_heading_motors(RAW_IMU, ATTITUDE, declination, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs):
    '''calculate heading from raw magnetometer'''
    ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs)

    if declination is None:
        from . import mavutil
        declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
    mag_x = RAW_IMU.xmag
    mag_y = RAW_IMU.ymag
    mag_z = RAW_IMU.zmag
    if SENSOR_OFFSETS is not None and ofs is not None:
        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z

    headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch)
    headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll)
    heading = degrees(atan2(-headY,headX)) + declination
    if heading < 0:
        heading += 360
    return heading

def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None):
    '''calculate magnetic field strength from raw magnetometer'''
    mag_x = RAW_IMU.xmag
    mag_y = RAW_IMU.ymag
    mag_z = RAW_IMU.zmag
    if SENSOR_OFFSETS is not None and ofs is not None:
        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
    return sqrt(mag_x**2 + mag_y**2 + mag_z**2)

def mag_field_df(MAG, ofs=None):
    '''calculate magnetic field strength from raw magnetometer (dataflash version)'''
    mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
    offsets = Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
    if ofs is not None:
        mag = (mag - offsets) + Vector3(ofs[0], ofs[1], ofs[2])
    return mag.length()

def get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs):
    '''calculate magnetic field strength from raw magnetometer'''
    from . import mavutil
    self = mavutil.mavfile_global

    m = SERVO_OUTPUT_RAW
    motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
    motor_pwm *= 0.25
    rc3_min = self.param('RC3_MIN', 1100)
    rc3_max = self.param('RC3_MAX', 1900)
    motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
    if motor > 1.0:
        motor = 1.0
    if motor < 0.0:
        motor = 0.0

    motor_offsets0 = motor_ofs[0] * motor
    motor_offsets1 = motor_ofs[1] * motor
    motor_offsets2 = motor_ofs[2] * motor
    ofs = (ofs[0] + motor_offsets0, ofs[1] + motor_offsets1, ofs[2] + motor_offsets2)

    return ofs

def mag_field_motors(RAW_IMU, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs):
    '''calculate magnetic field strength from raw magnetometer'''
    mag_x = RAW_IMU.xmag
    mag_y = RAW_IMU.ymag
    mag_z = RAW_IMU.zmag

    ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs)

    if SENSOR_OFFSETS is not None and ofs is not None:
        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
    return sqrt(mag_x**2 + mag_y**2 + mag_z**2)

def angle_diff(angle1, angle2):
    '''show the difference between two angles in degrees'''
    ret = angle1 - angle2
    if ret > 180:
        ret -= 360;
    if ret < -180:
        ret += 360
    return ret

average_data = {}

def average(var, key, N):
    '''average over N points'''
    global average_data
    if not key in average_data:
        average_data[key] = [var]*N
        return var
    l = average_data[key]
    l.pop(0)
    l.append(var)
    return builtin_sum(l) / N

derivative_data = {}

def second_derivative_5(var, key):
    '''5 point 2nd derivative'''
    global derivative_data
    from . import mavutil
    tnow = mavutil.mavfile_global.timestamp

    if not key in derivative_data:
        derivative_data[key] = (tnow, [var]*5)
        return 0
    (last_time, data) = derivative_data[key]
    data.pop(0)
    data.append(var)
    derivative_data[key] = (tnow, data)
    h = (tnow - last_time)
    # N=5 2nd derivative from
    # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
    ret = ((data[4] + data[0]) - 2*data[2]) / (4*h**2)
    return ret

def second_derivative_9(var, key):
    '''9 point 2nd derivative'''
    global derivative_data
    from . import mavutil
    tnow = mavutil.mavfile_global.timestamp

    if not key in derivative_data:
        derivative_data[key] = (tnow, [var]*9)
        return 0
    (last_time, data) = derivative_data[key]
    data.pop(0)
    data.append(var)
    derivative_data[key] = (tnow, data)
    h = (tnow - last_time)
    # N=5 2nd derivative from
    # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
    f = data
    ret = ((f[8] + f[0]) + 4*(f[7] + f[1]) + 4*(f[6]+f[2]) - 4*(f[5]+f[3]) - 10*f[4])/(64*h**2)
    return ret

lowpass_data = {}

def lowpass(var, key, factor):
    '''a simple lowpass filter'''
    global lowpass_data
    if not key in lowpass_data:
        lowpass_data[key] = var
    else:
        lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var
    return lowpass_data[key]

def lpalpha(sample_rate_hz, cutoff_hz):
    '''find alpha for low pass filter'''
    rc = 1.0 / (2*pi*cutoff_hz)
    dt = 1.0 / sample_rate_hz
    return 1.0 - dt/(dt+rc)

lowpass_hz_data = {}

def lowpassHz(var, key, sample_rate_hz, cutoff_hz):
    '''a simple lowpass filter with specified frequency'''
    global lowpass_hz_data
    alpha = lpalpha(sample_rate_hz, cutoff_hz)
    if not key in lowpass_hz_data:
        lowpass_hz_data[key] = var
    else:
        lowpass_hz_data[key] = alpha*lowpass_hz_data[key] + (1.0-alpha)*var
    return lowpass_hz_data[key]

last_diff = {}

def diff(var, key):
    '''calculate differences between values'''
    global last_diff
    ret = 0
    if not key in last_diff:
        last_diff[key] = var
        return 0
    ret = var - last_diff[key]
    last_diff[key] = var
    return ret

last_delta = {}

def delta(var, key, tusec=None):
    '''calculate slope'''
    if isnan(var):
        return None
    global last_delta
    if tusec is not None:
        tnow = tusec * 1.0e-6
    else:
        from . import mavutil
        tnow = mavutil.mavfile_global.timestamp
    ret = 0
    if key in last_delta:
        (last_v, last_t, last_ret) = last_delta[key]
        if last_t == tnow:
            return last_ret
        if tnow == last_t:
            ret = 0
        else:
            ret = (var - last_v) / (tnow - last_t)
    last_delta[key] = (var, tnow, ret)
    return ret

last_sum = {}

def sum(var, key):
    '''sum variable'''
    global last_sum
    ret = 0
    if not key in last_sum:
        last_sum[key] = 0
    last_sum[key] += var
    return last_sum[key]

last_integral = {}

def integral(var, key, timeus):
    '''integrate variable'''
    global last_integral
    ret = 0
    if not key in last_integral:
        last_integral[key] = (0,timeus)
    (lastsum,lastt) = last_integral[key]
    dt = (timeus - lastt) * 1.0e-6
    dv = var * dt
    newv = lastsum + dv
    last_integral[key] = (newv,timeus)
    return newv


def delta_angle(var, key, tusec=None):
    '''calculate slope of an angle'''
    global last_delta
    if tusec is not None:
        tnow = tusec * 1.0e-6
    else:
        from . import mavutil
        tnow = mavutil.mavfile_global.timestamp
    dv = 0
    ret = 0
    if key in last_delta:
        (last_v, last_t, last_ret) = last_delta[key]
        if last_t == tnow:
            return last_ret
        if tnow == last_t:
            ret = 0
        else:
            dv = var - last_v
            if dv > 180:
                dv -= 360
            if dv < -180:
                dv += 360
            ret = dv / (tnow - last_t)
    last_delta[key] = (var, tnow, ret)
    return ret

def roll_estimate(RAW_IMU,GPS_RAW_INT=None,ATTITUDE=None,SENSOR_OFFSETS=None, ofs=None, mul=None,smooth=0.7):
    '''estimate roll from accelerometer'''
    rx = RAW_IMU.xacc * 9.81 / 1000.0
    ry = RAW_IMU.yacc * 9.81 / 1000.0
    rz = RAW_IMU.zacc * 9.81 / 1000.0
    if ATTITUDE is not None and GPS_RAW_INT is not None:
        ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01
        rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01
    if SENSOR_OFFSETS is not None and ofs is not None:
        rx += SENSOR_OFFSETS.accel_cal_x
        ry += SENSOR_OFFSETS.accel_cal_y
        rz += SENSOR_OFFSETS.accel_cal_z
        rx -= ofs[0]
        ry -= ofs[1]
        rz -= ofs[2]
        if mul is not None:
            rx *= mul[0]
            ry *= mul[1]
            rz *= mul[2]
    return lowpass(degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))),'_roll',smooth)

def pitch_estimate(RAW_IMU, GPS_RAW_INT=None,ATTITUDE=None, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7):
    '''estimate pitch from accelerometer'''
    rx = RAW_IMU.xacc * 9.81 / 1000.0
    ry = RAW_IMU.yacc * 9.81 / 1000.0
    rz = RAW_IMU.zacc * 9.81 / 1000.0
    if ATTITUDE is not None and GPS_RAW_INT is not None:
        ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01
        rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01
    if SENSOR_OFFSETS is not None and ofs is not None:
        rx += SENSOR_OFFSETS.accel_cal_x
        ry += SENSOR_OFFSETS.accel_cal_y
        rz += SENSOR_OFFSETS.accel_cal_z
        rx -= ofs[0]
        ry -= ofs[1]
        rz -= ofs[2]
        if mul is not None:
            rx *= mul[0]
            ry *= mul[1]
            rz *= mul[2]
    return lowpass(degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))),'_pitch',smooth)

def rotation(ATTITUDE):
    '''return the current DCM rotation matrix'''
    r = Matrix3()
    if hasattr(ATTITUDE, 'roll'):
        r.from_euler(ATTITUDE.roll, ATTITUDE.pitch, ATTITUDE.yaw)
    else:
        r.from_euler(radians(ATTITUDE.Roll), radians(ATTITUDE.Pitch), radians(ATTITUDE.Yaw))
    return r

def mag_rotation(RAW_IMU, inclination, declination):
    '''return an attitude rotation matrix that is consistent with the current mag
       vector'''
    m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
    m_earth = Vector3(m_body.length(), 0, 0)

    r = Matrix3()
    r.from_euler(0, -radians(inclination), radians(declination))
    m_earth = r * m_earth

    r.from_two_vectors(m_earth, m_body)
    return r

def mag_pitch(RAW_IMU, inclination, declination):
    '''estimate pithc from mag'''
    m = mag_rotation(RAW_IMU, inclination, declination)
    (r, p, y) = m.to_euler()
    return degrees(p)

def mag_roll(RAW_IMU, inclination, declination):
    '''estimate roll from mag'''
    m = mag_rotation(RAW_IMU, inclination, declination)
    (r, p, y) = m.to_euler()
    return degrees(r)

def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7):
    '''estimate pitch from accelerometer'''
    if hasattr(RAW_IMU, 'xacc'):
        rx = RAW_IMU.xacc * 9.81 / 1000.0
        ry = RAW_IMU.yacc * 9.81 / 1000.0
        rz = RAW_IMU.zacc * 9.81 / 1000.0
    else:
        rx = RAW_IMU.AccX
        ry = RAW_IMU.AccY
        rz = RAW_IMU.AccZ
    if SENSOR_OFFSETS is not None and ofs is not None:
        rx += SENSOR_OFFSETS.accel_cal_x
        ry += SENSOR_OFFSETS.accel_cal_y
        rz += SENSOR_OFFSETS.accel_cal_z
        rx -= ofs[0]
        ry -= ofs[1]
        rz -= ofs[2]
        if mul is not None:
            rx *= mul[0]
            ry *= mul[1]
            rz *= mul[2]
    return sqrt(rx**2+ry**2+rz**2)



def pitch_sim(SIMSTATE, GPS_RAW):
    '''estimate pitch from SIMSTATE accels'''
    xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9)
    zacc = SIMSTATE.zacc
    zacc += SIMSTATE.ygyro * GPS_RAW.v;
    if xacc/zacc >= 1:
        return 0
    if xacc/zacc <= -1:
        return -0
    return degrees(-asin(xacc/zacc))

ORGN = None

def get_origin():
  global ORGN
  if ORGN is not None:
      return ORGN
  from . import mavutil
  self = mavutil.mavfile_global
  ret = self.messages.get('ORGN', None)
  if ret is None:
      ret = self.messages.get('GPS', None)
      if ret.Status < 3:
          return None
  ORGN = ret
  return ret

# graph distance_two(GPS,XKF1[0])

def get_lat_lon_alt(MSG):
    '''gets lat and lon in radians and alt in meters from a position msg'''
    if hasattr(MSG, 'Lat') and hasattr(MSG, 'Lng'):
        lat = radians(MSG.Lat)
        lon = radians(MSG.Lng)
        alt = MSG.Alt
    elif hasattr(MSG, 'Lat') and hasattr(MSG, 'Lon'):
        lat = radians(MSG.Lat)
        lon = radians(MSG.Lon)
        alt = MSG.Alt
    elif hasattr(MSG, 'cog'):
        lat = radians(MSG.lat)*1.0e-7
        lon = radians(MSG.lon)*1.0e-7
        alt = MSG.alt*0.001
    elif hasattr(MSG,'lat') and hasattr(MSG,'lon'):
        lat = radians(MSG.lat)
        lon = radians(MSG.lon)
        alt = MSG.alt*0.001
    elif hasattr(MSG,'lat') and hasattr(MSG,'lng'):
        lat = radians(MSG.lat)
        lon = radians(MSG.lng)
        alt = MSG.alt*0.001
    elif hasattr(MSG, 'PN'):
        # origin relative position from EKF
        (lat,lon,alt) = ekf1_pos(MSG)
        lat = radians(lat)
        lon = radians(lon)
    else:
        return None
    return (lat, lon, alt)


def _distance_two(MSG1, MSG2, horizontal=True):
    '''
    Return the distance between two points in metres
    Calculated as the great-circle distance using 'haversine’ formula
    (Ref: http://www.movable-type.co.uk/scripts/latlong.html)
    Uses the globally-average earth radius value of 6371km
    '''
    # get_lat_lon_alt returns radians - so no need to convert here
    (lat1, lon1, alt1) = get_lat_lon_alt(MSG1)
    (lat2, lon2, alt2) = get_lat_lon_alt(MSG2)
    dLat = lat2 - lat1
    dLon = lon2 - lon1

    a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2)
    c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
    ground_dist = 6371 * 1000 * c
    if horizontal:
        return ground_dist
    return sqrt(ground_dist**2 + (alt2-alt1)**2)

def distance_two(MSG1, MSG2, horizontal=True):
    '''distance between two points'''
    try:
        return _distance_two(MSG1, MSG2)
    except Exception as ex:
        print(ex)
        return None

first_fix = None

def distance_home(GPS_RAW):
    '''distance from first fix point'''
    global first_fix
    if (hasattr(GPS_RAW, 'fix_type') and GPS_RAW.fix_type < 2) or \
       (hasattr(GPS_RAW, 'Status')   and GPS_RAW.Status   < 2):
        return 0

    if first_fix is None:
        first_fix = GPS_RAW
        return 0
    return distance_two(GPS_RAW, first_fix)

def sawtooth(ATTITUDE, amplitude=2.0, period=5.0):
    '''sawtooth pattern based on uptime'''
    mins = (ATTITUDE.usec * 1.0e-6)/60
    p = fmod(mins, period*2)
    if p < period:
        return amplitude * (p/period)
    return amplitude * (period - (p-period))/period

def rate_of_turn(speed, bank):
    '''return expected rate of turn in degrees/s for given speed in m/s and
       bank angle in degrees'''
    if abs(speed) < 2 or abs(bank) > 80:
        return 0
    ret = degrees(9.81*tan(radians(bank))/speed)
    return ret

def wingloading(bank):
    '''return expected wing loading factor for a bank angle in radians'''
    return 1.0/cos(bank)

def airspeed(VFR_HUD, ratio=None, used_ratio=None, offset=None):
    '''recompute airspeed with a different ARSPD_RATIO'''
    from . import mavutil
    mav = mavutil.mavfile_global
    if ratio is None:
        ratio = 1.9936 # APM default
    if used_ratio is None:
        if 'ARSPD_RATIO' in mav.params:
            used_ratio = mav.params['ARSPD_RATIO']
        else:
            print("no ARSPD_RATIO in mav.params")
            used_ratio = ratio
    if hasattr(VFR_HUD,'airspeed'):
        airspeed = VFR_HUD.airspeed
    else:
        airspeed = VFR_HUD.Airspeed
    airspeed_pressure = (airspeed**2) / used_ratio
    if offset is not None:
        airspeed_pressure += offset
        if airspeed_pressure < 0:
            airspeed_pressure = 0
    airspeed = sqrt(airspeed_pressure * ratio)
    return airspeed

def EAS2TAS(ARSP,GPS,BARO,ground_temp=25):
    '''EAS2TAS from ARSP.Temp'''
    tempK = ground_temp + 273.15 - 0.0065 * GPS.Alt
    return sqrt(1.225 / (BARO.Press / (287.26 * tempK)))

SSL_AIR_DENSITY = 1.225
C_TO_KELVIN = 273.15
ISA_LAPSE_RATE = 0.0065
ISA_GAS_CONSTANT = 287.26
SSL_AIR_TEMPERATURE = 288.15
SSL_AIR_PRESSURE = 101325.01576

def SimpleAtmosphere(alt_km):
    REARTH = 6369.0
    GMR    = 34.163195

    # geometric to geopotential altitude
    h = alt_km*REARTH/(alt_km+REARTH)

    if (h < 11.0):
        # Troposphere
        theta = (SSL_AIR_TEMPERATURE - 6.5 * h) / SSL_AIR_TEMPERATURE
        delta = pow(theta, GMR / 6.5)
    else:
        # Stratosphere
        theta = 216.65 / SSL_AIR_TEMPERATURE
        delta = 0.2233611 * exp(-GMR * (h - 11.0) / 216.65)

    sigma = delta/theta
    return (sigma, delta, theta)

def eas2tas(alt_m, groundtemp=25.0):
    '''eas2tas from altitude in meters AMSL'''
    (sigma, delta, theta) = SimpleAtmosphere(alt_m*0.001)
    pressure = SSL_AIR_PRESSURE * delta
    tempK = groundtemp + C_TO_KELVIN - ISA_LAPSE_RATE * alt_m
    eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK))
    return sqrt(eas2tas_squared)

def airspeed_tas(VFR_HUD,GLOBAL_POSITION_INT):
    '''airspeed as true airspeed from VFR_HUD and GLOBAL_POSITION_INT'''
    return eas2tas(GLOBAL_POSITION_INT.alt*0.001) * VFR_HUD.airspeed

def airspeed_ratio(VFR_HUD):
    '''recompute airspeed with a different ARSPD_RATIO'''
    from . import mavutil
    mav = mavutil.mavfile_global
    airspeed_pressure = (VFR_HUD.airspeed**2) / ratio
    airspeed = sqrt(airspeed_pressure * ratio)
    return airspeed

def airspeed_voltage(VFR_HUD, ratio=None):
    '''back-calculate the voltage the airspeed sensor must have seen'''
    from . import mavutil
    mav = mavutil.mavfile_global
    if ratio is None:
        ratio = 1.9936 # APM default
    if 'ARSPD_RATIO' in mav.params:
        used_ratio = mav.params['ARSPD_RATIO']
    else:
        used_ratio = ratio
    if 'ARSPD_OFFSET' in mav.params:
        offset = mav.params['ARSPD_OFFSET']
    else:
        return -1
    airspeed_pressure = (pow(VFR_HUD.airspeed,2)) / used_ratio
    raw = airspeed_pressure + offset
    SCALING_OLD_CALIBRATION = 204.8
    voltage = 5.0 * raw / 4096
    return voltage


def earth_rates(ATTITUDE):
    '''return angular velocities in earth frame'''
    from math import sin, cos, tan, fabs

    p     = ATTITUDE.rollspeed
    q     = ATTITUDE.pitchspeed
    r     = ATTITUDE.yawspeed
    phi   = ATTITUDE.roll
    theta = ATTITUDE.pitch
    psi   = ATTITUDE.yaw

    phiDot   = p + tan(theta)*(q*sin(phi) + r*cos(phi))
    thetaDot = q*cos(phi) - r*sin(phi)
    if fabs(cos(theta)) < 1.0e-20:
        theta += 1.0e-10
    psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta)
    return (phiDot, thetaDot, psiDot)

def roll_rate(ATTITUDE):
    '''return roll rate in earth frame'''
    (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
    return phiDot

def pitch_rate(ATTITUDE):
    '''return pitch rate in earth frame'''
    (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
    return thetaDot

def yaw_rate(ATTITUDE):
    '''return yaw rate in earth frame'''
    (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
    return psiDot


def gps_velocity(GLOBAL_POSITION_INT):
    '''return GPS velocity vector'''
    return Vector3(GLOBAL_POSITION_INT.vx, GLOBAL_POSITION_INT.vy, GLOBAL_POSITION_INT.vz) * 0.01


def gps_velocity_old(GPS_RAW_INT):
    '''return GPS velocity vector'''
    return Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)),
                   GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)), 0)

def gps_velocity_body(GPS_RAW_INT, ATTITUDE):
    '''return GPS velocity vector in body frame'''
    r = rotation(ATTITUDE)
    return r.transposed() * Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)),
                                    GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)),
                                    -tan(ATTITUDE.pitch)*GPS_RAW_INT.vel*0.01)

def earth_accel(IMU,ATT):
    '''return earth frame acceleration vector'''
    r = rotation(ATT)
    if hasattr(IMU, 'xacc'):
        accel = Vector3(IMU.xacc, IMU.yacc, IMU.zacc) * 9.81 * 0.001
    else:
        accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    return r * accel

def earth_gyro(IMU,ATT):
    '''return earth frame gyro vector'''
    r = rotation(ATT)
    if hasattr(IMU, 'xgyro'):
        gyro = Vector3(IMU.xgyro, IMU.ygyro, IMU.zgyro) * degrees(0.001)
    else:
        gyro = Vector3(IMU.GyrX, IMU.GyrY, IMU.GyrZ)
    return r * gyro

def airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD):
    '''return airspeed energy error matching APM internals
    This is positive when we are going too slow
    '''
    aspeed_cm = VFR_HUD.airspeed*100
    target_airspeed = NAV_CONTROLLER_OUTPUT.aspd_error + aspeed_cm
    airspeed_energy_error = ((target_airspeed*target_airspeed) - (aspeed_cm*aspeed_cm))*0.00005
    return airspeed_energy_error


def energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD):
    '''return energy error matching APM internals
    This is positive when we are too low or going too slow
    '''
    aspeed_energy_error = airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD)
    alt_error = NAV_CONTROLLER_OUTPUT.alt_error*100
    energy_error = aspeed_energy_error + alt_error*0.098
    return energy_error

def rover_turn_circle(SERVO_OUTPUT_RAW):
    '''return turning circle (diameter) in meters for steering_angle in degrees
    '''

    # this matches Toms slash
    max_wheel_turn = 35
    wheelbase      = 0.335
    wheeltrack     = 0.296

    steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0
    theta = radians(steering_angle)
    return (wheeltrack/2) + (wheelbase/sin(theta))

def rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW):
    '''return yaw rate in degrees/second given steering_angle and speed'''
    max_wheel_turn=35
    speed = VFR_HUD.groundspeed
    # assume 1100 to 1900 PWM on steering
    steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0
    if abs(steering_angle) < 1.0e-6 or abs(speed) < 1.0e-6:
        return 0
    d = rover_turn_circle(SERVO_OUTPUT_RAW)
    c = pi * d
    t = c / speed
    rate = 360.0 / t
    return rate

def rover_lat_accel(VFR_HUD, SERVO_OUTPUT_RAW):
    '''return lateral acceleration in m/s/s'''
    speed = VFR_HUD.groundspeed
    yaw_rate = rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW)
    accel = radians(yaw_rate) * speed
    return accel


def demix1(servo1, servo2, gain=0.5):
    '''de-mix a mixed servo output'''
    s1 = servo1 - 1500
    s2 = servo2 - 1500
    out1 = (s1+s2)*gain
    out2 = (s1-s2)*gain
    return out1+1500

def demix2(servo1, servo2, gain=0.5):
    '''de-mix a mixed servo output'''
    s1 = servo1 - 1500
    s2 = servo2 - 1500
    out1 = (s1+s2)*gain
    out2 = (s1-s2)*gain
    return out2+1500

def mixer(servo1, servo2, mixtype=1, gain=0.5):
    '''mix two servos'''
    s1 = servo1 - 1500
    s2 = servo2 - 1500
    v1 = (s1-s2)*gain
    v2 = (s1+s2)*gain
    if mixtype == 2:
        v2 = -v2
    elif mixtype == 3:
        v1 = -v1
    elif mixtype == 4:
        v1 = -v1
        v2 = -v2
    if v1 > 600:
        v1 = 600
    elif v1 < -600:
        v1 = -600
    if v2 > 600:
        v2 = 600
    elif v2 < -600:
        v2 = -600
    return (1500+v1,1500+v2)

def mix1(servo1, servo2, mixtype=1, gain=0.5):
    '''de-mix a mixed servo output'''
    (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain)
    return v1

def mix2(servo1, servo2, mixtype=1, gain=0.5):
    '''de-mix a mixed servo output'''
    (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain)
    return v2

def wrap_180(angle):
    if angle > 180:
        angle -= 360.0
    if angle < -180:
        angle += 360.0
    return angle


def wrap_360(angle):
    if angle > 360:
        angle -= 360.0
    if angle < 0:
        angle += 360.0
    return angle

class DCM_State(object):
    '''DCM state object'''
    def __init__(self, roll, pitch, yaw):
        self.dcm = Matrix3()
        self.dcm2 = Matrix3()
        self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw))
        self.dcm2.from_euler(radians(roll), radians(pitch), radians(yaw))
        self.mag = Vector3()
        self.gyro = Vector3()
        self.accel = Vector3()
        self.gps = None
        self.rate = 50.0
        self.kp = 0.2
        self.kp_yaw = 0.3
        self.omega_P = Vector3()
        self.omega_P_yaw = Vector3()
        self.omega_I = Vector3() # (-0.00199045287445, -0.00653007719666, -0.00714212376624)
        self.omega_I_sum = Vector3()
        self.omega_I_sum_time = 0
        self.omega = Vector3()
        self.ra_sum = Vector3()
        self.last_delta_angle = Vector3()
        self.last_velocity = Vector3()
        (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
        (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler()

    def update(self, gyro, accel, mag, GPS):
        if self.gyro != gyro or self.accel != accel:
            delta_angle = old_div((gyro+self.omega_I), self.rate)
            self.dcm.rotate(delta_angle)
            correction = self.last_delta_angle % delta_angle
            #print (delta_angle - self.last_delta_angle) * 58.0
            corrected_delta = delta_angle + 0.0833333 * correction
            self.dcm2.rotate(corrected_delta)
            self.last_delta_angle = delta_angle

            self.dcm.normalize()
            self.dcm2.normalize()

            self.gyro = gyro
            self.accel = accel
            (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
            (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler()

dcm_state = None

def DCM_update(IMU, ATT, MAG, GPS):
    '''implement full DCM system'''
    global dcm_state
    if dcm_state is None:
        dcm_state = DCM_State(ATT.Roll, ATT.Pitch, ATT.Yaw)

    mag   = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
    gyro  = Vector3(IMU.GyrX, IMU.GyrY, IMU.GyrZ)
    accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    accel2 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    dcm_state.update(gyro, accel, mag, GPS)
    return dcm_state

class PX4_State(object):
    '''PX4 DCM state object'''
    def __init__(self, roll, pitch, yaw, timestamp):
        self.dcm = Matrix3()
        self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw))
        self.gyro = Vector3()
        self.accel = Vector3()
        self.timestamp = timestamp
        (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()

    def update(self, gyro, accel, timestamp):
        if self.gyro != gyro or self.accel != accel:
            delta_angle = gyro * (timestamp - self.timestamp)
            self.timestamp = timestamp
            self.dcm.rotate(delta_angle)
            self.dcm.normalize()
            self.gyro = gyro
            self.accel = accel
            (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()

px4_state = None

def PX4_update(IMU, ATT):
    '''implement full DCM using PX4 native SD log data'''
    global px4_state
    if px4_state is None:
        px4_state = PX4_State(degrees(ATT.Roll), degrees(ATT.Pitch), degrees(ATT.Yaw), IMU._timestamp)

    gyro  = Vector3(IMU.GyroX, IMU.GyroY, IMU.GyroZ)
    accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    px4_state.update(gyro, accel, IMU._timestamp)
    return px4_state

_downsample_N = 0

def downsample(N):
    '''conditional that is true on every Nth sample'''
    global _downsample_N
    _downsample_N = (_downsample_N + 1) % N
    return _downsample_N == 0

def armed(HEARTBEAT):
    '''return 1 if armed, 0 if not'''
    from . import mavutil
    if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS:
        self = mavutil.mavfile_global
        if self.motors_armed():
            return 1
        return 0
    if HEARTBEAT.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
        return 1
    return 0

def rotation_df(ATT):
    '''return the current DCM rotation matrix'''
    r = Matrix3()
    r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw))
    return r

def rotation2(AHRS2):
    '''return the current DCM rotation matrix'''
    r = Matrix3()
    r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw)
    return r

def earth_accel2(RAW_IMU,ATTITUDE):
    '''return earth frame acceleration vector from AHRS2'''
    r = rotation2(ATTITUDE)
    accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001
    return r * accel

def earth_accel_df(IMU,ATT):
    '''return earth frame acceleration vector from df log'''
    r = rotation_df(ATT)
    accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    return r * accel

def earth_accel2_df(IMU,IMU2,ATT):
    '''return earth frame acceleration vector from df log'''
    r = rotation_df(ATT)
    accel1 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    accel2 = Vector3(IMU2.AccX, IMU2.AccY, IMU2.AccZ)
    accel = 0.5 * (accel1 + accel2)
    return r * accel

def gps_velocity_df(GPS):
    '''return GPS velocity vector'''
    vx = GPS.Spd * cos(radians(GPS.GCrs))
    vy = GPS.Spd * sin(radians(GPS.GCrs))
    return Vector3(vx, vy, GPS.VZ)

def distance_gps2(GPS, GPS2):
    '''distance between two points'''
    if GPS.TimeMS != GPS2.TimeMS:
        # reject messages not time aligned
        return None
    return distance_two(GPS, GPS2)

# SI base unit for 1 Rgeo / equatorial radius
radius_of_earth = 6378100.0 # in meters

def wrap_valid_longitude(lon):
  ''' wrap a longitude value around to always have a value in the range
      [-180, +180) i.e 0 => 0, 1 => 1, -1 => -1, 181 => -179, -181 => 179
  '''
  return (((lon + 180.0) % 360.0) - 180.0)

def gps_newpos(lat, lon, bearing, distance):
  '''extrapolate latitude/longitude given a heading and distance
  thanks to http://www.movable-type.co.uk/scripts/latlong.html
  '''
  import math
  lat1 = math.radians(lat)
  lon1 = math.radians(lon)
  brng = math.radians(bearing)
  dr = distance/radius_of_earth

  lat2 = math.asin(math.sin(lat1)*math.cos(dr) +
                   math.cos(lat1)*math.sin(dr)*math.cos(brng))
  lon2 = lon1 + math.atan2(math.sin(brng)*math.sin(dr)*math.cos(lat1),
                           math.cos(dr)-math.sin(lat1)*math.sin(lat2))
  return (math.degrees(lat2), wrap_valid_longitude(math.degrees(lon2)))

def gps_offset(lat, lon, east, north):
  '''return new lat/lon after moving east/north
  by the given number of meters'''
  import math
  bearing = math.degrees(math.atan2(east, north))
  distance = math.sqrt(east**2 + north**2)
  return gps_newpos(lat, lon, bearing, distance)

ekf_origin = None

def ekf1_pos(EKF1):
  '''calculate EKF position when EKF disabled'''
  global ekf_origin
  from . import mavutil
  self = mavutil.mavfile_global
  if ekf_origin is None:
      if not 'ORGN' in self.messages:
          return None
      ekf_origin = self.messages['ORGN']
      (ekf_origin.Lat, ekf_origin.Lng) = (ekf_origin.Lat, ekf_origin.Lng)
  (lat,lon) = gps_offset(ekf_origin.Lat, ekf_origin.Lng, EKF1.PE, EKF1.PN)
  alt = ekf_origin.Alt - EKF1.PD
  return (lat, lon, alt)

def quat_to_euler(q):
  '''
  Get Euler angles from a quaternion
  :param q: quaternion [w, x, y , z]
  :returns: euler angles [roll, pitch, yaw]
  '''
  quat = Quaternion(q)
  return quat.euler

def euler_to_quat(e):
  '''
  Get quaternion from euler angles
  :param e: euler angles [roll, pitch, yaw]
  :returns: quaternion [w, x, y , z]
  '''
  quat = Quaternion(e)
  return quat.q

def rotate_quat(attitude, roll, pitch, yaw):
  '''
  Returns rotated quaternion
  :param attitude: quaternion [w, x, y , z]
  :param roll: rotation in rad
  :param pitch: rotation in rad
  :param yaw: rotation in rad
  :returns: quaternion [w, x, y , z]
  '''
  quat = Quaternion(attitude)
  rotation = Quaternion([roll, pitch, yaw])
  res = rotation * quat

  return res.q

def qroll(MSG):
    '''return quaternion roll in degrees'''
    q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4])
    return degrees(q.euler[0])

    
def qpitch(MSG):
    '''return quaternion pitch in degrees'''
    q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4])
    return degrees(q.euler[1])

    
def qyaw(MSG):
    '''return quaternion yaw in degrees'''
    q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4])
    return degrees(q.euler[2])

def euler_rotated(MSG, roll, pitch, yaw):
    '''return eulers in radians from quaternion for view at given attitude in euler radians'''
    rot_view = Matrix3()
    rot_view.from_euler(roll, pitch, yaw)
    q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4])
    dcm = (rot_view * q.dcm.transposed()).transposed()
    return dcm.to_euler()

def euler_p90(MSG):
    '''return eulers in radians from quaternion for view at pitch 90'''
    return euler_rotated(MSG, 0, radians(90), 0);

def qroll_p90(MSG):
    '''return quaternion roll in degrees for view at pitch 90'''
    return degrees(euler_p90(MSG)[0])

def qpitch_p90(MSG):
    '''return quaternion roll in degrees for view at pitch 90'''
    return degrees(euler_p90(MSG)[1])

def qyaw_p90(MSG):
    '''return quaternion roll in degrees for view at pitch 90'''
    return degrees(euler_p90(MSG)[2])


def rotation_df(ATT):
    '''return the current DCM rotation matrix'''
    r = Matrix3()
    r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw))
    return r

def rotation2(AHRS2):
    '''return the current DCM rotation matrix'''
    r = Matrix3()
    r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw)
    return r

def earth_accel2(RAW_IMU,ATTITUDE):
    '''return earth frame acceleration vector from AHRS2'''
    r = rotation2(ATTITUDE)
    accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001
    return r * accel

def earth_accel_df(IMU,ATT):
    '''return earth frame acceleration vector from df log'''
    r = rotation_df(ATT)
    accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    return r * accel

def earth_accel2_df(IMU,IMU2,ATT):
    '''return earth frame acceleration vector from df log'''
    r = rotation_df(ATT)
    accel1 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
    accel2 = Vector3(IMU2.AccX, IMU2.AccY, IMU2.AccZ)
    accel = 0.5 * (accel1 + accel2)
    return r * accel

def gps_velocity_df(GPS):
    '''return GPS velocity vector'''
    vx = GPS.Spd * cos(radians(GPS.GCrs))
    vy = GPS.Spd * sin(radians(GPS.GCrs))
    return Vector3(vx, vy, GPS.VZ)

def armed(HEARTBEAT):
    '''return 1 if armed, 0 if not'''
    from pymavlink import mavutil
    if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS:
        self = mavutil.mavfile_global
        if self.motors_armed():
            return 1
        return 0
    if HEARTBEAT.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
        return 1
    return 0

def mode(HEARTBEAT):
    '''return flight mode number'''
    from pymavlink import mavutil
    if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS:
        return None
    return HEARTBEAT.custom_mode


'''
    magnetic field tables for estimating earths mag field
    updated 2019-12-20
'''

# set to the sampling in degrees for the table below
SAMPLING_RES = 10.0
SAMPLING_MIN_LAT = -90.0
SAMPLING_MAX_LAT = 90.0
SAMPLING_MIN_LON = -180.0
SAMPLING_MAX_LON = 180.0

declination_table = [
    [148.83402,138.83401,128.83401,118.83402,108.83402,98.83402,88.83402,78.83402,68.83402,58.83402,48.83402,38.83402,28.83402,18.83402,8.83402,-1.16598,-11.16598,-21.16598,-31.16598,-41.16598,-51.16598,-61.16598,-71.16598,-81.16598,-91.16598,-101.16598,-111.16598,-121.16598,-131.16598,-141.16598,-151.16598,-161.16598,-171.16598,178.83402,168.83402,158.83402,148.83402],
    [129.09306,116.89412,105.78898,95.63017,86.23504,77.42476,69.04348,60.96591,53.09841,45.37592,37.75568,30.20867,22.70998,15.23027,7.73037,0.16098,-7.53238,-15.40109,-23.48496,-31.80703,-40.37375,-49.18062,-58.22152,-67.49946,-77.03640,-86.88079,-97.11212,-107.84156,-119.20626,-131.35191,-144.39481,-158.35719,-173.08769,171.78274,156.78187,142.43310,129.09306],
    [85.81367,77.83602,71.40003,65.88433,60.88263,56.08204,51.22755,46.12774,40.67419,34.85457,28.74909,22.50583,16.29363,10.23812,4.36029,-1.45095,-7.40778,-13.74178,-20.61213,-28.04922,-35.95530,-44.15322,-52.45486,-60.71951,-68.88732,-76.99183,-85.16746,-93.67473,-102.97950,-113.96429,-128.46293,-150.36073,175.55488,138.00554,112.28051,96.48319,85.81367],
    [48.22805,46.81921,45.24300,43.71189,42.27245,40.80022,39.00177,36.49549,32.95972,28.26545,22.54379,16.18846,9.77818,3.88971,-1.16066,-5.50500,-9.68832,-14.40052,-20.13990,-26.99385,-34.63319,-42.50584,-50.08478,-57.00760,-63.07978,-68.20773,-72.30336,-75.14845,-76.14220,-73.56875,-61.46573,-19.66028,28.39616,43.98783,48.35027,49.03265,48.22805],
    [31.42931,31.60530,31.28145,30.79047,30.38024,30.16777,29.97487,29.25597,27.27948,23.46202,17.66378,10.37176,2.68673,-4.06559,-9.01317,-12.19989,-14.47861,-17.04830,-21.00672,-26.83158,-33.98769,-41.32654,-47.85214,-52.97945,-56.36644,-57.72068,-56.62201,-52.29959,-43.64045,-30.08348,-13.41611,2.35315,14.41455,22.53506,27.54777,30.28028,31.42931],
    [22.67985,23.21169,23.25494,22.99742,22.63545,22.42369,22.46877,22.43187,21.42571,18.33186,12.46440,4.21692,-4.75068,-12.30560,-17.22721,-19.74039,-20.77499,-21.16946,-22.14607,-25.39336,-31.01102,-37.19196,-42.26227,-45.34969,-45.95366,-43.77299,-38.67388,-30.72261,-20.87969,-11.16390,-2.84711,4.17466,10.15622,15.07307,18.83144,21.33469,22.67985],
    [17.07334,17.59062,17.77934,17.69577,17.34929,16.88674,16.56217,16.38656,15.63204,12.86517,6.93562,-1.75751,-10.98436,-18.20308,-22.41588,-24.27993,-24.67590,-23.50719,-21.00605,-19.72369,-21.87960,-26.21214,-30.23195,-32.25069,-31.49417,-28.07385,-22.58093,-15.67611,-8.73630,-3.38117,0.43868,3.90117,7.49078,10.92679,13.85484,15.93237,17.07334],
    [13.37426,13.67217,13.78744,13.78998,13.54222,12.98780,12.38852,11.96065,11.09372,8.25499,2.21162,-6.34723,-14.82596,-20.89032,-23.89434,-24.45752,-23.26867,-20.14461,-15.21492,-10.62227,-9.09952,-11.18829,-14.91638,-17.58833,-17.75073,-15.65321,-12.03643,-7.38772,-2.92537,-0.15156,1.30857,3.04660,5.56513,8.30463,10.76538,12.51642,13.37426],
    [11.10969,11.11944,11.01236,10.99728,10.84097,10.32286,9.70494,9.19687,8.09157,4.94277,-1.12864,-9.05484,-16.32980,-21.04376,-22.55109,-21.19496,-17.89373,-13.44411,-8.58084,-4.24867,-1.62827,-1.86509,-4.55045,-7.42469,-8.64775,-8.12536,-6.32716,-3.48850,-0.64489,0.72027,0.97976,1.90952,4.01623,6.51266,8.80749,10.44185,11.10969],
    [9.88349,9.72040,9.41289,9.38439,9.33291,8.89921,8.31537,7.68011,6.17272,2.60695,-3.33977,-10.37899,-16.38823,-19.72660,-19.70115,-16.80477,-12.41185,-7.91607,-4.15177,-1.10381,1.22204,1.82879,0.23517,-2.13784,-3.62813,-3.94630,-3.34494,-1.85961,-0.22644,0.26762,-0.08803,0.44077,2.38763,4.89878,7.32401,9.15291,9.88349],
    [9.12464,9.18177,8.94457,9.04553,9.19618,8.89744,8.21223,7.14260,4.96349,0.86055,-4.97931,-11.14357,-15.85640,-17.78532,-16.57645,-13.08478,-8.67898,-4.59243,-1.60427,0.55144,2.39028,3.23916,2.31654,0.46434,-0.91732,-1.53003,-1.62589,-1.18564,-0.62403,-0.86624,-1.66562,-1.50145,0.21812,2.79294,5.56329,7.91823,9.12464],
    [8.06290,8.93172,9.29767,9.81684,10.30693,10.20311,9.30721,7.53634,4.41412,-0.45099,-6.41113,-11.85029,-15.25565,-15.88209,-13.96742,-10.49677,-6.46933,-2.75504,-0.08743,1.66921,3.12874,3.96903,3.45981,2.05876,0.88578,0.20951,-0.29117,-0.68669,-1.18101,-2.27487,-3.63054,-3.95562,-2.62221,-0.08793,3.01927,6.01454,8.06290],
    [6.31041,8.41617,9.93058,11.21483,12.13996,12.23034,11.14011,8.66524,4.49928,-1.34733,-7.71989,-12.71272,-15.08305,-14.75641,-12.45343,-9.09705,-5.35725,-1.83470,0.82823,2.57001,3.86519,4.69588,4.60243,3.76104,2.87601,2.13967,1.24833,0.02788,-1.62859,-3.80231,-5.90990,-6.79346,-5.87079,-3.45535,-0.15720,3.34006,6.31041],
    [4.26294,7.59975,10.43666,12.70491,14.16020,14.43398,13.18770,10.10768,4.90586,-2.08386,-9.15208,-14.09196,-15.93051,-15.08268,-12.51517,-9.09089,-5.35759,-1.77919,1.14514,3.24292,4.77489,5.90082,6.46683,6.42854,5.95440,5.04613,3.49019,1.14215,-1.96091,-5.46021,-8.43365,-9.79880,-9.13330,-6.77311,-3.34752,0.50970,4.26294],
    [2.57464,6.81988,10.72127,13.95370,16.10049,16.72460,15.39548,11.64478,5.15172,-3.36781,-11.49173,-16.72206,-18.40831,-17.31430,-14.50140,-10.79977,-6.74992,-2.76545,0.78143,3.70100,6.08013,8.07892,9.67963,10.67456,10.77058,9.65347,7.07600,3.02043,-2.09809,-7.27683,-11.16806,-12.81861,-12.13043,-9.62141,-5.97603,-1.77704,2.57464],
    [1.43503,6.27921,10.88019,14.88261,17.80063,19.02158,17.77297,13.12636,4.53718,-6.54055,-16.27050,-21.85552,-23.31365,-21.85455,-18.61101,-14.36797,-9.64057,-4.79964,-0.13239,4.18385,8.10605,11.64519,14.70734,16.97990,17.92977,16.88820,13.23164,6.81146,-1.34665,-8.97876,-14.01257,-15.81005,-14.85926,-12.00033,-7.99733,-3.40265,1.43502],
    [0.13094,5.45479,10.54962,15.08235,18.55436,20.17754,18.67574,12.20150,-0.36226,-15.49225,-26.46004,-31.19380,-31.33251,-28.66356,-24.33699,-19.03627,-13.18344,-7.06335,-0.88475,5.19490,11.05071,16.55252,21.50321,25.56424,28.16988,28.41286,24.95220,16.43585,3.63861,-8.67079,-16.18909,-18.69045,-17.68753,-14.54612,-10.18137,-5.16795,0.13094],
    [-4.14830,1.12345,5.96040,9.84415,11.94596,10.80871,4.02869,-10.36405,-27.94912,-39.79617,-44.16477,-43.57950,-40.12657,-34.99189,-28.83083,-22.02403,-14.80922,-7.34799,0.23881,7.85050,15.39197,22.75942,29.82156,36.38924,42.15868,46.59144,48.63821,46.09967,34.72041,12.13936,-8.98865,-18.65098,-20.47841,-18.40133,-14.39834,-9.45818,-4.14830],
    [-169.79948,-159.79948,-149.79948,-139.79948,-129.79948,-119.79948,-109.79948,-99.79948,-89.79948,-79.79948,-69.79948,-59.79948,-49.79948,-39.79948,-29.79948,-19.79948,-9.79948,0.20052,10.20052,20.20052,30.20052,40.20052,50.20052,60.20052,70.20052,80.20052,90.20052,100.20052,110.20052,120.20052,130.20052,140.20052,150.20052,160.20052,170.20052,-179.79948,-169.79948]
]

inclination_table = [
    [-72.02070,-72.02071,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02071,-72.02070,-72.02071],
    [-78.23208,-77.46612,-76.54604,-75.51344,-74.40408,-73.25043,-72.08420,-70.93779,-69.84384,-68.83332,-67.93241,-67.15960,-66.52407,-66.02648,-65.66205,-65.42519,-65.31392,-65.33255,-65.49161,-65.80511,-66.28626,-66.94289,-67.77395,-68.76776,-69.90201,-71.14489,-72.45664,-73.79090,-75.09549,-76.31306,-77.38240,-78.24183,-78.83644,-79.12876,-79.10879,-78.79618,-78.23208],
    [-80.80007,-78.97830,-77.14756,-75.29021,-73.38193,-71.40769,-69.37743,-67.33857,-65.37896,-63.61481,-62.16302,-61.10460,-60.45316,-60.14557,-60.06470,-60.08698,-60.13217,-60.19219,-60.32958,-60.65151,-61.27183,-62.27481,-63.69257,-65.50302,-67.64474,-70.03800,-72.60073,-75.25455,-77.92173,-80.51496,-82.91263,-84.88736,-85.95658,-85.63251,-84.30522,-82.60297,-80.80007],
    [-77.45516,-75.43606,-73.49604,-71.57699,-69.59653,-67.45749,-65.08505,-62.48902,-59.81600,-57.35291,-55.46368,-54.46054,-54.44855,-55.23685,-56.40404,-57.49090,-58.18917,-58.42457,-58.34278,-58.24952,-58.51231,-59.42525,-61.10217,-63.46936,-66.34503,-69.53469,-72.88656,-76.29320,-79.66218,-82.88130,-85.73520,-87.38955,-86.36194,-84.15583,-81.83294,-79.58616,-77.45517],
    [-71.58214,-69.61565,-67.71488,-65.86479,-64.01505,-62.04510,-59.77482,-57.07400,-54.02405,-51.03212,-48.80491,-48.09823,-49.26412,-51.92460,-55.16562,-58.07793,-60.10267,-61.01513,-60.84749,-59.97822,-59.15799,-59.18575,-60.45940,-62.86069,-65.97595,-69.36736,-72.71868,-75.80268,-78.35851,-80.06262,-80.67983,-80.25871,-79.08901,-77.46953,-75.59576,-73.59787,-71.58215],
    [-64.39807,-62.40221,-60.40960,-58.42549,-56.48151,-54.55795,-52.49863,-50.03954,-47.03044,-43.80319,-41.39118,-41.16169,-43.78688,-48.51954,-53.83971,-58.60413,-62.35170,-64.81766,-65.60605,-64.62579,-62.64650,-61.08659,-61.05364,-62.68316,-65.34905,-68.24163,-70.78236,-72.62117,-73.51797,-73.52965,-73.02048,-72.24528,-71.21594,-69.88980,-68.25886,-66.38361,-64.39807],
    [-55.02826,-52.87120,-50.69073,-48.44731,-46.19513,-44.04849,-42.00465,-39.74123,-36.79801,-33.33110,-30.77621,-31.26765,-35.68330,-42.58861,-49.78570,-56.05256,-61.22645,-65.24992,-67.52021,-67.43115,-65.26549,-62.35447,-60.45735,-60.49007,-61.96843,-63.81210,-65.26064,-65.90429,-65.53604,-64.51130,-63.52424,-62.77292,-61.94296,-60.76846,-59.16172,-57.17787,-55.02826],
    [-42.23934,-39.72248,-37.30606,-34.84704,-32.29950,-29.84715,-27.62882,-25.25073,-22.02346,-18.13398,-15.56621,-17.09131,-23.52381,-32.78684,-42.11183,-49.91489,-55.93881,-60.34177,-62.85919,-63.04596,-60.93631,-57.41244,-54.21948,-52.82417,-53.10885,-54.02972,-54.81946,-54.92826,-53.94911,-52.38029,-51.27875,-50.79557,-50.20937,-49.05107,-47.23568,-44.84941,-42.23934],
    [-25.31616,-22.26353,-19.64432,-17.13765,-14.48794,-11.90452,-9.56551,-6.93532,-3.35246,0.62534,2.69744,0.20162,-7.49683,-18.49333,-29.80260,-38.99555,-45.22087,-48.81124,-50.28975,-49.88122,-47.49942,-43.57896,-39.80201,-37.81589,-37.58213,-38.12733,-38.79073,-38.91636,-37.84560,-36.15384,-35.28930,-35.35164,-35.11620,-33.90633,-31.71407,-28.67497,-25.31616],
    [-5.22365,-1.68002,0.98130,3.28459,5.73141,8.13291,10.33797,12.92226,16.28019,19.52644,20.64919,17.75076,10.26968,-0.68527,-12.39654,-21.81573,-27.55757,-29.98547,-30.24089,-29.18412,-26.62224,-22.51726,-18.50023,-16.36665,-16.02599,-16.47452,-17.18522,-17.56335,-16.82114,-15.49276,-15.21405,-16.04947,-16.38744,-15.33409,-12.92178,-9.31548,-5.22365],
    [14.65727,18.24457,20.73560,22.68750,24.74090,26.83478,28.82717,31.06453,33.64927,35.73650,35.91607,33.06931,26.82267,17.82545,8.14938,0.35249,-4.22224,-5.65846,-5.08244,-3.67265,-1.30171,2.36894,6.00997,7.96495,8.31670,8.01335,7.45026,6.96852,7.19717,7.76654,7.31456,5.77666,4.68883,5.07867,7.05547,10.50432,14.65727],
    [31.00203,34.03206,36.22486,37.93041,39.71929,41.67672,43.64451,45.63057,47.51149,48.64179,48.13476,45.46014,40.64785,34.34474,27.90343,22.77471,19.78522,19.08118,19.96093,21.39191,23.28877,25.96005,28.61507,30.09860,30.42073,30.29938,30.04062,29.74069,29.65970,29.53545,28.53162,26.62276,24.91817,24.37385,25.31100,27.72542,31.00203],
    [43.33608,45.45851,47.28808,48.92364,50.69187,52.68920,54.75852,56.70372,58.24993,58.87748,58.03709,55.61656,52.01961,47.93440,44.13769,41.24459,39.61639,39.39065,40.22833,41.47069,42.89022,44.59331,46.23881,47.24171,47.57154,47.63935,47.66249,47.62919,47.50096,47.00810,45.71114,43.68014,41.65440,40.38784,40.27252,41.37670,43.33608],
    [53.10131,54.36161,55.82606,57.45873,59.32146,61.39014,63.50628,65.43734,66.85905,67.31408,66.44780,64.38450,61.68268,58.96721,56.68844,55.08044,54.25566,54.24982,54.89128,55.82780,56.83438,57.87845,58.85920,59.59271,60.06042,60.40887,60.71534,60.90716,60.81057,60.15911,58.74748,56.73559,54.64618,53.02448,52.20550,52.28132,53.10131],
    [61.90363,62.59082,63.73000,65.25974,67.10373,69.13404,71.16923,72.97878,74.24745,74.58969,73.79370,72.08739,69.99806,68.01780,66.44167,65.38391,64.86230,64.84287,65.21557,65.80372,66.46017,67.12970,67.80720,68.49179,69.18909,69.89166,70.52514,70.92481,70.86509,70.14456,68.73300,66.86155,64.92449,63.29260,62.20331,61.74779,61.90363],
    [70.57319,70.97549,71.81891,73.05700,74.60513,76.33503,78.07584,79.60158,80.60606,80.76720,79.99878,78.58008,76.92858,75.37566,74.11340,73.22087,72.70518,72.52979,72.62677,72.91213,73.31657,73.81370,74.42101,75.17239,76.07666,77.07532,78.01176,78.63384,78.66938,77.98598,76.70086,75.10445,73.50638,72.14451,71.16426,70.63412,70.57319],
    [78.82351,79.06440,79.60559,80.41612,81.44382,82.60991,83.79747,84.82323,85.41014,85.29919,84.52961,83.39228,82.15604,80.99185,79.99787,79.22370,78.68527,78.37555,78.27394,78.35635,78.60546,79.01788,79.60326,80.37283,81.31875,82.38760,83.44917,84.26774,84.54142,84.12292,83.18704,82.03917,80.91872,79.97342,79.28584,78.89722,78.82351],
    [85.92404,85.99807,86.21370,86.55320,86.98729,87.46743,87.90712,88.15645,88.05609,87.61718,86.98684,86.28678,85.58976,84.94098,84.37090,83.90045,83.54357,83.30879,83.20077,83.22179,83.37274,83.65354,84.06234,84.59369,85.23581,85.96691,86.74942,87.51716,88.14109,88.39226,88.15089,87.63861,87.08412,86.59723,86.22810,86.00055,85.92404],
    [88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420]
]

intensity_table = [
    [0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507],
    [0.60562,0.59923,0.59133,0.58213,0.57184,0.56070,0.54892,0.53679,0.52458,0.51262,0.50121,0.49067,0.48128,0.47330,0.46697,0.46249,0.46006,0.45986,0.46203,0.46665,0.47372,0.48312,0.49460,0.50778,0.52217,0.53717,0.55217,0.56652,0.57964,0.59102,0.60027,0.60715,0.61152,0.61342,0.61294,0.61026,0.60562],
    [0.62993,0.61663,0.60163,0.58513,0.56721,0.54792,0.52741,0.50597,0.48417,0.46274,0.44252,0.42424,0.40840,0.39524,0.38483,0.37722,0.37262,0.37149,0.37444,0.38211,0.39496,0.41303,0.43591,0.46271,0.49220,0.52288,0.55321,0.58160,0.60660,0.62705,0.64217,0.65166,0.65569,0.65477,0.64966,0.64113,0.62993],
    [0.61859,0.59954,0.57951,0.55859,0.53652,0.51281,0.48703,0.45921,0.43010,0.40125,0.37460,0.35184,0.33383,0.32034,0.31043,0.30313,0.29816,0.29624,0.29887,0.30789,0.32478,0.35003,0.38290,0.42162,0.46385,0.50716,0.54918,0.58755,0.62009,0.64502,0.66138,0.66918,0.66921,0.66276,0.65127,0.63614,0.61859],
    [0.58434,0.56134,0.53820,0.51511,0.49174,0.46715,0.44010,0.40984,0.37699,0.34382,0.31364,0.28967,0.27359,0.26453,0.25966,0.25610,0.25262,0.24997,0.25053,0.25779,0.27518,0.30435,0.34417,0.39141,0.44206,0.49253,0.54000,0.58194,0.61593,0.64015,0.65395,0.65794,0.65353,0.64245,0.62626,0.60641,0.58434],
    [0.53921,0.51448,0.48995,0.46595,0.44250,0.41888,0.39359,0.36521,0.33366,0.30097,0.27115,0.24895,0.23726,0.23477,0.23677,0.23885,0.23920,0.23803,0.23691,0.23986,0.25296,0.28060,0.32245,0.37391,0.42855,0.48085,0.52753,0.56643,0.59562,0.61429,0.62316,0.62341,0.61626,0.60311,0.58508,0.56327,0.53921],
    [0.48785,0.46379,0.43988,0.41636,0.39367,0.37182,0.34998,0.32674,0.30099,0.27346,0.24780,0.22959,0.22251,0.22507,0.23198,0.23916,0.24558,0.25057,0.25278,0.25389,0.26060,0.28058,0.31692,0.36595,0.41928,0.46891,0.51065,0.54231,0.56238,0.57214,0.57493,0.57236,0.56426,0.55095,0.53309,0.51149,0.48785],
    [0.43209,0.41089,0.38995,0.36941,0.34979,0.33155,0.31470,0.29826,0.28044,0.26067,0.24164,0.22828,0.22418,0.22862,0.23770,0.24867,0.26122,0.27397,0.28296,0.28650,0.28895,0.29863,0.32271,0.36133,0.40653,0.44908,0.48374,0.50731,0.51774,0.51826,0.51530,0.51075,0.50252,0.48985,0.47321,0.45336,0.43209],
    [0.37890,0.36283,0.34739,0.33271,0.31922,0.30724,0.29696,0.28780,0.27809,0.26658,0.25432,0.24439,0.23994,0.24247,0.25117,0.26398,0.27930,0.29517,0.30771,0.31403,0.31546,0.31823,0.33068,0.35590,0.38846,0.42051,0.44683,0.46322,0.46680,0.46142,0.45465,0.44832,0.43950,0.42721,0.41228,0.39568,0.37890],
    [0.34114,0.33192,0.32348,0.31616,0.31061,0.30672,0.30410,0.30225,0.29980,0.29492,0.28700,0.27755,0.26956,0.26653,0.27083,0.28123,0.29446,0.30791,0.31935,0.32678,0.32987,0.33216,0.33985,0.35562,0.37648,0.39779,0.41572,0.42632,0.42691,0.42012,0.41134,0.40209,0.39087,0.37775,0.36429,0.35181,0.34114],
    [0.32818,0.32516,0.32319,0.32283,0.32517,0.32980,0.33535,0.34062,0.34397,0.34296,0.33615,0.32475,0.31211,0.30265,0.30010,0.30443,0.31265,0.32241,0.33228,0.34067,0.34700,0.35327,0.36224,0.37416,0.38773,0.40158,0.41356,0.42060,0.42076,0.41455,0.40362,0.38928,0.37303,0.35700,0.34334,0.33369,0.32818],
    [0.33981,0.34000,0.34266,0.34809,0.35721,0.36937,0.38240,0.39400,0.40180,0.40293,0.39568,0.38157,0.36502,0.35110,0.34343,0.34233,0.34605,0.35337,0.36304,0.37282,0.38187,0.39163,0.40273,0.41394,0.42487,0.43601,0.44625,0.45294,0.45399,0.44803,0.43420,0.41393,0.39124,0.37028,0.35389,0.34377,0.33981],
    [0.37236,0.37289,0.37826,0.38816,0.40237,0.41956,0.43724,0.45274,0.46333,0.46592,0.45864,0.44316,0.42447,0.40808,0.39741,0.39283,0.39339,0.39860,0.40735,0.41725,0.42701,0.43745,0.44902,0.46095,0.47308,0.48570,0.49764,0.50631,0.50895,0.50305,0.48719,0.46316,0.43591,0.41060,0.39068,0.37789,0.37236],
    [0.42245,0.42215,0.42846,0.44064,0.45724,0.47601,0.49435,0.50993,0.52038,0.52304,0.51631,0.50148,0.48288,0.46550,0.45264,0.44515,0.44269,0.44488,0.45083,0.45875,0.46742,0.47714,0.48867,0.50225,0.51759,0.53373,0.54863,0.55948,0.56339,0.55795,0.54232,0.51859,0.49131,0.46537,0.44426,0.42977,0.42245],
    [0.48319,0.48226,0.48758,0.49842,0.51308,0.52913,0.54416,0.55622,0.56358,0.56459,0.55836,0.54575,0.52954,0.51323,0.49956,0.48985,0.48445,0.48324,0.48569,0.49085,0.49796,0.50710,0.51894,0.53390,0.55144,0.56981,0.58634,0.59810,0.60259,0.59827,0.58527,0.56571,0.54318,0.52144,0.50331,0.49032,0.48319],
    [0.53929,0.53799,0.54070,0.54691,0.55552,0.56497,0.57363,0.58010,0.58328,0.58236,0.57702,0.56775,0.55586,0.54317,0.53137,0.52173,0.51505,0.51167,0.51159,0.51458,0.52039,0.52900,0.54059,0.55506,0.57159,0.58845,0.60334,0.61392,0.61846,0.61626,0.60790,0.59512,0.58032,0.56589,0.55363,0.54459,0.53929],
    [0.57278,0.57090,0.57068,0.57187,0.57403,0.57656,0.57878,0.58007,0.57991,0.57795,0.57409,0.56849,0.56162,0.55414,0.54681,0.54040,0.53554,0.53271,0.53223,0.53423,0.53873,0.54564,0.55475,0.56559,0.57735,0.58891,0.59900,0.60643,0.61040,0.61066,0.60758,0.60203,0.59514,0.58801,0.58156,0.57637,0.57278],
    [0.57900,0.57728,0.57584,0.57463,0.57360,0.57263,0.57160,0.57039,0.56890,0.56707,0.56489,0.56241,0.55976,0.55710,0.55465,0.55263,0.55127,0.55074,0.55121,0.55273,0.55530,0.55883,0.56314,0.56798,0.57300,0.57785,0.58216,0.58562,0.58802,0.58928,0.58943,0.58865,0.58716,0.58523,0.58309,0.58097,0.57900],
    [0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830]
]


def interpolate_table(table, latitude_deg, longitude_deg):
    '''interpolate inside a table for a given lat/lon in degrees'''
    # round down to nearest sampling resolution
    min_lat = int(floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
    min_lon = int(floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)

    # find index of nearest low sampling point
    min_lat_index = int(floor(-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES)
    min_lon_index = int(floor(-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES)

    # calculate intensity
    data_sw = table[min_lat_index][min_lon_index]
    data_se = table[min_lat_index][min_lon_index + 1]
    data_ne = table[min_lat_index + 1][min_lon_index + 1]
    data_nw = table[min_lat_index + 1][min_lon_index]

    # perform bilinear interpolation on the four grid corners
    data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
    data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw

    value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
    return value

earth_declination = None

'''
calculate magnetic field intensity and orientation, interpolating in tables

returns array [declination_deg, inclination_deg, intensity] or None
'''
def get_mag_field_ef(latitude_deg, longitude_deg):
    # limit to table bounds
    if latitude_deg < SAMPLING_MIN_LAT:
        return None
    if latitude_deg >= SAMPLING_MAX_LAT:
        return None
    if longitude_deg < SAMPLING_MIN_LON:
        return None
    if longitude_deg >= SAMPLING_MAX_LON:
        return None

    intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
    declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
    inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)

    global earth_declination
    earth_declination = declination_deg

    return [declination_deg, inclination_deg, intensity_gauss]


def expected_earth_field_lat_lon(lat, lon):
    '''return expected magnetic field for a location'''
    field_var = get_mag_field_ef(lat, lon)
    mag_ef = Vector3(field_var[2]*1000.0, 0.0, 0.0)
    R = Matrix3()
    R.from_euler(0.0, -radians(field_var[1]), radians(field_var[0]))
    mag_ef = R * mag_ef
    earth_field = mag_ef
    return earth_field

def get_lat_lon_status(GPS):
    '''get lat, lon and status from a GPS message'''
    if hasattr(GPS,'fix_type'):
        gps_status = GPS.fix_type
        lat = GPS.lat*1.0e-7
        lon = GPS.lon*1.0e-7
    else:
        gps_status = GPS.Status
        lat = GPS.Lat
        lon = GPS.Lng
    return (lat, lon, gps_status)

def expected_earth_field(GPS):
    '''return expected magnetic field for a location'''
    (lat, lon, gps_status) = get_lat_lon_status(GPS)

    if gps_status < 3:
        return Vector3(0,0,0)
    return expected_earth_field_lat_lon(lat, lon)


def expected_mag(GPS,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
    '''return expected magnetic field for a location and attitude'''
    earth_field = expected_earth_field(GPS)
    if earth_field is None:
        return Vector3(0,0,0)

    if hasattr(ATT,'roll'):
        roll = degrees(ATT.roll)+roll_adjust
        pitch = degrees(ATT.pitch)+pitch_adjust
        yaw = degrees(ATT.yaw)+yaw_adjust
    else:
        roll = ATT.Roll+roll_adjust
        pitch = ATT.Pitch+pitch_adjust
        yaw = ATT.Yaw+yaw_adjust

    rot = Matrix3()
    rot.from_euler(radians(roll), radians(pitch), radians(yaw))

    field = rot.transposed() * earth_field

    return field

def expected_mag_latlon(lat,lon,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
    '''return expected magnetic field for a location and attitude'''
    earth_field = expected_earth_field_lat_lon(lat,lon)
    if earth_field is None:
        return Vector3(0,0,0)

    if hasattr(ATT,'roll'):
        roll = degrees(ATT.roll)+roll_adjust
        pitch = degrees(ATT.pitch)+pitch_adjust
        yaw = degrees(ATT.yaw)+yaw_adjust
    else:
        roll = ATT.Roll+roll_adjust
        pitch = ATT.Pitch+pitch_adjust
        yaw = ATT.Yaw+yaw_adjust

    rot = Matrix3()
    rot.from_euler(radians(roll), radians(pitch), radians(yaw))

    field = rot.transposed() * earth_field

    return field

def mag_yaw(GPS,ATT,MAG):
    '''calculate heading from raw magnetometer'''
    ef = expected_earth_field(GPS)
    mag_x = MAG.MagX
    mag_y = MAG.MagY
    mag_z = MAG.MagZ

    # go via a DCM matrix to match the APM calculation
    dcm_matrix = rotation_df(ATT)
    cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
    headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y
    headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z)

    # we need the declination too
    (lat, lon, gps_status) = get_lat_lon_status(GPS)
    field = get_mag_field_ef(lat, lon)
    declination = field[0]
    heading = degrees(atan2(-headY,headX)) + declination
    if heading < 0:
        heading += 360
    return heading

def expected_mag_yaw(GPS,ATT,MAG,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
    '''return expected magnetic field for a location and attitude'''

    earth_field = expected_earth_field(GPS)

    roll = ATT.Roll+roll_adjust
    pitch = ATT.Pitch+pitch_adjust
    yaw = mag_yaw(GPS,ATT,MAG)

    rot = Matrix3()
    rot.from_euler(radians(roll), radians(pitch), radians(yaw))

    field = rot.transposed() * earth_field

    return field

def earth_field_error(GPS,NKF2):
    '''return vector error in earth field estimate'''
    earth_field = expected_earth_field(GPS)
    if earth_field is None:
        return Vector3(0,0,0)
    ef = Vector3(NKF2.MN,NKF2.ME,NKF2.MD)
    ret = ef - earth_field
    return ret


def distance_home_df(GPS,ORGN):
    '''distance from home origin'''
    return distance_two(GPS_RAW, first_fix)

def airspeed_estimate(GLOBAL_POSITION_INT,WIND):
    '''estimate airspeed'''
    wind = WIND
    gpi = GLOBAL_POSITION_INT
    from pymavlink.rotmat import Vector3
    import math
    wind3d = Vector3(wind.speed*math.cos(math.radians(wind.direction)),
                     wind.speed*math.sin(math.radians(wind.direction)), 0)
    ground = Vector3(gpi.vx*0.01, gpi.vy*0.01, 0)
    airspeed = (ground + wind3d).length()
    return airspeed


def distance_from(GPS_RAW1, lat, lon):
    '''
    Return the distance from a given location in meters
    Calculated as the great-circle distance using 'haversine’ formula
    (Ref: http://www.movable-type.co.uk/scripts/latlong.html)
    Uses the globally-average earth radius value of 6371km
    '''
    if hasattr(GPS_RAW1, 'Lat'):
        lat1 = radians(GPS_RAW1.Lat)
        lon1 = radians(GPS_RAW1.Lng)
    elif hasattr(GPS_RAW1, 'cog'):
        lat1 = radians(GPS_RAW1.lat)*1.0e-7
        lon1 = radians(GPS_RAW1.lon)*1.0e-7
    else:
        lat1 = radians(GPS_RAW1.lat)
        lon1 = radians(GPS_RAW1.lon)

    lat2 = radians(lat)
    lon2 = radians(lon)

    dLat = lat2 - lat1
    dLon = lon2 - lon1

    a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2)
    c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
    ground_dist = 6371 * 1000 * c
    return ground_dist

def distance_lat_lon(lat1, lon1, lat2, lon2):
    '''
    Return the distance between two points in metres
    Calculated as the great-circle distance using 'haversine’ formula
    (Ref: http://www.movable-type.co.uk/scripts/latlong.html)
    Uses the globally-average earth radius value of 6371km
    '''
    lat1 = radians(lat1)
    lon1 = radians(lon1)
    lat2 = radians(lat2)
    lon2 = radians(lon2)

    dLat = lat2 - lat1
    dLon = lon2 - lon1

    a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2)
    c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
    ground_dist = 6371 * 1000 * c
    return ground_dist

def constrain(v, minv, maxv):
    if v < minv:
        v = minv
    if v > maxv:
        v = maxv
    return v

def sim_body_rates(SIM):
    '''return body frame rates from simulator attitudes'''
    rollRate = delta(SIM.Roll,'sbr',SIM.TimeUS)
    pitchRate = delta(SIM.Pitch,'sbp',SIM.TimeUS)
    yawRate = delta(SIM.Yaw,'sby',SIM.TimeUS)
    phi = radians(SIM.Roll)
    theta = radians(SIM.Pitch)
    phiDot = radians(rollRate)
    thetaDot = radians(pitchRate)
    psiDot = radians(yawRate)

    p = phiDot - psiDot*sin(theta)
    q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
    r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
    return Vector3(p, q, r)

def reset_state_data():
    '''reset state data, used on log rewind'''
    global average_data
    global derivative_data
    global lowpass_data
    global last_diff
    global last_delta
    global first_fix
    global dcm_state
    global last_sum
    global last_integral
    average_data.clear()
    derivative_data.clear()
    lowpass_data.clear()
    last_delta.clear()
    last_sum.clear()
    last_integral.clear()
    first_fix = None
    dcm_state = None

# terrain functions, using MAVProxy elevation module
EleModel = None

def terrain_height(lat,lon):
    '''get terrain height'''
    global EleModel
    if EleModel is None:
        from MAVProxy.modules.mavproxy_map.mp_elevation import ElevationModel
        EleModel = ElevationModel("srtm",offline=1)
    return EleModel.GetElevation(lat,lon)

def terrain_margin_lat_lon(lat1,lon1,alt1,lat2,lon2,alt2):
    '''
    return minimum height above terrain on path between two positions (AMSL)
    '''
    distance = distance_lat_lon(lat1, lon1, lat2, lon2)
    steps = distance / 10
    dlat = (lat2-lat1) / steps
    dlon = (lon2-lon1) / steps
    dalt = (alt2-alt1) / steps
    min_margin = None

    for i in range(max(1,int(steps))):
        talt = terrain_height(lat1,lon1)
        margin = alt1 - talt
        if min_margin is None or margin < min_margin:
            min_margin = margin
        lat1 += dlat
        lon1 += dlon
        alt1 += dalt
    return min_margin

def terrain_margin(TERR,lat,lon,antenna_height):
    '''
    return minimum height above terrain on path between two positions (AMSL)
    '''
    alt = terrain_height(lat,lon)+antenna_height
    return terrain_margin_lat_lon(TERR.Lat,TERR.Lng,TERR.CHeight+TERR.TerrH,lat,lon,alt)

def radio_margin(TERR,lat,lon,antenna_height):
    '''
    return how much height we could lose and still have line of sight from an antenna at antenna_height
    above ground at lat/lon
    '''
    ant_alt = terrain_height(lat,lon)+antenna_height
    if hasattr(TERR,'CHeight'):
        alt = TERR.CHeight+TERR.TerrH
    else:
        # allow for GPS messages
        alt = TERR.Alt
    low = -alt
    high = 0
    while high > low+1:
        test = 0.5*(low+high)
        m = terrain_margin_lat_lon(TERR.Lat,TERR.Lng,alt+test,lat,lon,ant_alt)
        if m > 0:
            high = test
        elif m < 0:
            low = test
        else:
            low = test
            high = test
    return -high
    
def mm_curr(RCOU,BAT,PWM_MIN,PWM_MAX,Mfirst,Mlast):
    '''
    motor model to predict current draw given PWM to VTOL motors
    returned value should be proportional to expected total current draw
    '''
    total_curr = 0.0
    voltage = BAT.Volt
    for m in range(Mfirst,Mlast+1):
        pwm = getattr(RCOU,'C%u'%m,None)
        if pwm is None:
            return 0.0
        command = voltage*max(pwm - PWM_MIN,0)/(PWM_MAX-PWM_MIN)
        total_curr += command**2
    return total_curr

def RotateMag(MAG,rotation):
    '''rotate a MAG message by rotation enumeration'''
    v = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ)
    return v.rotate_by_id(rotation)