#include <Python.h>

#include <stdlib.h>
#include <math.h>

/* conversion parameters */
double degree, radian;
/* globals */
int    COFF, LOFF;
float CFAC, LFAC, sub_lon;

/*
degree = M_PI / 180.;
radian = 1./degree;
*/

void
geos(double lon, double lat, int *c, int *l) {
    /* return (col, line) numbers */
    /* inputs are in degree */
    double c_lat, rl, r1, r2, r3, rn, x, y;

    degree = M_PI / 180.;
    radian = 1./degree;

    c_lat = atan(0.993243 * tan(lat*degree));
    rl = 6356.5838/sqrt(1-0.00675701*cos(c_lat)*cos(c_lat));
    r1 = 42164.-rl*cos(c_lat)*cos((lon-sub_lon)*degree);
    r2 = -rl*cos(c_lat)*sin((lon-sub_lon)*degree);
    r3 = rl*sin(c_lat);
    rn = sqrt(r1*r1 + r2*r2 + r3*r3);
    x = atan(-r2/r1) * radian;
    y = asin(-r3/rn) * radian;
    *c = COFF + (int)(x*CFAC+0.5);
    *l = LOFF + (int)(y*LFAC+0.5);
}

void
polar(int x, int y, int x_offset, int y_offset,
      double lambda_0, double m,
      double *lon, double *lat) {
    double r, lmd, phi;

    x = x + x_offset;
    y = y + y_offset;
    r = sqrt((double)x*x + (double)y*y);
    lmd = atan2((double)y, (double)x);
    lmd = lmd/M_PI*180. + lambda_0;
    phi = 2.* atan(r*m/6356.5838/2.);
    phi = 90. - phi/M_PI*180.;
    *lon = lmd;
    *lat = phi;
}

static PyObject *
_util_geotrans(PyObject *self, PyObject *args)
{ 
    /* imgfile ... original IMG_...pgm */
    /* outfile ... GAME file name */
    /* lon_tl, lat_tl, lon_br, lat_br ... mapping box */
    /* width, height .................... its size    */
    /* COFF, LOFF, CFAC, LFAC, sub_lon .. parameters  */
    int    insize, inwidth;
    float  lon_tl, lat_tl, lon_br, lat_br;
    int    width, height;
    double lat_inc, lon_inc;
    char   *inbuf, *data, *dp;
    double lon, lat;
    int    x, y, l, c;
    PyObject *res;

    if (!PyArg_ParseTuple(args, "s#iffffiiiifff",
                            &inbuf, &insize, &inwidth,
                            &lon_tl, &lat_tl, &lon_br, &lat_br,
                            &width, &height,
                            &COFF, &LOFF, &CFAC, &LFAC, &sub_lon))
        return NULL;
/*
    printf("%d %f %f %f %f\n%d %d\n%d %d %f %f %f\n", 
        inwidth, lon_tl, lat_tl, lon_br, lat_br, width, height, 
        COFF, LOFF, CFAC, LFAC, sub_lon);
*/
    if ((data = (char *)PyMem_Malloc(width*height)) == NULL)
        return NULL;

    lat_inc = (float)(lat_br - lat_tl) / height;
    lon_inc = (float)(lon_br - lon_tl) / width;

    dp = data;
    for (y=0; y<height; y++) {
        lat = lat_tl + lat_inc * y;
        for (x=0; x<width; x++) {
            lon = lon_tl + lon_inc * x;
            geos(lon, lat, &c, &l);
            *data++ = *(inbuf + (l*inwidth+c));
        }
    }
    res = Py_BuildValue("s#", dp, width*height);
    PyMem_Free(dp);
    return res;
}

