#include <AP_gtest.h>

#include <AP_Math/AP_Math.h>
#include <AP_Declination/AP_Declination.h>

/*
  test data from AP_Declination/generate/testvectors.py
 */
static struct {
    float lat, lon;
    Vector3f field;
} test_data[] = {
{-5.347785, -55.396586, {240.284, -79.152, -18.954}},
{-49.307646, 128.384946, {116.253, 0.061, -644.547}},
{-54.681456, -36.248963, {154.192, -18.211, -234.732}},
{-62.435767, -62.106564, {194.290, 43.804, -294.683}},
{8.779390, 37.908713, {357.108, 14.172, 14.024}},
{41.538128, -148.450769, {227.080, 53.006, 387.268}},
{36.054359, -80.267792, {220.590, -32.594, 440.521}},
{34.199500, 43.418665, {289.166, 26.382, 369.560}},
{-59.875966, -81.614124, {195.629, 84.684, -312.342}},
{17.601001, -93.758586, {274.301, 7.775, 274.744}},
{-86.268095, 101.445984, {-99.788, -126.515, -534.840}},
{68.089832, 99.531480, {70.104, 3.974, 607.705}},
{-30.787050, 104.800267, {232.740, -28.398, -519.563}},
{-45.686919, 23.376860, {111.202, -77.903, -239.402}},
{42.344796, 170.972092, {256.985, 1.579, 361.160}},
{11.180749, 126.619946, {392.427, -12.519, 60.590}},
{16.085182, 45.400453, {367.527, 13.863, 138.729}},
{85.912578, -111.799061, {8.172, -16.213, 568.954}},
{61.702909, 71.317152, {115.475, 38.354, 577.707}},
{-18.117488, -107.440690, {267.590, 53.018, -104.495}},
{-62.458351, 93.834118, {20.921, -130.137, -583.142}},
{-4.456960, 24.690442, {268.996, 0.281, -181.715}},
{-11.938973, 57.584278, {286.506, -51.380, -249.702}},
{-17.077653, 68.903062, {276.594, -67.083, -330.818}},
{-0.641855, -85.362927, {273.764, -0.021, 93.528}},
{70.208175, -70.794011, {54.788, -33.030, 565.194}},
{65.974445, -37.033989, {110.056, -41.337, 525.313}},
{-12.136829, 86.003211, {342.692, -33.667, -313.206}},
{-85.859369, -148.301355, {-48.300, 152.537, -536.334}},
{-19.307265, 115.056146, {324.659, 3.429, -396.785}},
{63.409946, -125.963384, {102.250, 34.962, 561.223}},
{10.445067, 60.380511, {384.749, -5.779, 62.938}},
{-54.102628, 134.827917, {84.120, 9.249, -655.864}},
{-37.512392, -13.418317, {98.314, -39.221, -216.540}},
{72.345349, 113.696131, {53.912, -7.336, 602.335}},
{55.212086, -11.672616, {179.144, -14.309, 464.924}},
{-69.962700, -143.828634, {72.069, 139.770, -551.999}},
{87.579976, -163.341588, {-9.629, -20.622, 569.787}},
{73.111174, 143.018689, {72.564, -23.169, 586.064}},
{23.697573, -29.994152, {309.137, -54.381, 178.330}},
{8.771240, -117.029349, {293.100, 40.791, 152.298}},
{-17.705434, 2.904797, {141.603, -31.071, -250.994}},
{36.273922, 68.280726, {289.178, 20.771, 423.563}},
{-23.761563, 163.511301, {300.464, 67.082, -379.323}},
{79.029797, 176.355901, {47.596, -4.724, 577.193}},
{-52.405560, 22.440300, {122.338, -88.770, -252.169}},
{72.485297, -46.002536, {70.887, -38.208, 546.530}},
{-22.183293, -159.478002, {295.811, 77.374, -257.498}},
{-24.950420, 35.883896, {150.668, -57.547, -261.731}},
{34.311721, -178.681293, {263.416, 26.642, 291.758}},
{-61.490634, 130.934200, {30.474, -18.155, -665.682}},
{2.793989, -178.226296, {331.384, 56.367, 5.643}},
{51.905236, 130.494014, {204.801, -48.610, 530.005}},
{-25.962740, -60.589052, {188.450, -40.089, -113.336}},
{64.153460, 76.711770, {98.437, 34.425, 588.123}},
{85.273389, 80.411045, {8.157, 23.324, 574.387}},
{72.227644, -175.679117, {94.668, 2.434, 565.494}},
{-15.800057, -88.398789, {255.341, 26.361, -44.696}},
{-32.252553, 140.907109, {249.932, 37.054, -516.256}},
{0.990803, 19.645312, {302.237, 6.632, -135.887}},
{13.221787, -167.051664, {297.419, 47.578, 134.337}},
{-55.704793, -147.770348, {153.269, 119.486, -498.314}},
{-21.527164, 98.030817, {291.813, -24.330, -432.389}},
{-29.459735, -169.175151, {269.771, 84.511, -361.577}},
{-77.026676, -128.521814, {57.096, 156.826, -529.076}},
{59.744040, 154.466555, {174.852, -31.321, 530.246}},
{-8.667233, -84.596843, {263.264, 6.250, 19.318}},
{-44.970992, -90.210588, {203.908, 78.088, -237.970}},
{-81.281504, 53.655592, {53.564, -172.598, -484.541}},
{-28.899249, -168.937713, {272.725, 84.310, -354.906}},
{-5.495801, -115.945089, {294.242, 45.813, 2.378}},
{-56.855722, -27.312216, {150.561, -22.699, -243.436}},
{-5.160334, -104.515570, {287.811, 39.169, 23.270}},
{-4.765155, 27.513017, {270.761, 0.084, -179.570}},
{38.850519, -83.245266, {208.706, -24.590, 466.759}},
{82.903976, -111.774971, {15.131, -10.610, 569.636}},
{65.049275, 72.501496, {95.332, 38.164, 582.824}},
{62.904242, 91.633672, {102.309, 14.476, 603.680}},
{-26.703878, 172.749541, {285.797, 77.084, -385.509}},
{-33.226726, 75.903795, {173.490, -100.050, -456.333}},
{24.643218, -30.321324, {307.312, -53.948, 191.067}},
{16.206490, 22.434794, {358.048, 19.103, 94.086}},
{46.445304, -34.626939, {217.483, -45.670, 423.301}},
{19.266180, 125.974692, {377.297, -24.128, 183.176}},
{-24.732609, -162.086198, {289.118, 80.970, -291.171}},
{10.102379, -89.724793, {278.231, 3.333, 201.223}},
{44.251765, 81.116331, {239.108, 19.539, 512.551}},
{-72.418531, -130.000797, {82.558, 150.448, -523.607}},
{12.703973, 18.436378, {353.684, 14.320, 30.949}},
{29.149731, -105.686368, {248.213, 29.644, 376.837}},
{-17.956078, -35.751483, {164.178, -72.043, -167.376}},
{-38.321171, -141.763622, {232.842, 93.184, -359.596}},
{21.175405, 50.561510, {355.480, 13.624, 223.431}},
{54.947454, -169.310847, {199.460, 23.943, 468.886}},
{-23.361657, 44.446547, {177.462, -68.674, -281.151}},
{-24.280380, 19.644762, {122.126, -31.739, -246.213}},
{-63.110180, -122.152350, {143.859, 136.021, -464.339}},
{4.519890, 40.756994, {348.158, 6.163, -44.400}},
{-61.062289, 59.265124, {84.041, -166.618, -424.664}},
{-42.770992, -60.512815, {174.635, -6.856, -175.937}}};


TEST(MagField, test_field_error)
{
    for (const auto &d : test_data) {
        Location loc(d.lat*1.0e7, d.lon*1.0e7, 0, Location::AltFrame::ABSOLUTE);
        const Vector3f m = AP_Declination::get_earth_field_ga(loc);
        EXPECT_FLOAT_EQ(roundf(m.x*1000), roundf(d.field.x));
        EXPECT_FLOAT_EQ(roundf(m.y*1000), roundf(d.field.y));
        EXPECT_FLOAT_EQ(roundf(m.z*1000), roundf(d.field.z));
    }
}


AP_GTEST_MAIN()
int hal = 0;