#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[] = {
{65.815374, -143.062333, {107.860, 35.113, 556.328}},
{4.841310, -64.150382, {266.887, -64.218, 111.937}},
{45.369500, 48.968768, {222.477, 32.849, 462.635}},
{56.434560, -25.336510, {167.362, -35.285, 476.757}},
{66.808874, 79.148521, {82.702, 33.023, 589.400}},
{-7.916276, -44.424167, {220.765, -86.936, -83.784}},
{64.118293, -0.153519, {134.901, -3.555, 500.255}},
{-35.352160, 99.449185, {191.986, -54.353, -541.436}},
{-42.283291, -97.597266, {210.008, 85.539, -250.617}},
{83.930834, 5.940172, {47.596, 4.145, 554.006}},
{47.012991, 36.119569, {213.213, 29.443, 453.806}},
{-21.339327, 36.712924, {166.444, -45.453, -261.021}},
{-26.958910, 135.309269, {283.842, 26.564, -468.649}},
{76.337075, -77.042161, {30.209, -27.702, 564.548}},
{-68.792368, -26.587185, {184.541, -12.654, -318.117}},
{42.204177, -164.412272, {236.899, 39.169, 370.136}},
{73.424992, -118.704385, {43.023, 8.787, 575.713}},
{65.283281, 2.411133, {129.587, -0.609, 504.668}},
{34.510044, 135.655983, {309.899, -40.362, 353.492}},
{68.831024, -173.333644, {115.391, 10.915, 556.720}},
{-65.176537, 164.992638, {19.605, 85.294, -639.778}},
{-57.434603, 152.791413, {72.536, 58.406, -647.895}},
{15.880162, 66.262931, {387.182, -2.057, 146.598}},
{67.203361, -177.608822, {128.854, 7.288, 548.788}},
{-30.523857, 29.118199, {113.791, -54.955, -248.678}},
{31.809160, -119.132163, {244.130, 49.844, 376.711}},
{-15.663087, -80.002773, {247.203, 5.676, -31.897}},
{-58.047622, 52.078363, {101.589, -155.733, -380.717}},
{88.215263, -126.430760, {-3.492, -18.889, 566.858}},
{-14.693858, 100.649221, {343.282, -9.516, -348.166}},
{37.646886, -40.902135, {250.225, -59.227, 358.666}},
{-73.309744, 171.966445, {-32.052, 91.220, -623.196}},
{66.381521, 93.066045, {81.832, 15.501, 601.880}},
{-51.800132, -48.161067, {166.155, -10.418, -218.457}},
{-1.470683, -101.101444, {289.312, 35.209, 67.787}},
{30.018412, -128.584173, {250.048, 54.126, 338.710}},
{60.430386, -85.104119, {90.333, -20.598, 572.518}},
{65.859137, -119.499590, {78.769, 25.428, 573.031}},
{49.072308, -47.606189, {188.603, -58.266, 465.766}},
{77.503573, -60.423956, {38.556, -33.783, 556.409}},
{82.714485, 128.717387, {19.657, 10.220, 580.422}},
{38.801986, 91.587255, {282.915, 4.742, 471.180}},
{-31.287123, -94.113422, {226.905, 60.645, -171.111}},
{25.047470, 65.541191, {352.871, 8.642, 282.449}},
{14.206388, 26.081254, {358.987, 18.786, 66.843}},
{-29.299607, -166.418773, {271.109, 83.632, -352.255}},
{26.591442, -73.547420, {251.844, -46.851, 346.171}},
{88.640245, 169.476288, {-18.244, 11.586, 567.557}},
{33.894442, 72.675222, {307.721, 15.765, 398.253}},
{-88.031320, 74.684074, {-37.224, -164.448, -519.832}},
{79.537032, 32.735513, {58.741, 24.554, 552.941}},
{45.437015, 80.470038, {231.120, 22.130, 515.505}},
{49.325371, -172.704252, {224.668, 24.546, 424.141}},
{14.903743, -64.771466, {269.743, -64.464, 209.310}},
{17.835966, 174.249220, {303.993, 38.460, 147.669}},
{7.298126, 118.399877, {406.420, -6.502, 8.517}},
{-63.751920, 68.614309, {61.074, -168.670, -474.255}},
{-38.960700, 10.784171, {92.427, -43.852, -222.347}},
{53.775617, 81.590013, {168.976, 26.522, 565.427}},
{43.838294, -155.062495, {226.785, 48.323, 397.111}},
{44.222391, 169.462356, {252.609, -1.206, 379.944}},
{17.959121, 54.928441, {369.427, 6.915, 175.187}},
{76.044596, -122.732672, {35.189, 6.653, 573.863}},
{52.971416, -26.099079, {185.527, -36.203, 458.872}},
{-74.070371, -138.645368, {53.523, 150.678, -546.192}},
{-81.736068, -31.431979, {189.895, 4.156, -439.712}},
{10.192549, -127.948140, {292.468, 45.734, 152.605}},
{-47.221957, 78.382924, {103.034, -130.997, -499.588}},
{51.576442, -154.944501, {201.013, 45.570, 460.479}},
{-63.852951, 35.163443, {127.325, -135.698, -341.812}},
{28.300507, -163.781419, {260.632, 42.939, 261.746}},
{0.595936, 141.700759, {374.127, 18.689, -98.455}},
{-64.367741, 95.249244, {11.465, -130.514, -585.559}},
{-69.280536, -143.001117, {77.504, 139.710, -549.996}},
{48.335410, 70.288703, {206.898, 31.805, 519.597}},
{18.370932, -0.992613, {343.193, -6.545, 96.215}},
{-33.668004, 31.933840, {115.929, -68.241, -254.100}},
{-66.283464, 136.355865, {35.080, 29.372, -658.843}},
{87.188467, 80.272993, {6.995, 22.295, 569.340}},
{-37.843441, -37.567972, {133.282, -46.568, -190.835}},
{69.911723, 4.695994, {108.423, 1.595, 519.605}},
{8.137470, 11.523994, {335.731, 0.997, -49.510}},
{49.732747, 90.330000, {198.706, 11.573, 559.178}},
{-29.391961, -88.979254, {225.191, 49.068, -144.567}},
{35.156541, 43.801954, {283.633, 25.974, 375.362}},
{49.352233, -56.328211, {177.143, -59.472, 486.532}},
{68.219860, 29.192947, {109.116, 27.847, 527.438}},
{-16.720512, 171.687827, {328.473, 68.966, -270.540}},
{-65.879394, 156.212441, {-0.674, 68.128, -650.680}},
{59.992056, 167.198631, {178.767, -11.904, 516.533}},
{15.571870, -163.519672, {290.435, 46.049, 160.239}},
{-4.927750, 48.044786, {309.256, -21.794, -163.621}},
{8.440285, -48.490401, {273.723, -89.165, 76.441}},
{31.432914, 128.102195, {327.297, -38.624, 341.520}},
{-76.539795, 114.525530, {-79.373, -91.953, -597.405}},
{37.595668, 10.343255, {273.196, 12.709, 353.344}},
{7.995321, 66.515830, {392.070, -12.382, 18.917}},
{14.426346, -7.141914, {336.552, -21.023, 32.344}},
{-25.434922, -161.299687, {286.912, 80.742, -297.084}},
{67.878431, 32.272372, {109.027, 30.505, 529.484}}};


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;