static PyObject *
_util_poltrans(PyObject *self, PyObject *args)
{
    /* inbuf ... original input IMG_ (8bit) buffer */
    /* insize, inwidth ... input buffer size, width */
    /* res ..... return buffer */
    /* X_OFFSET, Y_OFFSET ... upper left offset from north pole in pixel */
    /* O_WIDTH, O_HEIGHT ... note that width and height is inverted */
    /* LAMBDA_0 ... center longitude (straight up to north) */
    /* M ....... magnitude ... M km = 1 pixel at north pole */
    /* COFF, LOFF, CFAC, LFAC, sub_lon .. parameters  */
    char  *inbuf;
    int   insize, inwidth;
    int   X_OFFSET, Y_OFFSET, O_WIDTH, O_HEIGHT;
    float LAMBDA_0, M;
    char  *data, *dp;
    int   x, y;
    double lon, lat;
    int   c, l;
    PyObject *res;

    if (!PyArg_ParseTuple(args, "s#iiiiiffiifff",
                            &inbuf, &insize, &inwidth,
                            &X_OFFSET, &Y_OFFSET, &O_WIDTH, &O_HEIGHT,
                            &LAMBDA_0, &M,
                            &COFF, &LOFF, &CFAC, &LFAC, &sub_lon))
        return NULL;

    if ((data = (char *)PyMem_Malloc(O_WIDTH*O_HEIGHT)) == NULL)
        return NULL;

    dp = data;
    for (x = 0; x < O_WIDTH; x++) {
        for (y = 0; y < O_HEIGHT; y++) {
            polar(x, y, X_OFFSET, Y_OFFSET, LAMBDA_0, M, &lon, &lat);
            geos(lon, lat, &c, &l);
            *data++ = *(inbuf + (l*inwidth + c));
        }
    }
    res = Py_BuildValue("s#", dp, O_WIDTH*O_HEIGHT);
    PyMem_Free(dp);
    return res;
}

static PyObject *
_util_ten2eight(PyObject *self, PyObject *args)
{
    int i, size;
    int a, b, d;
    unsigned char *origdata, *data, *dp;
    PyObject *res;

    if (!PyArg_ParseTuple(args, "s#", 
			    &origdata, &size))
	return NULL;
    if ((data = PyMem_Malloc(size/2)) == NULL)
	return NULL;
    dp = data;
    for (i = 0; i < (size/2); i++) {
        a = (int)(*origdata++);
        b = (int)(*origdata++);
        d = ((a << 8) + b) >> 2;
        *data++ = (unsigned char)d;
    }
    res = Py_BuildValue("s#", dp, size/2);
    PyMem_Free(dp);
    return res;
}

static unsigned char
limit(double x)
{
    if (x > 255.) x = 255.;
    if (x < 0.)   x = 0.;
    return (unsigned char)x;
}

static PyObject *
_util_adduv(PyObject *self, PyObject *args)
{
    int i, size, dummy;
    double r, g, b, u, v, y, gamma;
    unsigned char *bwdata, *uvdata, *cldata, *cl;
    PyObject *res;

    if (!PyArg_ParseTuple(args, "s#s#d", 
                            &bwdata, &size, &uvdata, &dummy, &gamma))
        return NULL;
    if ((cldata = PyMem_Malloc(3*size)) == NULL)
        return NULL;
    cl = cldata;
    for (i = 0; i < size; i++) {
        r = (double)(*uvdata++);
        g = (double)(*uvdata++);
        b = (double)(*uvdata++);
        u = -0.169 * r - 0.331 * g + 0.500 *b;
        v =  0.500 * r - 0.419 * g - 0.081 *b;
        y = (double)(*bwdata++);
        y = 255.*pow(y/255., gamma);
        if (y > 255.) y = 255.;
        if (y < 0.)  y = 0.;
        *cldata++ = limit(y             + 1.402 * v);
        *cldata++ = limit(y - 0.344 * u - 0.714 * v);
        *cldata++ = limit(y + 1.772 * u);
    }
    res = Py_BuildValue("s#", cl, size*3);
    PyMem_Free(cl);
    return res;
}


static PyMethodDef _utilMethods[] = {
    {"ten2eight", _util_ten2eight, METH_VARARGS, "ten2eight(origdata)"},
    {"geotrans", _util_geotrans, METH_VARARGS, "geotrans(...)"},
    {"adduv", _util_adduv, METH_VARARGS, "adduv(bwdata, uvdata, gamma)"},
    {"poltrans", _util_poltrans, METH_VARARGS, "poltrans(...)"},
    {NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC
init_util(void)
{
    (void) Py_InitModule("_util", _utilMethods);
}

