#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#ifndef PI
    #define PI 3.14159265358979323846
#endif

typedef struct __default_gps_mgrs_new_coordinate_t {
    union{
        char values[15];
        struct{
            char gzd[3];
            char id[2];
            char e[5];
            char n[5];
        };
    };
} gps_mgrs_new_coordinate_t;

char* pad (int val) {
    char* result = (char*)calloc(sizeof(char), 7);
    sprintf(result, "%05d", val);
    return result;
}
    
int16_t str_pos(const char * str, const char search) {
    int16_t i;
    const char * found = strchr(str, search);
    int16_t pos = 0;
    if (found == 0)
        return -1;
    while (*str && str < found) {
        i = 1;
        if ((*str & 0xE0) == 0xC0) i=2;
        else if ((*str & 0xF0) == 0xE0) i=3;
        else if ((*str & 0xF8) == 0xF0) i=4;
        str += i;
        pos += i == 4 ? 2 : 1;
    }
    return pos;
}

double* LatLongFromMGRSstring(gps_mgrs_new_coordinate_t mgrs) {
    char* temp = (char*)calloc(sizeof(char), 8);
    memcpy(temp, mgrs.gzd, 2);
    int c = atoi(temp);
    char d = mgrs.gzd[2];
    double e = (c*6-183)*PI / 180;
    const char* table0[] = {
        "ABCDEFGH","JKLMNPQR","STUVWXYZ"
    };
    int f = str_pos(table0[(c-1) % 3], mgrs.id[0]) + 1;
    const char* table1 = "CDEFGHJKLMNPQRSTUVWXX";
    int g = str_pos(table1, d);
    const char* table2[] = {
        "ABCDEFGHJKLMNPQRSTUV","FGHJKLMNPQRSTUVABCDE"
    };
    int h = str_pos(table2[(c-1) % 2], mgrs.id[1]);
    const double i[] = {1.1,2.0,2.8,3.7,4.6,5.5,6.4,7.3,8.2,9.1,0,0.8,1.7,2.6,3.5,4.4,5.3,6.2,7.0,7.9};
    const int j[] = {0,2,2,2,4,4,6,6,8,8,0,0,0,2,2,4,4,6,6,6};
    double k = i[g];
    double l = (double)j[g] + h / 10.0;
    if (l < k) {
        l += 2;
    }
    memset(temp, 0x00, 5);
    memcpy(temp, mgrs.e, 5);
    double m = f*100000.0 + atof(temp);
    memset(temp, 0x00, 5);
    memcpy(temp, mgrs.n, 5);
    double n = l*1000000 + atof(temp);
    m -= 500000.0;
    if (d < 'N') {
        n -= 10000000.0;
    }
    m /= 0.9996; 
    n /= 0.9996;
    double o = n / 6367449.14570093;
    double p = o + 
        (0.0025188266133249035*sin(2.0*o)) +
        (0.0000037009491206268*sin(4.0*o)) + 
        (0.0000000074477705265*sin(6.0*o)) + 
        (0.0000000000170359940*sin(8.0*o));

    double q = tan(p);
    double r = q*q;
    double s = r*r;
    double t = cos(p);
    double u = 0.006739496819936062*pow(t,2.0);
    double v = 40680631590769 / (6356752.314*sqrt(1.0 + u));
    double w = v;
    double x = 1.0 / (w*t); w *= v;
    double y = q / (2.0*w); w *= v;
    double z = 1.0 / (6.0*w*t); w *= v;
    double aa = q / (24.0*w); w *= v;
    double ab = 1.0 / (120.0*w*t); w *= v;
    double ac = q / (720.0*w); w *= v;
    double ad = 1.0 / (5040.0*w*t); w *= v;
    double ae = q / (40320.0*w);
    double af = -1.0-u;
    double ag = -1.0-2*r-u;
    double ah = 5.0 + 3.0*r + 6.0*u-6.0*r*u-3.0*(u*u)-9.0*r*(u*u);
    double ai = 5.0 + 28.0*r + 24.0*s + 6.0*u + 8.0*r*u;
    double aj = -61.0-90.0*r-45.0*s-107.0*u + 162.0*r*u;
    double ak = -61.0-662.0*r-1320.0*s-720.0*(s*r);
    double al = 1385.0 + 3633.0*r + 4095.0*s + 1575*(s*r);
    double lat = p + y*af*(m*m) + aa*ah*pow(m,4) + ac*aj*pow(m,6) + ae*al*pow(m,8);
    double lng = e + x*m + z*ag*pow(m,3) + ab*ai*pow(m,5) + ad*ak*pow(m,7);
    free(temp);
    double* result = (double*)calloc(sizeof(double), 2);
    result[0] = lat*180 / PI;
    result[1] = lng*180 / PI;
    return result;
}

