#include <math.h>

#define   C1   (6378137.0)
#define C2   (6356752.314)
#define   C3   (4.068063159E+13)
#define C4   (1.643835165E+27)


float WGS84Distance(float lat1, float lon1, float lat2, float lon2) {
   
   float t1, t2, t3, t4, t5, t6;
   
   t1 = sinf((lat1 - lat2) / 2);
   t2 = sinf((lon1 - lon2) / 2);
   t3 = cosf(lat1);
   t4 = 2 * asinf(sqrtf(t1 * t1 + t3 * cosf(lat2) * t2 * t2));
   t5 = (sinf(lat2) - sinf(lat1) * cosf(t4)) / (sinf(t4) * t3);
   t6 = sinf(acosf(t5));
   t3 = (lat1 + lat2) / 2;
   t1 = C1 * cosf(t3);
   t2 = C2 * sinf(t3);
   t3 = t1 * t1 + t2 * t2;
   return t4 / (t5 * t5 * expf(logf(t3) * 1.5) / C4 + t6 * t6 * sqrtf(t3) / C3);
}

int testCalcSpeed(void) {
   static const float testData[568] = {
         -0.024554743, 0.0049280105, 0.007437401, 0.030578617,
         0.03175952, 0.017858354, -0.02195712, -0.02186809,
         0.03114443, 0.04099197, 0.017912176, -0.043625165,
         0.06868667, -0.02183334, 0.05844636, 0.012727903,
         0.05861937, 0.029816773, 0.06683186, 0.024283528,
         0.012207284, 0.05112016, 0.0050859437, -0.041081056,
         0.05136775, 0.017964838, 0.07807105, -0.005371601,
         0.09875725, -0.016189734, 0.0040930416, -0.02505305,
         0.078876816, -0.045351665, 0.06443052, -9.859721E-4,
         0.11245904, 0.02668503, 0.046022892, -0.005054367,
         0.1066363, -0.017019015, 0.078549795, 0.019999584,
         0.042892683, -0.031836137, 0.08620672, 0.046372265,
         0.13031062, -0.0018083801, 0.05910002, -9.089224E-4,
         0.1145482, -0.050511833, 0.10038405, 0.043840647,
         0.10047158, 0.04359573, 0.095855825, 0.049453355,
         0.08323971, 0.022934834, 0.14033516, 0.0018331357,
         0.16149022, -0.026471224, 0.17116845, 0.040303588,
         0.10331598, 0.024189074, 0.18007384, 0.03797311,
         0.15739058, -0.045962505, 0.11087838, 0.019062765,
         0.12879424, -0.031684034, 0.17147987, -0.04258154,
         0.18490085, 0.021093449, 0.16354641, 0.005188685,
         0.15225215, 0.026853023, 0.20465106, -0.0108276475,
         0.20402679, -0.04703418, 0.21832809, -0.006551552,
         0.2342803, -0.018696029, 0.14465661, -0.048402302,
         0.1722239, -0.019647753, 0.17476524, -0.04280905,
         0.21877526, 0.037989337, 0.21076834, -0.0112720905,
         0.17856964, 0.0304331, 0.20785794, -0.043660875,
         0.18127114, -0.032619033, 0.1943349, 0.04994064,
         0.26377636, 0.018123288, 0.19537827, 0.035942685,
         0.24895242, 0.023672374, 0.24745247, -0.0020499937,
         0.24020413, 0.032604333, 0.29710156, 0.020011099,
         0.2112274, -0.020606508, 0.31027865, 0.03095813,
         0.24491207, 2.5765403E-4, 0.27740034, 0.038384877,
         0.282219, 0.044981033, 0.27138457, -0.04711068,
         0.24551192, -0.024083616, 0.29391518, 0.0064092567,
         0.29245892, 0.008112993, 0.33754638, 0.014420619,
         0.27906, 0.015238173, 0.3248683, 0.047085743,
         0.34665892, 0.04855925, 0.3563396, -0.026223509,
         0.37678996, 0.047077984, 0.3294159, -0.029254185,
         0.32956415, -0.017764173, 0.29065028, 0.04247912,
         0.38412553, -0.0446996, 0.30328038, 0.0113617,
         0.3542849, -0.028664256, 0.3790272, 0.05086285,
         0.40901214, -0.025248803, 0.33438292, -0.007939167,
         0.3728698, -0.022177225, 0.35355622, -0.025054535,
         0.4324903, 0.04003138, 0.34431624, -0.043283127,
         0.41519776, -0.050036464, 0.33325076, 0.044420425,
         0.44286764, -0.02797077, 0.35341683, -0.036217306,
         0.4089911, 0.008859528, 0.35608518, 0.012891081,
         0.4651716, -0.021388365, 0.43167737, -0.015156893,
         0.41870728, 0.006660681, 0.4346274, -0.0010987971,
         0.48588187, 0.003398127, 0.44021988, 0.012607126,
         0.46555185, 0.027256684, 0.40223157, -0.0343913,
         0.4823829, 0.017012682, 0.5058071, -0.040539313,
         0.4522113, 0.049210265, 0.4922693, 0.038217373,
         0.4524164, -0.0058237086, 0.44815636, -0.02510158,
         0.5064645, 0.027569514, 0.45128426, -0.016349353,
         0.4435337, -0.004480742, 0.46389046, 0.029917121,
         0.48703644, -0.024319831, 0.5371022, 0.051582076,
         0.4778284, 0.039954, 0.5538515, 0.025007984,
         0.5366497, 0.046997946, 0.5272661, 0.03523006,
         0.47666913, -0.022952154, 0.4740135, 0.0028686165,
         0.48315796, -0.011417686, 0.50770634, 0.0076686335,
         0.49959147, -0.012285631, 0.52415305, 0.04695578,
         0.4973171, -0.044052925, 0.5094945, 0.01094066,
         0.5643108, 0.03770716, 0.55628425, -0.029401403,
         0.598639, -0.033445843, 0.5644754, -0.045389406,
         0.57145494, -0.04056786, 0.5538895, -0.051410392,
         0.5801241, -0.018374084, 0.55146027, 0.03791599,
         0.54345644, -0.04736111, 0.5828327, -0.0031415545,
         0.56131244, -0.012042554, 0.56601804, 0.020001704,
         0.65400445, 0.037914418, 0.6375514, -0.02425563,
         0.59885746, -0.024990864, 0.6619488, -0.04376933,
         0.62917906, 0.03377573, 0.59197015, 0.012147746,
         0.61125827, 0.016718967, 0.5927565, -0.007983111,
         0.6146617, -0.047358613, 0.6740112, -0.020796213,
         0.6177519, 0.039155576, 0.6003201, 0.051938657,
         0.6815886, -0.022075923, 0.6834693, 0.023153799,
         0.6630458, 0.007050294, 0.6327674, -0.0522692,
         0.6465639, 0.022244131, 0.67596257, 0.043018915,
         0.63733095, 0.0056094783, 0.714267, 0.009875709,
         0.7294305, 0.018681347, 0.7044053, 0.0080318535,
         0.72461283, 0.00816754, 0.6865014, 0.04367828,
         0.7449645, 0.0025873038, 0.665201, -0.039090607,
         0.6991263, -0.04200074, 0.68317807, -0.039143067,
         0.7606462, -0.01604184, 0.772754, 0.021124879,
         0.68782574, -0.04514815, 0.76678324, -0.0077443863,
         0.70723355, 0.029246952, 0.77734333, -0.0306983,
         0.7809586, 0.022598006, 0.77477455, -0.041921996,
         0.7223736, 0.029016646, 0.7537937, -0.01647595,
         0.7473587, -0.018391319, 0.7940923, 0.0041983025,
         0.7611761, -0.010808042, 0.73822886, 0.014290897,
         0.79693687, -0.050555553, 0.79791296, -0.022884825,
         0.83510554, 0.008151899, 0.7837037, -0.0027720053,
         0.7723753, 0.0037887546, 0.8060014, -0.049176965,
         0.8319939, 0.044382825, 0.8063974, -0.03891988,
         0.8068985, -0.049899004, 0.8356233, 0.02064502,
         0.85965234, -0.0017092455, 0.8506851, 0.03098356,
         0.83012706, 0.031747337, 0.8524579, -0.007653782,
         0.8564447, 0.0034531609, 0.8306113, 0.0128596015,
         0.8069695, -0.03538453, 0.8262301, 0.013724689,
         0.83017945, -0.0151674645, 0.89968103, 0.02500554,
         0.8717944, -0.038595267, 0.89106315, -0.050434563,
         0.887562, -0.04997182, 0.87817407, -0.022231603,
         0.93900406, -0.024319647, 0.87751156, 0.013231412,
         0.949488, -0.03363121, 0.9370958, -0.0043035606,
         0.941926, -0.006874703, 0.89179283, 0.03689775,
         0.88521546, 0.033285227, 0.9207711, 0.0019326515,
         0.9669857, 0.008632794, 0.9525404, -0.0015770046,
         0.9606963, -0.04531668, 0.92607576, 0.035646487,
         0.96899927, 0.00339891, 0.94171643, 0.024621641,
         0.92555064, 0.049074717, 0.93918943, 0.045866847,
         1.0100657, 0.026734848, 0.97440356, -0.024342434,
         0.98499763, -0.025247868, 0.99594826, -5.2565505E-4,
         0.9748599, -0.036280006, 0.9308381, -0.028084682,
         0.9915492, -0.03648105, 1.0280373, -0.010806465,
         1.0189643, 0.035893243, 1.0356417, -0.021606417,
         0.9980618, -0.039976258, 1.0233431, 0.027425908,
         0.9700023, 0.04530727, 0.97097546, 0.017417187,
         1.0777488, 0.017731842, 1.0815854, 0.049522374,
         0.9854099, -0.020060387, 0.9793702, -0.037529923,
         1.0394742, -0.038141362, 1.0510712, 0.021775514,
         1.0582889, -0.009214772, 1.0816005, -0.039653163,
         1.0855235, -0.0052193543, 1.1079253, 0.04941183,
         1.0468595, -0.049929984, 1.0275978, 0.015546161,
         1.108017, 0.044809546, 1.078685, -0.039014786,
         1.1227914, -0.048057836, 1.0851202, -0.0064566648,
         1.1372424, -0.047275763, 1.1242507, 0.044687722,
         1.0676188, -0.024594583, 1.0481881, 0.042941764,
         1.1616675, -0.027537832, 1.1671991, 0.004424525,
         1.1399498, -0.033868592, 1.1304743, -0.029573077,
         1.1573918, 0.015173606, 1.1568565, 0.010283944,
         1.1624418, 0.022991698, 1.1831783, -0.022922859,
         1.1176, 0.04519321, 1.1249298, -0.031228341,
         1.1795722, -0.043728672, 1.1695375, 0.033317562,
         1.1867331, -0.04758717, 1.1364198, 0.04968198,
         1.1729314, -0.0022003537, 1.2204603, -0.013087612,
         1.1663435, -0.05174692, 1.1358141, -0.04366795,
         1.1953956, 0.045004584, 1.1665587, -0.014883602,
         1.1728643, 0.019157121, 1.2121586, -0.023968114,
         1.1911387, -0.04783553, 1.2221, -0.04565134,
         1.2481841, 0.008402959, 1.2358112, 0.040879086,
         1.2345253, 0.031099362, 1.2028805, 0.022344049
   };
   int tot;
   int i;
   int j;
   float la1;
   float lo1;
   float la2;
   float lo2;
   float res;


     printf("Speed test is started.\n");

     tot = 0;
     for(i = 0; i < 568; i += 4){
        la1 = testData[i];
        lo1 = testData[i+1];
        la2 = testData[i+2];
        lo2 = testData[i+3];

        for(j = 0; j < 10000; j++) {
           res = WGS84Distance(la1, lo1, la2, lo2);
           tot++;
        }
     }

     printf("Total number of calls: %u\n", tot);

     return tot;
}