char* latLngToMGRS(double lat, double lng){
    int c = (int)(1 + floor((lng + 180) / 6));
    double e = c * 6 - 183;
    double k = lat * PI / 180;
    double l = lng * PI / 180;
    double m = e * PI / 180;
    double n = cos(k);
    double o = 0.006739496819936062 * pow(n, 2);
    double p = 40680631590769/(6356752.314*sqrt(1 + o));
    double q = tan(k);
    double r = q * q;
    double s = (r*r*r) - pow(q,6);
    double t = l - m;
    double u = 1.0 - r + o;
    double v = 5.0 - r + 9*o + 4.0*(o*o);
    double w = 5.0 - 18.0*r + (r*r) + 14.0*o - 58.0*r*o;
    double x = 61.0 - 58.0*r + (r*r) + 270.0*o - 330.0*r*o;
    double y = 61.0 - 479.0*r + 179.0*(r*r) - (r*r*r);
    double z = 1385.0 - 3111.0*r + 543.0*(r*r) - (r*r*r);
    double aa = p*n*t + (p/6.0*pow (n,3)*u*pow (t,3)) 
        + (p/120.0*pow (n,5)*w*pow (t,5)) 
        + (p/5040.0*pow (n,7)*y*pow (t,7));
    double ab = 6367449.14570093*(k - (0.00251882794504*sin (2*k)) 
        + (0.00000264354112*sin (4*k)) - (0.00000000345262*sin (6*k)) 
        + (0.000000000004892*sin (8*k))) + (q/2.0*p*pow (n,2)*pow (t,2)) 
        + (q/24.0*p*pow (n,4)*v*pow (t,4)) + (q/720.0*p*pow (n,6)*x*pow (t,6)) 
        + (q/40320.0*p*pow (n,8)*z*pow (t,8));
    aa = aa*0.9996 + 500000.0;
    ab = ab*0.9996; 
    if (ab < 0.0) {
        ab += 10000000.0;
    }
    const char* temp0 = "CDEFGHJKLMNPQRSTUVWXX\0";
    char ad = temp0[(int)floor (lat/8 + 10)];
    int ae = (int)floor (aa/100000);
    const char* temp1[] = {
        "ABCDEFGH\0"
        ,"JKLMNPQR\0"
        ,"STUVWXYZ\0"
    };
    char af = temp1[(c-1)%3][ae-1];
    int ag = (int)floor(ab/100000)%20;

    const char* temp2[] = {
        "ABCDEFGHJKLMNPQRSTUV\0"
        ,"FGHJKLMNPQRSTUVABCDE\0"
    };
    char ah = temp2[(c-1)%2][ag];
    aa = floor ((int)aa%100000); 
    char* aaStr = pad ((int)aa);
    ab = floor ((int)ab%100000); 
    char* abStr = pad ((int)ab);
    char* result = (char*)calloc(sizeof(char), 256);
    sprintf(result, "%d%c%c%c%s%s\n", c, ad, af, ah, aaStr, abStr);
    free(aaStr);
    free(abStr);
    return result;
}
#define ONE 0
int main(int argc, char const *argv[]){
    #if ONE
    double lat = 37.9975178;
    double lng = 127.2190851;
    #else
    double lat = 38.9972110;
    double lng = 123.2187474;
    #endif
    char* result = latLngToMGRS(lat, lng);
    printf("[START]\nLat : %lf \t Lng : %lf\n", lat, lng);
    printf("%s", result);
    gps_mgrs_new_coordinate_t mgrs;
    memcpy(mgrs.values, result, sizeof(gps_mgrs_new_coordinate_t));
    double* latLng = LatLongFromMGRSstring(mgrs);
    printf("Lat : %lf \t Lng : %lf\n", latLng[0], latLng[1]);
    free(result);
    result = latLngToMGRS(latLng[0], latLng[1]);
    printf("%s", result);
    free(latLng);
    free(result);
    return 0;
}