/*
 *  projcl_run.c
 *  Magic Maps
 *
 *  Created by Evan Miller on 2/12/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include <projcl/projcl.h>
#include <projcl/projcl_warp.h>
#include "projcl_run.h"
#include "projcl_kernel.h"
#include "projcl_util.h"
#include "projcl_spheroid.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#endif
#ifdef __linux__
#include <cblas.h>
typedef int __CLPK_integer;
typedef double __CLPK_doublereal;
extern int dgetrf_(__CLPK_integer *, __CLPK_integer *, __CLPK_doublereal *, __CLPK_integer *, __CLPK_integer *, __CLPK_integer *);
extern int dgetri_(__CLPK_integer *, __CLPK_doublereal *, __CLPK_integer *, __CLPK_integer *, __CLPK_doublereal *, __CLPK_integer *, __CLPK_integer *);
#endif

#define EPS7 1.e-7

#define SEC_TO_RAD 4.84813681109535993589914102357e-6

static cl_int pl_enqueue_kernel_albers_equal_area(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_american_polyconic(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_lambert_azimuthal_equal_area(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_lambert_conformal_conic(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_mercator(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_oblique_stereographic(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_robinson(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_transverse_mercator(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);
static cl_int pl_enqueue_kernel_winkel_tripel(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count);

struct pl_projection_info {
    PLProjection proj;
    char         name[80];
    cl_int (*func)(PLContext *pl_ctx,
                          cl_kernel kernel,
                          PLProjectionParams *params,
                          cl_mem xy_in,
                          cl_mem xy_out,
                          size_t count);
};

static struct pl_projection_info _pl_projection_info[] = {
    {
        .proj = PL_PROJECT_ALBERS_EQUAL_AREA,
        .name = "albers_equal_area",
        .func = &pl_enqueue_kernel_albers_equal_area,
    },
    {
        .proj = PL_PROJECT_AMERICAN_POLYCONIC,
        .name = "american_polyconic",
        .func = &pl_enqueue_kernel_american_polyconic,
    },
    {
        .proj = PL_PROJECT_LAMBERT_AZIMUTHAL_EQUAL_AREA,
        .name = "lambert_azimuthal_equal_area",
        .func = &pl_enqueue_kernel_lambert_azimuthal_equal_area,
    },
    {
        .proj = PL_PROJECT_LAMBERT_CONFORMAL_CONIC,
        .name = "lambert_conformal_conic",
        .func = &pl_enqueue_kernel_lambert_conformal_conic,
    },
    {
        .proj = PL_PROJECT_MERCATOR,
        .name = "mercator",
        .func = &pl_enqueue_kernel_mercator,
    },
    {
        .proj = PL_PROJECT_OBLIQUE_STEREOGRAPHIC,
        .name = "oblique_stereographic",
        .func = &pl_enqueue_kernel_oblique_stereographic,
    },
    {
        .proj = PL_PROJECT_ROBINSON,
        .name = "robinson",
        .func = &pl_enqueue_kernel_robinson,
    },
    {
        .proj = PL_PROJECT_TRANSVERSE_MERCATOR,
        .name = "transverse_mercator",
        .func = &pl_enqueue_kernel_transverse_mercator,
    },
    {
        .proj = PL_PROJECT_WINKEL_TRIPEL,
        .name = "winkel_tripel",
        .func = &pl_enqueue_kernel_winkel_tripel,
    }
};

struct pl_datum_info {
    double dx;
    double dy;
    double dz;
    double ex;
    double ey;
    double ez;
    double ppm;
};

/* Source: "WGS 84 Implementation Manual" */
static struct pl_datum_info pl_datum_params[] = {
    /*    Dx       Dy        Dz      Ex        Ey       Ez       m   */

    { /* PL_DATUM_WGS_84 */
          0.0,     0.0,    0.0,     0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_WGS_72 */
          0.0,     0.0,    4.5,     0.0,     0.0,    -0.554,  0.22    },
    { /* PL_DATUM_ED_50 */
        -87.0,   -98.0,  -121.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_ED_79 */
        -86.0,   -98.0,  -119.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_ED_87 */
        -82.5,   -91.7,  -117.7,    0.1338, -0.0625, -0.047,  0.045   },
    { /* PL_DATUM_AUSTRIA_NS */
        595.6,    87.3,   473.3,    4.7994,  0.0671,  5.7850, 2.555   },
    { /* PL_DATUM_BELGIUM_50 */
        -55.0,    49.0,  -158.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_BERNE_1873 */
        649.0,     9.0,   376.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_CH_1903 */
        660.1,    13.1,   369.2,    0.8048,  0.5777,  0.9522, 5.660   },
    { /* PL_DATUM_DANISH_GI_1934 */
        662.0,    18.0,   734.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_NOUV_TRIG_DE_FRANCE_GREENWICH */
       -168.0,   -60.0,   320.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_NOUV_TRIG_DE_FRANCE_PARIS */
       -168.0,   -60.0,   320.0,    0.0,     0.0,  8414.03,   0.0     },
    { /* PL_DATUM_POTSDAM */
        587.0,    16.0,   393.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_GGRS_87 */
        199.6,   -75.1,  -246.3,    0.0202,  0.0034,  0.0135, -0.015  },
    { /* PL_DATUM_HJORSEY_55 */
        -73.0,    46.0,   -86.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_IRELAND_65 */
        506.0,  -122.0,   611.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_ITALY_1940 */
       -133.0,   -50.0,    97.0,    0.0,     0.0, 44828.40,   0.0     },
    { /* PL_DATUM_NOUV_TRIG_DE_LUX */
       -262.0,    75.0,    25.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_NETHERLANDS_1921 */
        719.0,    47.0,   640.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_OSGB_36 */
        375.0,  -111.0,   431.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_PORTUGAL_DLX */
        504.1,  -220.9,   563.0,    0.0,     0.0,    -0.554,  0.220   },
    { /* PL_DATUM_PORTUGAL_1973 */
        227.0,    97.5,    35.4,    0.0,     0.0,    -0.554,  0.220   },
    { /* PL_DATUM_RNB_72 */
       -104.0,    80.0,   -75.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_RT_90 */
        424.3,   -80.5,   613.1,    4.3965, -1.9866,  5.1846, 0.0     },
    { /* PL_DATUM_NAD_27 */
         -8.0,   160.0,   176.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_NAD_83 */
          0.0,     0.0,     0.0,    0.0,     0.0,     0.0,    0.0     },
    { /* PL_DATUM_ETRS_89 */
          0.0,     0.0,     0.0,    0.0,     0.0,     0.0,    0.0     }
};

struct pl_matrix {
    double elements[4][4];
};

static struct pl_matrix pl_affine_transform_make(
        double Rx, double Ry, double Rz, double M,
        double Dx, double Dy, double Dz) {
    /* store in column-major order */
    struct pl_matrix matrix = {
        .elements = {
            {      M,  M * Rz,  -M * Ry, 0.f },
            {-M * Rz,       M,   M * Rx, 0.f },
            { M * Ry, -M * Rx,        M, 0.f },
            {     Dx,      Dy,       Dz, 1.f }
        }
    };
    return matrix;
}

static const char *_pl_proj_name(PLProjection proj) {
    int i=0;
    const char *name = NULL;
    for (i=0; i<sizeof(_pl_projection_info)/sizeof(_pl_projection_info[0]); i++) {
        if (_pl_projection_info[i].proj == proj) {
            name = _pl_projection_info[i].name;
            break;
        }
    }
    return name;
}

static double _pl_mlfn(double phi, double sphi, double cphi, double *en) {
	cphi *= sphi;
	sphi *= sphi;
	return(en[0] * phi - cphi * (en[1] + sphi*(en[2] + sphi*(en[3] + sphi*en[4]))));
}

static double _pl_qsfn(double sinphi, double e, double one_es) {
	double con = e * sinphi;
	return (one_es * (sinphi / (1.0 - con * con) - (.5 / e) * (log1p(-con) - log1p(con))));
}

static double _pl_msfn(double sinphi, double cosphi, double es) {
	return (cosphi / sqrt(1.0 - es * sinphi * sinphi));
}

static double _pl_tsfn(double phi, double sinphi, double e) {
	double con = e * sinphi;
	return (tan(.5 * (M_PI_2 - phi)) /
            pow((1.0 - con) / (1.0 + con), .5 * e));
}

static cl_int _pl_set_projection_kernel_args(PLContext *ctx, cl_kernel kernel,
        cl_mem xy_in, cl_mem xy_out, size_t count, PLSpheroidInfo *info, int *offset_ptr) {
	cl_int error = 0;
	cl_uint vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
	int offset = *offset_ptr;

	error |= pl_set_kernel_arg_mem(ctx, kernel, offset++, xy_in);
	error |= pl_set_kernel_arg_mem(ctx, kernel, offset++, xy_out);
	error |= pl_set_kernel_arg_uint(ctx, kernel, offset++, vec_count);

	if (!_pl_spheroid_is_spherical(info->tag)) {
        error |= pl_set_kernel_arg_float(ctx, kernel, offset++, info->ecc);
        error |= pl_set_kernel_arg_float(ctx, kernel, offset++, info->ecc2);
        error |= pl_set_kernel_arg_float(ctx, kernel, offset++, info->one_ecc2);
	}

	*offset_ptr = offset;

	return error;
}

cl_kernel pl_find_projection_kernel(PLContext *pl_ctx, PLProjection proj, int fwd, PLSpheroid ell) {
	char requested_name[128];
	if (fwd) {
		sprintf(requested_name, "pl_project_%s_%c", _pl_proj_name(proj), _pl_spheroid_is_spherical(ell) ? 's' : 'e');
	} else {
		sprintf(requested_name, "pl_unproject_%s_%c", _pl_proj_name(proj), _pl_spheroid_is_spherical(ell) ? 's' : 'e');
	}
	return pl_find_kernel(pl_ctx, requested_name);
}

static cl_int _pl_enqueue_kernel_1d(cl_command_queue queue, cl_kernel kernel, size_t dim_count) {
    cl_int error = CL_SUCCESS;
	error = clEnqueueNDRangeKernel(queue, kernel, 1, NULL, &dim_count, NULL, 0, NULL, NULL);
    return error;
}

cl_int pl_read_buffer(cl_command_queue queue, cl_mem xy_out_buf, float *xy_out, size_t out_count) {
    cl_int error;

    error = clEnqueueReadBuffer(queue, xy_out_buf, CL_TRUE, 0, out_count, xy_out, 0, NULL, NULL);
	if (error != CL_SUCCESS)
		return error;

	error = clFinish(queue);
	if (error != CL_SUCCESS)
		return error;

	return CL_SUCCESS;
}

static cl_int _pl_enqueue_projection_kernel(PLContext *pl_ctx, cl_kernel kernel, PLProjection proj, PLProjectionParams *params,
        cl_mem xy_in, cl_mem xy_out, size_t count) {
    cl_int error = CL_SUCCESS;
    int i;
    for (i=0; i<sizeof(_pl_projection_info)/sizeof(_pl_projection_info[0]); i++) {
        if (_pl_projection_info[i].proj == proj) {
            error = _pl_projection_info[i].func(pl_ctx, kernel, params, xy_in, xy_out, count);
            break;
        }
    }
    return error;
}

cl_int pl_enqueue_projection_kernel_points(PLContext *pl_ctx, cl_kernel kernel, PLProjection proj, PLProjectionParams *params,
        PLProjectionBuffer *pl_buf) {
    return _pl_enqueue_projection_kernel(pl_ctx, kernel, proj, params, pl_buf->xy_in, pl_buf->xy_out, pl_buf->count);
}

cl_int pl_enqueue_projection_kernel_grid(PLContext *pl_ctx, cl_kernel kernel, PLProjection proj, PLProjectionParams *params,
        PLPointGridBuffer *src, PLPointGridBuffer *dst) {
    return _pl_enqueue_projection_kernel(pl_ctx, kernel, proj, params, src->grid, dst->grid, src->width * src->height);
}

cl_int pl_enqueue_kernel_albers_equal_area(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
	cl_int error = CL_SUCCESS;
	cl_int argc = 0;
	size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
	PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
	error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &argc);

	double phi1 = params->rlat1 * DEG_TO_RAD;
	double phi2 = params->rlat2 * DEG_TO_RAD;
	double phi0 = params->lat0 * DEG_TO_RAD;

	double rho0;
	double c, n;
    double sinphi, cosphi;

	n = sinphi = sin(phi1);
	cosphi = cos(phi1);

	if (_pl_spheroid_is_spherical(params->spheroid)) {
        n = .5 * (sinphi + sin(phi2));
        c = 1.0 + sin(phi2) * sinphi;
        rho0 = sqrt(c - 2.f * n * sin(phi0));
	} else {
		double ml1, m1;

		m1 = _pl_msfn(sinphi, cosphi, info.ecc2);
		ml1 = _pl_qsfn(sinphi, info.ecc, info.one_ecc2);
		if (fabs(phi1 - phi2) >= EPS7) {
			double ml2, m2;

			sinphi = sin(phi2);
			cosphi = cos(phi2);
			m2 = _pl_msfn(sinphi, cosphi, info.ecc2);
			ml2 = _pl_qsfn(sinphi, info.ecc, info.one_ecc2);
			n = (m1 * m1 - m2 * m2) / (ml2 - ml1);
		}

		c = m1 * m1 + ml1 * n;
		rho0 = sqrt(c - n * _pl_qsfn(sin(phi0), info.ecc, info.one_ecc2));
	}

	if (!_pl_spheroid_is_spherical(params->spheroid)) {
		error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, info.ec);
	}
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->scale * info.major_axis / n);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->y0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->lon0 * DEG_TO_RAD);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, rho0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, c);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, n);

	if (error != CL_SUCCESS)
		return error;

	return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_american_polyconic(PLContext *pl_ctx,cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
	cl_int error;
	cl_int offset = 0;
	size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
	PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
	error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &offset);

	double phi0 = params->lat0 * DEG_TO_RAD;

	double ml0 = _pl_mlfn(phi0, sin(phi0), cos(phi0), info.en);

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->scale * info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->y0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, phi0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->lon0 * DEG_TO_RAD);
    if (!_pl_spheroid_is_spherical(params->spheroid)) {
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, ml0);
        error |= pl_set_kernel_arg_float8(pl_ctx, kernel, offset++, info.en);
    }
	if (error != CL_SUCCESS) {
		return error;
	}

	return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_lambert_azimuthal_equal_area(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
    cl_int error;
    cl_int offset = 0;
    size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
	PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
	error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &offset);

	double phi0 = params->lat0 * DEG_TO_RAD;

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->scale * info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->y0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, phi0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->lon0 * DEG_TO_RAD);
    if (_pl_spheroid_is_spherical(params->spheroid)) {
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, sin(phi0));
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, cos(phi0));
    } else {
        double qp = _pl_qsfn(1.0, info.ecc, info.one_ecc2);
        double sinPhi = sin(phi0);
        double sinB1 = _pl_qsfn(sinPhi, info.ecc, info.one_ecc2) / qp;
        double cosB1 = sqrt(1.0 - sinB1 * sinB1);

        double rq = sqrt(0.5 * qp);
        double dd = cos(phi0) / (sqrt(1.0 - info.ecc2 * sinPhi * sinPhi) * rq * cosB1);
        double ymf = rq / dd;
        double xmf = rq * dd;

        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, qp);
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, sinB1);
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, cosB1);

        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, rq);
        error |= pl_set_kernel_arg_float4(pl_ctx, kernel, offset++, info.apa);

        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, dd);
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, xmf);
        error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, ymf);
    }
    if (error != CL_SUCCESS) {
		return error;
	}

    return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_lambert_conformal_conic(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
    cl_int error;
    cl_int offset = 0;

    size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
    PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
    error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &offset);

    double phi0 = params->lat0 * DEG_TO_RAD;
    double phi1 = params->rlat1 * DEG_TO_RAD;
    double phi2 = params->rlat2 * DEG_TO_RAD;

    double rho0, c, n, sinphi1, cosphi1, sinphi2;
    int secant = 0;

    sinphi1 = sin(phi1);
    cosphi1 = cos(phi1);
    if (fabs(phi1 - phi2) < 1.e-7) {
        n = sinphi1;
    } else {
        secant = 1;
    }

    if (_pl_spheroid_is_spherical(params->spheroid)) {
        if (secant)
            n = log(cosphi1 / cos(phi2)) / (asinh(tan(phi2)) - asinh(tan(phi1)));
        c = cosphi1 * pow(tan(M_PI_4 + .5 * phi1), n) / n;
        rho0 = c * pow(tan(M_PI_4 + .5 * phi0), -n);
    } else {
        double m1, ml1;

        m1 = _pl_msfn(sinphi1, cosphi1, info.ecc2);
        ml1 = _pl_tsfn(phi1, sinphi1, info.ecc);
        if (secant) {
            sinphi2 = sin(phi2);
            n = log(m1 / _pl_msfn(sinphi2, cos(phi2), info.ecc2));
            n /= log(ml1 / _pl_tsfn(phi2, sinphi2, info.ecc));
        }
        c = m1 * pow(ml1, -n) / n;
        rho0 = c * pow(_pl_tsfn(phi0, sin(phi0), info.ecc), n);
    }

    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->scale * info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->y0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->lon0 * DEG_TO_RAD);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, rho0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, c);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, n);

    if (error != CL_SUCCESS) {
		return error;
	}

    return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_mercator(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
	cl_int error;
	cl_int offset = 0;
	size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

	PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
	error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &offset);

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->scale * info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, offset++, params->y0);
	if (error != CL_SUCCESS) {
		return error;
	}
	return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_oblique_stereographic(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
	cl_int error = CL_SUCCESS;
	cl_int argc = 0;
	size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;
	PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);

	error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &argc);

    double phi0 = params->lat0 * DEG_TO_RAD;

    double sinPhi0, cosPhi0;
    sinPhi0 = sin(phi0);
    cosPhi0 = cos(phi0);

    double sinPhiC0, cosPhiC0;
    double scale_r2 = 2.0 * params->scale * info.major_axis * sqrt(info.one_ecc2) / (1.0 - info.ecc2 * sinPhi0 * sinPhi0);

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, scale_r2);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->x0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->y0);

	if (!_pl_spheroid_is_spherical(params->spheroid)) {
        double c0 = sqrt(1.0 + info.ecc2 * cosPhi0 * cosPhi0 * cosPhi0 * cosPhi0 / info.one_ecc2);
        double phiC0 = asin(sinPhi0 / c0);
        sinPhiC0 = sin(phiC0);
        cosPhiC0 = cos(phiC0);

        double k0 = tan(0.5 * phiC0 + M_PI_4) / (
                pow(tan(0.5 * phi0 + M_PI_4), c0) *
                pow((1.-info.ecc * sinPhi0)/(1.+info.ecc*sinPhi0), 0.5 * c0 * info.ecc) );

		error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, c0);
		error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, log(k0));
	} else {
        sinPhiC0 = sinPhi0;
        cosPhiC0 = cosPhi0;
    }

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->lon0 * DEG_TO_RAD);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, sinPhiC0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, cosPhiC0);
	if (error != CL_SUCCESS)
		return error;

	return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_robinson(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
	cl_int error = CL_SUCCESS;
	cl_int argc = 0;

	PLSpheroidInfo info = _pl_get_spheroid_info(PL_SPHEROID_SPHERE);

	error |= pl_set_kernel_arg_mem(pl_ctx, kernel, argc++, xy_in);
	error |= pl_set_kernel_arg_mem(pl_ctx, kernel, argc++, xy_out);
	error |= pl_set_kernel_arg_uint(pl_ctx, kernel, argc++, count);

	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->scale * info.major_axis);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->x0);
	error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->y0);
	if (error != CL_SUCCESS)
		return error;

	return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, count);
}

cl_int pl_enqueue_kernel_transverse_mercator(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
    cl_int error = CL_SUCCESS;
    cl_int argc = 0;
    size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    PLSpheroidInfo info = _pl_get_spheroid_info(params->spheroid);
    error = _pl_set_projection_kernel_args(pl_ctx, kernel, xy_in, xy_out, count, &info, &argc);

    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->scale * info.major_axis * info.krueger_A);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->y0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->lon0 * DEG_TO_RAD);
    if (!_pl_spheroid_is_spherical(params->spheroid)) {
        error |= pl_set_kernel_arg_float8(pl_ctx, kernel, argc++, info.krueger_alpha);
        error |= pl_set_kernel_arg_float8(pl_ctx, kernel, argc++, info.krueger_beta);
    }
    if (error != CL_SUCCESS)
        return error;

    return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_enqueue_kernel_winkel_tripel(PLContext *pl_ctx, cl_kernel kernel,
        PLProjectionParams *params, cl_mem xy_in, cl_mem xy_out, size_t count) {
    cl_int error = CL_SUCCESS;
    int argc = 0;

    size_t vec_count = ck_padding(count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    PLSpheroidInfo info = _pl_get_spheroid_info(PL_SPHEROID_SPHERE);

    double cosphi1 = isnan(params->rlat1) ? M_2_PI : cos(params->rlat1 * DEG_TO_RAD);

    error |= pl_set_kernel_arg_mem(pl_ctx, kernel, argc++, xy_in);
	error |= pl_set_kernel_arg_mem(pl_ctx, kernel, argc++, xy_out);
	error |= pl_set_kernel_arg_uint(pl_ctx, kernel, argc++, vec_count);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->scale * info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->x0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->y0);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, params->lon0 * DEG_TO_RAD);
    error |= pl_set_kernel_arg_float(pl_ctx, kernel, argc++, cosphi1);

    if (error != CL_SUCCESS)
        return error;

    return _pl_enqueue_kernel_1d(pl_ctx->queue, kernel, vec_count);
}

cl_int pl_run_kernel_inverse_geodesic( PLContext *pl_ctx, cl_kernel inv_kernel,
        PLInverseGeodesicBuffer *pl_buf, float *dist_out, PLSpheroid pl_ell, double scale) {
	int argc = 0;
	cl_int error = CL_SUCCESS;
	PLSpheroidInfo info = _pl_get_spheroid_info(pl_ell);

	size_t xy2VecCount = ck_padding(pl_buf->xy2_count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

	error |= pl_set_kernel_arg_mem(pl_ctx, inv_kernel, argc++, pl_buf->xy1_in);
	error |= pl_set_kernel_arg_mem(pl_ctx, inv_kernel, argc++, pl_buf->xy2_in);
	error |= pl_set_kernel_arg_mem(pl_ctx, inv_kernel, argc++, pl_buf->dist_out);
	error |= pl_set_kernel_arg_float(pl_ctx, inv_kernel, argc++, info.major_axis);

	if (error != CL_SUCCESS) {
		return error;
	}

	const size_t dim[2] = { pl_buf->xy1_count, xy2VecCount };

	error = clEnqueueNDRangeKernel(pl_ctx->queue, inv_kernel, 2, NULL, dim, NULL, 0, NULL, NULL);
	if (error != CL_SUCCESS) {
		return error;
	}

	float *dist_pad_out;
	size_t pad_count = 2 * pl_buf->xy1_count * ck_padding(pl_buf->xy2_count, PL_FLOAT_VECTOR_SIZE);
	if ((dist_pad_out = malloc(sizeof(float) * pad_count)) == NULL) {
		return CL_OUT_OF_HOST_MEMORY;
	}

	error = clEnqueueReadBuffer(pl_ctx->queue, pl_buf->dist_out, CL_TRUE, 0, pad_count * sizeof(cl_float),
								dist_pad_out, 0, NULL, NULL);
	if (error != CL_SUCCESS) {
		free(dist_pad_out);
		return error;
	}

	error = clFinish(pl_ctx->queue);
	if (error != CL_SUCCESS) {
		free(dist_pad_out);
		return error;
	}

	int j_size = ck_padding(pl_buf->xy2_count, PL_FLOAT_VECTOR_SIZE);
    int i, j;

	for (i=0; i<pl_buf->xy1_count; i++) {
		for (j=0; j<pl_buf->xy2_count; j++) {
			dist_out[i*pl_buf->xy2_count+j] = dist_pad_out[i*j_size+j] * scale;
		}
	}

	free(dist_pad_out);

	return CL_SUCCESS;
}

cl_int pl_run_kernel_forward_geodesic_fixed_distance(PLContext *pl_ctx, cl_kernel fwd_kernel,
    PLForwardGeodesicFixedDistanceBuffer *pl_buf, float *xy_out, PLSpheroid pl_ell, double distance) {
	PLSpheroidInfo info = _pl_get_spheroid_info(pl_ell);

	int argc = 0;

	cl_int error = CL_SUCCESS;

	double D, sinD, cosD;

	D = distance / info.major_axis;
	sinD = sin(D);
	cosD = cos(D);

	error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->xy_in);
	error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->phi_sincos);
	error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->az_sincos);
	error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->xy_out);
	error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, D);
	error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, sinD);
	error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, cosD);
	if (!_pl_spheroid_is_spherical(pl_ell)) {
		double flattening = 1.f/info.inverse_flattening;
		error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, flattening);
	}
	if (error != CL_SUCCESS) {
		return error;
	}

	const size_t dim[2] = { pl_buf->xy_count,
		ck_padding(pl_buf->az_count, PL_FLOAT_VECTOR_SIZE)/PL_FLOAT_VECTOR_SIZE };

	error = clEnqueueNDRangeKernel(pl_ctx->queue, fwd_kernel, 2, NULL,
								   dim, NULL, 0, NULL, NULL);
	if (error != CL_SUCCESS) {
		return error;
	}

	error = clEnqueueReadBuffer(pl_ctx->queue, pl_buf->xy_out, CL_TRUE, 0,
								2 * pl_buf->xy_count * pl_buf->az_count * sizeof(cl_float),
								xy_out, 0, NULL, NULL);
	if (error != CL_SUCCESS) {
		return error;
	}

	error = clFinish(pl_ctx->queue);
	if (error != CL_SUCCESS) {
		return error;
	}

	return CL_SUCCESS;
}

cl_int pl_run_kernel_forward_geodesic_fixed_angle(PLContext *pl_ctx, cl_kernel fwd_kernel,
    PLForwardGeodesicFixedAngleBuffer *pl_buf, double xy_in[2], float *xy_out, PLSpheroid pl_ell, double angle) {
    int argc = 0;

    size_t distVecCount = ck_padding(pl_buf->dist_count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    cl_int error = CL_SUCCESS;

    double sinAzimuth = sin(angle);
    double cosAzimuth = cos(angle);

    error |= pl_set_kernel_arg_float2(pl_ctx, fwd_kernel, argc++, xy_in);
    error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->dist_in);
    error |= pl_set_kernel_arg_mem(pl_ctx, fwd_kernel, argc++, pl_buf->xy_out);
    error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, angle);
    error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, sinAzimuth);
    error |= pl_set_kernel_arg_float(pl_ctx, fwd_kernel, argc++, cosAzimuth);
    if (error != CL_SUCCESS) {
        return error;
    }

    error = clEnqueueNDRangeKernel(pl_ctx->queue, fwd_kernel, 1, NULL, &distVecCount,
                                   NULL, 0, NULL, NULL);
    if (error != CL_SUCCESS) {
        return error;
    }

    error = clEnqueueReadBuffer(pl_ctx->queue, pl_buf->xy_out, CL_TRUE, 0,
                                2 * pl_buf->dist_count * sizeof(cl_float),
                                xy_out, 0, NULL, NULL);
    if (error != CL_SUCCESS) {
        return error;
    }

    error = clFinish(pl_ctx->queue);
    if (error != CL_SUCCESS) {
        return error;
    }

    return CL_SUCCESS;
}

cl_int pl_run_kernel_geodesic_to_cartesian(PLContext *pl_ctx, cl_kernel g2c_kernel,
        PLDatumShiftBuffer *pl_buf, PLSpheroid pl_ell) {
    PLSpheroidInfo info = _pl_get_spheroid_info(pl_ell);

    int argc = 0;
    cl_int error = CL_SUCCESS;
    size_t vec_count = ck_padding(pl_buf->count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    error |= pl_set_kernel_arg_mem(pl_ctx, g2c_kernel, argc++, pl_buf->xy_in);
    error |= pl_set_kernel_arg_mem(pl_ctx, g2c_kernel, argc++, pl_buf->x_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, g2c_kernel, argc++, pl_buf->y_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, g2c_kernel, argc++, pl_buf->z_rw);

    error |= pl_set_kernel_arg_float(pl_ctx, g2c_kernel, argc++, info.ecc);
    error |= pl_set_kernel_arg_float(pl_ctx, g2c_kernel, argc++, info.ecc2);
    error |= pl_set_kernel_arg_float(pl_ctx, g2c_kernel, argc++, info.one_ecc2);

    error |= pl_set_kernel_arg_float(pl_ctx, g2c_kernel, argc++, info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, g2c_kernel, argc++, info.minor_axis);

    if (error != CL_SUCCESS)
        return error;

    error = clEnqueueNDRangeKernel(pl_ctx->queue, g2c_kernel, 1, NULL, &vec_count, NULL, 0, NULL, NULL);
    if (error != CL_SUCCESS)
        return error;

    error = clFinish(pl_ctx->queue);
    if (error != CL_SUCCESS)
        return error;

    return CL_SUCCESS;
}

/* We cut the computations in half by doing some matrix algebra beforehand.
 * Each datum shift is essentially an affine transformation in 3D cartesian
 * coordinates. So to get from on datum to another, instead of transforming
 * to/from WGS 84, we concatenate the transformation matrix of the source
 * datum to the inverse transformation matrix of the destination matrix, and
 * then just do a single matrix multiplication on each point instead of two
 * in order to transform it.
 */
cl_int pl_run_kernel_transform_cartesian(PLContext *pl_ctx, cl_kernel transform_kernel,
        PLDatumShiftBuffer *pl_buf, PLDatum src_datum, PLDatum dst_datum) {
    double Rx1, Ry1, Rz1, M1, Dx1, Dy1, Dz1;
    double Rx2, Ry2, Rz2, M2, Dx2, Dy2, Dz2;

    cl_int error = CL_SUCCESS;
    int argc = 0;
    size_t vec_count = ck_padding(pl_buf->count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    Dx1 = pl_datum_params[src_datum].dx;
    Dy1 = pl_datum_params[src_datum].dy;
    Dz1 = pl_datum_params[src_datum].dz;
    M1  = pl_datum_params[src_datum].ppm*1.e-6 + 1;
    Rx1 = pl_datum_params[src_datum].ex * SEC_TO_RAD;
    Ry1 = pl_datum_params[src_datum].ey * SEC_TO_RAD;
    Rz1 = pl_datum_params[src_datum].ez * SEC_TO_RAD;

    Dx2 = pl_datum_params[dst_datum].dx;
    Dy2 = pl_datum_params[dst_datum].dy;
    Dz2 = pl_datum_params[dst_datum].dz;
    M2  = pl_datum_params[dst_datum].ppm*1.e-6 + 1;
    Rx2 = pl_datum_params[dst_datum].ex * SEC_TO_RAD;
    Ry2 = pl_datum_params[dst_datum].ey * SEC_TO_RAD;
    Rz2 = pl_datum_params[dst_datum].ez * SEC_TO_RAD;

    struct pl_matrix matrix1 = pl_affine_transform_make(Rx1, Ry1, Rz1, M1, Dx1, Dy1, Dz1);
    struct pl_matrix matrix2 = pl_affine_transform_make(Rx2, Ry2, Rz2, M2, Dx2, Dy2, Dz2);

    /* Invert the destination matrix */
    __CLPK_integer n = 4;
    __CLPK_integer info;

    __CLPK_integer ipiv[4];
    __CLPK_doublereal work[4];
    __CLPK_integer work_n = 4;

    dgetrf_(&n, &n, &matrix2.elements[0][0], &n, ipiv, &info);
    if (info != 0) {
        return info;
    }

    dgetri_(&n, &matrix2.elements[0][0], &n, ipiv, work, &work_n, &info);
    if (info != 0) {
        return info;
    }

    __CLPK_doublereal alpha = 1.0;
    __CLPK_doublereal beta = 0.0;
    __CLPK_doublereal result_matrix[4][4];

    /* Multiply the source matrix with the inverse destination matrix */
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, n, n, alpha,
          &matrix2.elements[0][0], n,
          &matrix1.elements[0][0], n,
          beta, &result_matrix[0][0], n);

    /* transpose the result and store single-precision */
    double tmatrix[4][4];
    int i, j;
    for (i=0; i<4; i++) {
        for (j=0; j<4; j++) {
            tmatrix[i][j] = result_matrix[j][i];
        }
    }

    error |= pl_set_kernel_arg_mem(pl_ctx, transform_kernel, argc++, pl_buf->x_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, transform_kernel, argc++, pl_buf->y_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, transform_kernel, argc++, pl_buf->z_rw);
    error |= pl_set_kernel_arg_float16(pl_ctx, transform_kernel, argc++, &tmatrix[0][0]);
    if (error != CL_SUCCESS)
        return error;

    error = clEnqueueNDRangeKernel(pl_ctx->queue, transform_kernel, 1, NULL,
                                   &vec_count, NULL, 0, NULL, NULL);
    if (error != CL_SUCCESS)
        return error;

    return error;
}

cl_int pl_run_kernel_cartesian_to_geodesic(PLContext *pl_ctx, cl_kernel c2g_kernel,
        PLDatumShiftBuffer *pl_buf, float *xy_out, PLSpheroid pl_ell) {
    PLSpheroidInfo info = _pl_get_spheroid_info(pl_ell);

    int argc = 0;
    cl_int error = CL_SUCCESS;
    size_t vec_count = ck_padding(pl_buf->count, PL_FLOAT_VECTOR_SIZE) / PL_FLOAT_VECTOR_SIZE;

    error |= pl_set_kernel_arg_mem(pl_ctx, c2g_kernel, argc++, pl_buf->x_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, c2g_kernel, argc++, pl_buf->y_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, c2g_kernel, argc++, pl_buf->z_rw);
    error |= pl_set_kernel_arg_mem(pl_ctx, c2g_kernel, argc++, pl_buf->xy_out);

    error |= pl_set_kernel_arg_float(pl_ctx, c2g_kernel, argc++, info.ecc);
    error |= pl_set_kernel_arg_float(pl_ctx, c2g_kernel, argc++, info.ecc2);
    error |= pl_set_kernel_arg_float(pl_ctx, c2g_kernel, argc++, info.one_ecc2);

    error |= pl_set_kernel_arg_float(pl_ctx, c2g_kernel, argc++, info.major_axis);
    error |= pl_set_kernel_arg_float(pl_ctx, c2g_kernel, argc++, info.minor_axis);
    if (error != CL_SUCCESS)
        return error;

    error = clEnqueueNDRangeKernel(pl_ctx->queue, c2g_kernel, 1, NULL,
                                   &vec_count, NULL, 0, NULL, NULL);
    if (error != CL_SUCCESS)
        return error;

    if (xy_out != NULL) {
        error = clEnqueueReadBuffer(pl_ctx->queue, pl_buf->xy_out, CL_TRUE, 0,
                                    2 * pl_buf->count * sizeof(cl_float), xy_out, 0, NULL, NULL);
        if (error != CL_SUCCESS)
            return error;
    }

    error = clFinish(pl_ctx->queue);
    if (error != CL_SUCCESS)
        return error;

    return CL_SUCCESS;
}


/* wavelet/haar.c
 *
 * Copyright (C) 2004 Ivo Alxneit
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_wavelet.h>

static const double ch_2[2] = { M_SQRT1_2, M_SQRT1_2 };
static const double cg_2[2] = { M_SQRT1_2, -(M_SQRT1_2) };

static int
haar_init (const double **h1, const double **g1, const double **h2,
           const double **g2, size_t * nc, size_t * offset,
           const size_t member)
{
  if (member != 2)
    {
      return GSL_FAILURE;
    }

  *h1 = ch_2;
  *g1 = cg_2;
  *h2 = ch_2;
  *g2 = cg_2;

  *nc = 2;
  *offset = 0;

  return GSL_SUCCESS;
}

static int
haar_centered_init (const double **h1, const double **g1, const double **h2,
                    const double **g2, size_t * nc, size_t * offset,
                    const size_t member)
{
  if (member != 2)
    {
      return GSL_FAILURE;
    }

  *h1 = ch_2;
  *g1 = cg_2;
  *h2 = ch_2;
  *g2 = cg_2;

  *nc = 2;
  *offset = 1;

  return GSL_SUCCESS;
}

static const gsl_wavelet_type haar_type = {
  "haar",
  &haar_init
};

static const gsl_wavelet_type haar_centered_type = {
  "haar-centered",
  &haar_centered_init
};

const gsl_wavelet_type *gsl_wavelet_haar = &haar_type;
const gsl_wavelet_type *gsl_wavelet_haar_centered = &haar_centered_type;


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifndef SCILIB_LINALG_MATRIX_NORM_H
#define SCILIB_LINALG_MATRIX_NORM_H

#ifdef USE_MKL
#include <mkl.h>
#else
#include <lapacke.h>
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg_impl/lapack_types.h>
#include <type_traits>

namespace Sci {
namespace Linalg {

namespace stdex = std::experimental;

// Matrix norm of a general rectangular matrix:
//
// Types of matrix norms:
// - M, m:       largest absolute value of the matrix
// - 1, O, o:    1-norm of the matrix (maximum column sum)
// - I, i:       infinity norm of the matrix (maximum row sum)
// - F, f, E, e: Frobenius norm of the matrix (square root of sum of squares)
//
template <class T, class Layout>
    requires(std::is_same_v<std::remove_cv_t<T>, double>)
inline auto matrix_norm(Sci::Matrix_view<T, Layout> a, char norm)
{
    static_assert(a.is_contiguous());

    assert(norm == 'M' || norm == 'm' || norm == '1' || norm == 'O' ||
           norm == 'o' || norm == 'I' || norm == 'i' || norm == 'F' ||
           norm == 'f' || norm == 'E' || norm == 'e');

    auto matrix_layout = LAPACK_ROW_MAJOR;
    BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = LAPACK_COL_MAJOR;
        lda = m;
    }
    return LAPACKE_dlange(matrix_layout, norm, m, n, a.data(), lda);
}

template <class Layout, class Allocator>
inline double matrix_norm(const Sci::Matrix<double, Layout, Allocator>& a,
                          char norm)
{
    return matrix_norm(a.view(), norm);
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_MATRIX_NORM_H


/*
 *  Copyright 2017 NVIDIA Corporation
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

/* include the ATLAS headers */

extern "C"
{
#include <cblas.h>
}

/* choose precision to train and classify.  Only float and double are
 * currently suppored
 */

typedef float floatType_t;

/* macro to convert 2d coords to 1d offset */

#define INDX(row,col,ld) (((col) * (ld)) + (row))

inline float sigmoid_f( float z )
{
  return 1.0f / ( 1.0f + expf( -z ) );
} /* end sigmoid_f */

inline double sigmoid( double z )
{
  return 1.0 / ( 1.0 + exp( -z ) );
} /* end sigmoid */

inline float sigmoidGradient_f( float z )
{
  float temp = sigmoid_f( z );
  return temp * ( 1.0f - temp );
} /* end sigGrad_f */

inline double sigmoidGradient( double z )
{
  double temp = sigmoid( z );
  return temp * ( 1.0 - temp );
} /* end sigGrad_f */

/* hardcoded constants for training and test set size and feature
 * vector size
 */

#define FEATURE_VECTOR_SIZE (784)
#define TRAINING_SET_SIZE (60000)
#define TEST_SET_SIZE (10000)
#define HIDDEN_LAYER_SIZE (25)
#define NUM_OUTPUT_CLASSES (10)

/* CUDA debugging */

#ifdef DEBUG
#define CUDA_CALL(F)  if( (F) != cudaSuccess ) \
  {printf("Error %s at %s:%d\n", cudaGetErrorString(cudaGetLastError()), \
   __FILE__,__LINE__); exit(-1);}
#define CUDA_CHECK()  if( (cudaPeekAtLastError()) != cudaSuccess ) \
  {printf("Error %s at %s:%d\n", cudaGetErrorString(cudaGetLastError()), \
   __FILE__,__LINE__-1); exit(-1);}
#else
#define CUDA_CALL(F) (F)
#define CUDA_CHECK()
#endif

/* function defs */

void readMatrixFromFile( char *, float *, const int, const int, const int );
void readCommandLineArgs( int, char *[], float *, int *, int *, int *);

void calcGradient( floatType_t *X,
                   int const XRows,
                   int const XCols,
                   floatType_t const *theta1,
                   int         const theta1Rows,
                   int         const theta1Cols,
                   floatType_t const *theta2,
                   int         const theta2Rows,
                   int         const theta2Cols,
                   floatType_t const *Y,
                   floatType_t *cost,
                   floatType_t       *theta1Grad,
                   floatType_t       *theta2Grad,
                   floatType_t       *tempMatrix );
void predict( floatType_t *X,
                   int const XRows,
                   int const XCols,
                   floatType_t const *theta1,
                   int         const theta1Rows,
                   int         const theta1Cols,
                   floatType_t const *theta2,
                   int         const theta2Rows,
                   int         const theta2Cols,
                   int               *predictVector);
void trainNetwork( floatType_t       *X,
                   int         const XRows,
                   int         const XCols,
                   floatType_t       *theta1,
                   int         const theta1Rows,
                   int         const theta1Cols,
                   floatType_t       *theta2,
                   int         const theta2Rows,
                   int         const theta2Cols,
                   floatType_t const *Y,
                   float       const learningRate,
                   int         const iterations,
                   int         const batchSize );


#if !defined(UTIL_H)
#define UTIL_H

#include <gsl/gsl_matrix.h>

// Flag to set if the program should exit on GSL error
const static bool EXIT_ON_GSL_ERROR = false;

struct stability_params {
  double lambda[8];
  double l;
  double u;
  double m;
  double v;
};

struct integration_params {
  double M;
  double m1;
  double m2;
  double gamma;
};

struct integration_params2 {
  double M;
  double x1;
  double m;
  double gamma;
};

struct integration_params_tt {
  double M;
  double mW;
  double mb;
  double mt;
  double x1;
  double gtop;
  int h;
};

struct integration_params_tb {
  double M;
  double mW;
  double mb;
  double mt;
  double x1;
  double gtop;
  double md;
  double Z_u;
  double Z_d;
};

int sign(double x);

double stability_fcn(double x, void *params);

bool stability_minimum(stability_params &p);

void print_gsl_matrix(gsl_matrix *mat,int m,int n);

double fint(double x, void* par);

double gint(double x, void* par);

double Lint_s(double x, void *param);

double Lint_p(double x, void *param);

double Lint_c(double x, void *param);

double hvv_fcn(double y, void *params);

double hvv_fcn1(double y, void *params);

double hvv_fcn2(double y, void *params);

double htt_fcn1(double y, void *params);

double htt_fcn2(double y, void *params);

double htb_fcn1(double y, void *params);

double htb_fcn2(double y, void *params);

double cubic(double x, double X[4], double Y[4]);

double hvh_fcn(double x, void *params);

double  L(double x, double y, double z);
double  DHp(double ui, double uj, double xi, double xj, double sqL);
double  DHm(double ui, double uj, double xi, double xj, double sqL);
double  BHp(double ui, double uj, double xi, double xj, double sqL);
double sqrtlambda(double a1, double a2, double a3);

#endif


/* permutation/permutation.c
 *
 * Copyright (C) 2001, 2002 Nicolas Darnis
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Modified for GSL by Brian Gough.
 * Use in-place algorithms, no need for workspace
 * Use conventions for canonical form given in Knuth (opposite of Sedgewick)
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_permutation.h>

int
gsl_permutation_linear_to_canonical (gsl_permutation * q,
                                     const gsl_permutation * p)
{
  const size_t n = p->size;
  size_t i, k, s;
  size_t t = n;

  const size_t *const pp = p->data;
  size_t *const qq = q->data;

  if (q->size != p->size)
    {
      GSL_ERROR ("size of q does not match size of p", GSL_EINVAL);
    }

  for (i = 0; i < n; i++)
    {

      k = pp[i];
      s = 1;

      while (k > i)
        {
          k = pp[k];
          s++;
        }

      if (k < i)
        continue;

      /* Now have k == i, i.e the least in its cycle, and s == cycle length */

      t -= s;

      qq[t] = i;

      k = pp[i];
      s = 1;

      while (k > i)
        {
          qq[t + s] = k;
          k = pp[k];
          s++;
        }

      if (t == 0)
        break;
    }

  return GSL_SUCCESS;
}

int
gsl_permutation_canonical_to_linear (gsl_permutation * p,
                                     const gsl_permutation * q)
{
  size_t i, k, kk, first;
  const size_t n = p->size;

  size_t *const pp = p->data;
  const size_t *const qq = q->data;

  if (q->size != p->size)
    {
      GSL_ERROR ("size of q does not match size of p", GSL_EINVAL);
    }

  for (i = 0; i < n; i++)
    {
      pp[i] = i;
    }

  k = qq[0];
  first = pp[k];

  for (i = 1; i < n; i++)
    {
      kk = qq[i];

      if (kk > first)
        {
          pp[k] = pp[kk];
          k = kk;
        }
      else
        {
          pp[k] = first;
          k = kk;
          first = pp[kk];
        }
    }

  pp[k] = first;

  return GSL_SUCCESS;
}

size_t
gsl_permutation_inversions (const gsl_permutation * p)
{
  size_t count = 0;
  size_t i, j;
  const size_t size = p->size;

  for (i = 0; i < size - 1; i++)
    {
      for (j = i + 1; j < size; j++)
        {
          if (p->data[i] > p->data[j])
            {
              count++;
            }
        }
    }

  return count;
}

size_t
gsl_permutation_linear_cycles (const gsl_permutation * p)
{
  size_t i, k;
  size_t count = 0;
  const size_t size = p->size;

  for (i = 0; i < size; i++)
    {

      k = p->data[i];

      while (k > i)
        {
          k = p->data[k];
        }

      if (k < i)
        continue;

      count++;
    }

  return count;
}

size_t
gsl_permutation_canonical_cycles (const gsl_permutation * p)
{
  size_t i;
  size_t count = 1;
  size_t min = p->data[0];

  for (i = 0; i < p->size; i++)
    {
      if (p->data[i] < min)
        {
          min = p->data[i];
          count++;
        }
    }

  return count;
}


//
// Created by L. Jonathan Feldstein
//

#ifndef CIMPLE_CIMPLE_CONTROLLER_H
#define CIMPLE_CIMPLE_CONTROLLER_H

#include <stddef.h>
#include <math.h>
#include "cimple_system.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include "cimple_polytope_library.h"
#include <pthread.h>
#include "cimple_mpc_computation.h"

/**
 * @brief Action to get plant from current abstract state to target_abs_state.
 *
 * @param target target region the plant is supposed to reach
 * @param now current state of the plant
 * @param d_dyn discrete abstraction of the system
 * @param s_dyn system dynamics including auxiliary matrices
 * @param f_cost cost function to be minimized on the path
 */
void ACT(int target,
         current_state * now,
         discrete_dynamics * d_dyn,
         system_dynamics * s_dyn,
         cost_function * f_cost,
         double sec);
/**
 * @brief Apply the calculated control to the current state using system dynamics
 * @param x current state at time [0]
 * @param u matrix with next N inputs calculated by the MPC controller
 * @param A system dynamics
 * @param B input dynamics
 */
void apply_control(gsl_vector *x,
                   gsl_vector *u,
                   gsl_matrix *A,
                   gsl_matrix *B,
                   gsl_matrix *E,
                   gsl_vector *w,
                   size_t current_time);

/**
 * Fill a vector with gaussian distributed noise
 * @param w
 */
void simulate_disturbance(gsl_vector *w,
                          double mu,
                          double sigma);

void * main_computation(void *arg);

#endif //CIMPLE_CIMPLE_CONTROLLER_H


/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <math.h>

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>

#include "sip-utils.h"
#include "tweak.h"
#include "healpix.h"
#include "dualtree_rangesearch.h"
#include "kdtree_fits_io.h"
#include "mathutil.h"
#include "log.h"
#include "permutedsort.h"
#include "gslutils.h"
#include "errors.h"
#include "fit-wcs.h"

// TODO:
//
//  1. Write the document which explains every step of tweak in detail with
//     comments relating exactly to the code.
//  2. Implement polynomial terms - use order parameter to zero out A matrix
//  3. Make it robust to outliers
//     - Make jitter evolve as fit evolves
//     - Sigma clipping
//  4. Need test image with non-trivial rotation to test CD transpose problem
//
//
//  - Put CD inverse into its own function
//
//  BUG? transpose of CD matrix is similar to CD matrix!
//  BUG? inverse when computing sx/sy (i.e. same transpose issue)
//  Ability to fit without re-doing correspondences
//  Split fit x/y (i.e. two fits one for x one for y)

#define KERNEL_SIZE 5
#define KERNEL_MARG ((KERNEL_SIZE-1)/2)

sip_t* tweak_just_do_it(const tan_t* wcs, const starxy_t* imagexy,
                        const double* starxyz,
                        const double* star_ra, const double* star_dec,
                        const double* star_radec,
                        int nstars, double jitter_arcsec,
                        int order, int inverse_order, int iterations,
                        anbool weighted, anbool skip_shift) {
    tweak_t* twee = NULL;
    sip_t* sip = NULL;

    twee = tweak_new();
    twee->jitter = jitter_arcsec;
    twee->sip->a_order  = twee->sip->b_order  = order;
    twee->sip->ap_order = twee->sip->bp_order = inverse_order;
    twee->weighted_fit = weighted;
    if (skip_shift)
        tweak_skip_shift(twee);

    tweak_push_image_xy(twee, imagexy);
    if (starxyz)
        tweak_push_ref_xyz(twee, starxyz, nstars);
    else if (star_ra && star_dec)
        tweak_push_ref_ad(twee, star_ra, star_dec, nstars);
    else if (star_radec)
        tweak_push_ref_ad_array(twee, star_radec, nstars);
    else {
        logerr("Need starxyz, (star_ra and star_dec), or star_radec");
        return NULL;
    }
    tweak_push_wcs_tan(twee, wcs);
    tweak_iterate_to_order(twee, order, iterations);

    // Steal the resulting SIP structure
    sip = twee->sip;
    twee->sip = NULL;

    tweak_free(twee);
    return sip;
}

static void get_dydx_range(double* ximg, double* yimg, int nimg,
                           double* xcat, double* ycat, int ncat,
                           double *mindx, double *mindy,
                           double *maxdx, double *maxdy) {
    int i, j;
    *maxdx = -1e100;
    *mindx = 1e100;
    *maxdy = -1e100;
    *mindy = 1e100;

    for (i = 0; i < nimg; i++) {
        for (j = 0; j < ncat; j++) {
            double dx = ximg[i] - xcat[j];
            double dy = yimg[i] - ycat[j];
            *maxdx = MAX(dx, *maxdx);
            *maxdy = MAX(dy, *maxdy);
            *mindx = MIN(dx, *mindx);
            *mindy = MIN(dy, *mindy);
        }
    }
}

static void get_shift(double* ximg, double* yimg, int nimg,
                      double* xcat, double* ycat, int ncat,
                      double mindx, double mindy, double maxdx, double maxdy,
                      double* xshift, double* yshift) {
    int i, j;
    int themax, themaxind, ys, xs;

    // hough transform
    int hsz = 1000; // hough histogram size (per side)
    int *hough = calloc(hsz * hsz, sizeof(int)); // allocate bins
    int kern[] = {0, 2, 3, 2, 0,  // approximate gaussian smoother
                  2, 7, 12, 7, 2,       // should be KERNEL_SIZE x KERNEL_SIZE
                  3, 12, 20, 12, 3,
                  2, 7, 12, 7, 2,
                  0, 2, 3, 2, 0};

    assert(sizeof(kern) == KERNEL_SIZE * KERNEL_SIZE * sizeof(int));

    for (i = 0; i < nimg; i++) {    // loop over all pairs of source-catalog objs
        for (j = 0; j < ncat; j++) {
            double dx = ximg[i] - xcat[j];
            double dy = yimg[i] - ycat[j];
            int hszi = hsz - 1;
            int iy = hszi * ( (dy - mindy) / (maxdy - mindy) ); // compute deltay using implicit floor
            int ix = hszi * ( (dx - mindx) / (maxdx - mindx) ); // compute deltax using implicit floor

            // check to make sure the point is in the box
            if (KERNEL_MARG <= iy && iy < hsz - KERNEL_MARG &&
                KERNEL_MARG <= ix && ix < hsz - KERNEL_MARG) {
                int kx, ky;
                for (ky = -2; ky <= 2; ky++)
                    for (kx = -2; kx <= 2; kx++)
                        hough[(iy - ky)*hsz + (ix - kx)] += kern[(ky + 2) * 5 + (kx + 2)];
            }
        }
    }

    // find argmax in hough
    themax = 0;
    themaxind = -1;
    for (i = 0; i < hsz*hsz; i++) {
        if (themax < hough[i]) {
            themaxind = i;
            themax = hough[i];
        }
    }
    // which hough bin is the max?
    ys = themaxind / hsz;
    xs = themaxind % hsz;

    *yshift = (ys / (double)hsz) * (maxdy - mindy) + mindy;
    *xshift = (xs / (double)hsz) * (maxdx - mindx) + mindx;
    debug("xs = %d, ys = %d\n", xs, ys);
    debug("get_shift: mindx=%g, maxdx=%g, mindy=%g, maxdy=%g\n", mindx, maxdx, mindy, maxdy);
    debug("get_shift: xs=%g, ys=%g\n", *xshift, *yshift);

    free(hough);
}

static sip_t* do_entire_shift_operation(tweak_t* t, double rho) {
    get_shift(t->x, t->y, t->n,
              t->x_ref, t->y_ref, t->n_ref,
              rho*t->mindx, rho*t->mindy, rho*t->maxdx, rho*t->maxdy,
              &t->xs, &t->ys);
    wcs_shift(&(t->sip->wcstan), t->xs, t->ys);
    return NULL;
}

/* This function is intended only for initializing newly allocated tweak
 * structures, NOT for operating on existing ones.*/
void tweak_init(tweak_t* t) {
    memset(t, 0, sizeof(tweak_t));
    t->sip = sip_create();
}

tweak_t* tweak_new() {
    tweak_t* t = malloc(sizeof(tweak_t));
    tweak_init(t);
    return t;
}

void tweak_iterate_to_order(tweak_t* t, int maxorder, int iterations) {
    int order;
    int k;

    for (order=1; order<=maxorder; order++) {
        logverb("\n");
        logverb("--------------------------------\n");
        logverb("Order %i\n", order);
        logverb("--------------------------------\n");

        t->sip->a_order  = t->sip->b_order  = order;
        //t->sip->ap_order = t->sip->bp_order = order;
        tweak_go_to(t, TWEAK_HAS_CORRESPONDENCES);

        for (k=0; k<iterations; k++) {
            logverb("\n");
            logverb("--------------------------------\n");
            logverb("Iterating tweak: order %i, step %i\n", order, k);
            t->state &= ~TWEAK_HAS_LINEAR_CD;
            tweak_go_to(t, TWEAK_HAS_LINEAR_CD);
            tweak_clear_correspondences(t);
        }
    }
}

#define CHECK_STATE(x) if (state & x) sl_append(s, #x)

static char* state_string(unsigned int state) {
    sl* s = sl_new(4);
    char* str;
    CHECK_STATE(TWEAK_HAS_SIP);
    CHECK_STATE(TWEAK_HAS_IMAGE_XY);
    CHECK_STATE(TWEAK_HAS_IMAGE_XYZ);
    CHECK_STATE(TWEAK_HAS_IMAGE_AD);
    CHECK_STATE(TWEAK_HAS_REF_XY);
    CHECK_STATE(TWEAK_HAS_REF_XYZ);
    CHECK_STATE(TWEAK_HAS_REF_AD);
    CHECK_STATE(TWEAK_HAS_CORRESPONDENCES);
    CHECK_STATE(TWEAK_HAS_COARSLY_SHIFTED);
    CHECK_STATE(TWEAK_HAS_FINELY_SHIFTED);
    CHECK_STATE(TWEAK_HAS_REALLY_FINELY_SHIFTED);
    CHECK_STATE(TWEAK_HAS_LINEAR_CD);
    str = sl_join(s, " ");
    sl_free2(s);
    return str;
}

char* tweak_get_state_string(const tweak_t* t) {
    return state_string(t->state);
}

#undef CHECK_STATE

void tweak_clear_correspondences(tweak_t* t) {
    if (t->state & TWEAK_HAS_CORRESPONDENCES) {
        // our correspondences are also now toast
        assert(t->image);
        assert(t->ref);
        assert(t->dist2);
        il_free(t->image);
        il_free(t->ref);
        dl_free(t->dist2);
        if (t->weight)
            dl_free(t->weight);
        t->image    = NULL;
        t->ref      = NULL;
        t->dist2    = NULL;
        t->weight   = NULL;
        t->state &= ~TWEAK_HAS_CORRESPONDENCES;
    }
    assert(!t->image);
    assert(!t->ref);
    assert(!t->dist2);
    assert(!t->weight);
}

void tweak_clear_on_sip_change(tweak_t* t) {
    // tweak_clear_correspondences(t);
    tweak_clear_image_ad(t);
    tweak_clear_ref_xy(t);
    tweak_clear_image_xyz(t);
}

// ref_xy are the catalog star positions in image coordinates
void tweak_clear_ref_xy(tweak_t* t) {
    if (t->state & TWEAK_HAS_REF_XY) {
        //assert(t->x_ref);
        free(t->x_ref);
        //assert(t->y_ref);
        t->x_ref = NULL;
        free(t->y_ref);
        t->y_ref = NULL;
        t->state &= ~TWEAK_HAS_REF_XY;
    }
    assert(!t->x_ref);
    assert(!t->y_ref);
}

// radec of catalog stars
void tweak_clear_ref_ad(tweak_t* t) {
    if (t->state & TWEAK_HAS_REF_AD) {
        assert(t->a_ref);
        free(t->a_ref);
        t->a_ref = NULL;
        assert(t->d_ref);
        free(t->d_ref);
        t->d_ref = NULL;
        t->n_ref = 0;
        tweak_clear_correspondences(t);
        tweak_clear_ref_xy(t);
        t->state &= ~TWEAK_HAS_REF_AD;
    }
    assert(!t->a_ref);
    assert(!t->d_ref);
}

// image objs in ra,dec according to current tweak
void tweak_clear_image_ad(tweak_t* t) {
    if (t->state & TWEAK_HAS_IMAGE_AD) {
        assert(t->a);
        free(t->a);
        t->a = NULL;
        assert(t->d);
        free(t->d);
        t->d = NULL;
        t->state &= ~TWEAK_HAS_IMAGE_AD;
    }
    assert(!t->a);
    assert(!t->d);
}

void tweak_clear_image_xyz(tweak_t* t) {
    if (t->state & TWEAK_HAS_IMAGE_XYZ) {
        assert(t->xyz);
        free(t->xyz);
        t->xyz = NULL;
        t->state &= ~TWEAK_HAS_IMAGE_XYZ;
    }
    assert(!t->xyz);
}

void tweak_clear_image_xy(tweak_t* t) {
    if (t->state & TWEAK_HAS_IMAGE_XY) {
        assert(t->x);
        free(t->x);
        t->x = NULL;
        assert(t->y);
        free(t->y);
        t->y = NULL;
        t->state &= ~TWEAK_HAS_IMAGE_XY;
    }
    assert(!t->x);
    assert(!t->y);
}

// tell us (from outside tweak) where the catalog stars are
void tweak_push_ref_ad(tweak_t* t, const double* a, const double *d, int n) {
    assert(a);
    assert(d);
    assert(n);
    tweak_clear_ref_ad(t);
    assert(!t->a_ref);
    assert(!t->d_ref);
    t->a_ref = malloc(sizeof(double) * n);
    t->d_ref = malloc(sizeof(double) * n);
    memcpy(t->a_ref, a, n*sizeof(double));
    memcpy(t->d_ref, d, n*sizeof(double));
    t->n_ref = n;
    t->state |= TWEAK_HAS_REF_AD;
}

void tweak_push_ref_ad_array(tweak_t* t, const double* ad, int n) {
    int i;
    assert(ad);
    assert(n);
    tweak_clear_ref_ad(t);
    assert(!t->a_ref);
    assert(!t->d_ref);
    t->a_ref = malloc(sizeof(double) * n);
    t->d_ref = malloc(sizeof(double) * n);
    for (i=0; i<n; i++) {
        t->a_ref[i] = ad[2*i + 0];
        t->d_ref[i] = ad[2*i + 1];
    }
    t->n_ref = n;
    t->state |= TWEAK_HAS_REF_AD;
}

static void ref_xyz_from_ad(tweak_t* t) {
    int i;
    assert(t->state & TWEAK_HAS_REF_AD);
    assert(!t->xyz_ref);
    t->xyz_ref = malloc(sizeof(double) * 3 * t->n_ref);
    assert(t->xyz_ref);
    for (i = 0; i < t->n_ref; i++)
        radecdeg2xyzarr(t->a_ref[i], t->d_ref[i], t->xyz_ref + 3 * i);
    t->state |= TWEAK_HAS_REF_XYZ;
}

static void ref_ad_from_xyz(tweak_t* t) {
    int i, n;
    assert(t->state & TWEAK_HAS_REF_XYZ);
    assert(!t->a_ref);
    assert(!t->d_ref);
    n = t->n_ref;
    t->a_ref = malloc(sizeof(double) * n);
    t->d_ref = malloc(sizeof(double) * n);
    assert(t->a_ref);
    assert(t->d_ref);
    for (i=0; i<n; i++)
        xyzarr2radecdeg(t->xyz_ref + 3*i, t->a_ref + i, t->d_ref + i);
    t->state |= TWEAK_HAS_REF_XYZ;
}

// tell us (from outside tweak) where the catalog stars are
void tweak_push_ref_xyz(tweak_t* t, const double* xyz, int n) {
    assert(xyz);
    assert(n);
    tweak_clear_ref_ad(t);
    assert(!t->xyz_ref);
    t->xyz_ref = malloc(sizeof(double) * 3 * n);
    assert(t->xyz_ref);
    memcpy(t->xyz_ref, xyz, 3*n*sizeof(double));
    t->n_ref = n;
    t->state |= TWEAK_HAS_REF_XYZ;
}

void tweak_push_image_xy(tweak_t* t, const starxy_t* xy) {
    tweak_clear_image_xy(t);
    t->x = starxy_copy_x(xy);
    t->y = starxy_copy_y(xy);
    t->n = starxy_n(xy);
    t->state |= TWEAK_HAS_IMAGE_XY;
}

void tweak_skip_shift(tweak_t* t) {
    t->state |= (TWEAK_HAS_COARSLY_SHIFTED | TWEAK_HAS_FINELY_SHIFTED |
                 TWEAK_HAS_REALLY_FINELY_SHIFTED);
}

// DualTree RangeSearch callback. We want to keep track of correspondences.
// Potentially the matching could be many-to-many; we allow this and hope the
// optimizer can take care of it.
static void dtrs_match_callback(void* extra, int image_ind, int ref_ind, double dist2) {
    tweak_t* t = extra;
    image_ind = kdtree_permute(t->kd_image, image_ind);
    ref_ind   = kdtree_permute(t->kd_ref,   ref_ind);
    il_append(t->image, image_ind);
    il_append(t->ref, ref_ind);
    dl_append(t->dist2, dist2);
    if (t->weight)
        dl_append(t->weight, exp(-dist2 / (2.0 * t->jitterd2)));
}

void tweak_push_correspondence_indices(tweak_t* t, il* image, il* ref, dl* distsq, dl* weight) {
    t->image = image;
    t->ref = ref;
    t->dist2 = distsq;
    t->weight = weight;
    t->state |= TWEAK_HAS_CORRESPONDENCES;
}

// The jitter is in radians
static void find_correspondences(tweak_t* t, double jitter) {
    double dist;
    double* data_image = malloc(sizeof(double) * t->n * 3);
    double* data_ref = malloc(sizeof(double) * t->n_ref * 3);

    assert(t->state & TWEAK_HAS_IMAGE_XYZ);
    assert(t->state & TWEAK_HAS_REF_XYZ);
    tweak_clear_correspondences(t);

    memcpy(data_image, t->xyz, 3*t->n*sizeof(double));
    memcpy(data_ref, t->xyz_ref, 3*t->n_ref*sizeof(double));

    t->kd_image = kdtree_build(NULL, data_image, t->n, 3, 4, KDTT_DOUBLE,
                               KD_BUILD_BBOX);

    t->kd_ref = kdtree_build(NULL, data_ref, t->n_ref, 3, 4, KDTT_DOUBLE,
                             KD_BUILD_BBOX);

    // Storage for correspondences
    t->image = il_new(600);
    t->ref = il_new(600);
    t->dist2 = dl_new(600);
    if (t->weighted_fit)
        t->weight = dl_new(600);

    dist = rad2dist(jitter);

    logverb("search radius = %g arcsec\n", rad2arcsec(jitter));

    // Find closest neighbours
    dualtree_rangesearch(t->kd_image, t->kd_ref,
                         RANGESEARCH_NO_LIMIT, dist, FALSE, NULL,
                         dtrs_match_callback, t,
                         NULL, NULL);

    kdtree_free(t->kd_image);
    kdtree_free(t->kd_ref);
    t->kd_image = NULL;
    t->kd_ref = NULL;
    free(data_image);
    free(data_ref);

    logverb("Number of correspondences: %zu\n", il_size(t->image));
}

static double correspondences_rms_arcsec(tweak_t* t, int weighted) {
    double err2 = 0.0;
    int i;
    double totalweight = 0.0;
    for (i=0; i<il_size(t->image); i++) {
        double imgxyz[3];
        double refxyz[3];
        double weight;
        int refi, imgi;
        if (weighted && t->weight)
            weight = dl_get(t->weight, i);
        else
            weight = 1.0;
        totalweight += weight;

        imgi = il_get(t->image, i);
        sip_pixelxy2xyzarr(t->sip, t->x[imgi], t->y[imgi], imgxyz);

        refi = il_get(t->ref, i);
        radecdeg2xyzarr(t->a_ref[refi], t->d_ref[refi], refxyz);

        err2 += weight * distsq(imgxyz, refxyz, 3);
    }
    return distsq2arcsec( err2 / totalweight );
}

#if 0
// in arcseconds^2 on the sky (chi-sq)
static double figure_of_merit(tweak_t* t, double *rmsX, double *rmsY) {
    double sqerr = 0.0;
    int i;
    for (i = 0; i < il_size(t->image); i++) {
        double a, d;
        double xyzpt[3];
        double xyzpt_ref[3];
        sip_pixelxy2radec(t->sip, t->x[il_get(t->image, i)],
                          t->y[il_get(t->image, i)], &a, &d);

        // xref and yref should be intermediate WC's not image x and y!
        radecdeg2xyzarr(a, d, xyzpt);
        radecdeg2xyzarr(t->a_ref[il_get(t->ref, i)],
                        t->d_ref[il_get(t->ref, i)], xyzpt_ref);
        sqerr += distsq(xyzpt, xyzpt_ref, 3);
    }
    return rad2arcsec(1)*rad2arcsec(1)*sqerr;
}

static double figure_of_merit2(tweak_t* t) {
    // find error in pixels^2
    double sqerr = 0.0;
    int i;
    for (i = 0; i < il_size(t->image); i++) {
        double x, y, dx, dy;
        Unused anbool ok;
        ok = sip_radec2pixelxy(t->sip, t->a_ref[il_get(t->ref, i)], t->d_ref[il_get(t->ref, i)], &x, &y);
        assert(ok);
        dx = t->x[il_get(t->image, i)] - x;
        dy = t->y[il_get(t->image, i)] - y;
        sqerr += dx * dx + dy * dy;
    }
    // convert to arcsec^2
    return sqerr * square(sip_pixel_scale(t->sip));
}
#endif

// FIXME: adapt this function to take as input the correspondences to use VVVVV
//    wic is World Intermediate Coordinates, either along ra or dec
//       i.e. canonical image coordinates
//    wic_corr is the list of corresponding indexes for wip
//    pix_corr is the list of corresponding indexes for pixels
//    pix is raw image pixels (either u or v)
//    siporder is the sip order up to MAXORDER (defined in sip.h)
//    the correspondences are passed so that we can stick RANSAC around the whole
//    thing for better estimation.

// Run a polynomial tweak
static void do_sip_tweak(tweak_t* t) {
    sip_t sipout;
    size_t i, M;

    // a_order and b_order should be the same!
    assert(t->sip->a_order == t->sip->b_order);

    debug("do_sip_tweak starting.\n");
    logverb("RMS error of correspondences: %g arcsec\n",
            correspondences_rms_arcsec(t, 0));
    if (t->weighted_fit)
        logverb("Weighted RMS error of correspondences: %g arcsec\n",
                correspondences_rms_arcsec(t, 1));

    M = il_size(t->image);
    double* starxyz = malloc(M * 3 * sizeof(double));
    double* fieldxy = malloc(M * 2 * sizeof(double));
    double* weights = NULL;
    if (t->weighted_fit)
        weights = malloc(M * sizeof(double));

    int result;
    for (i=0; i<M; i++) {
        int refi;
        int imi;
        imi = il_get(t->image, i);
        fieldxy[2*i + 0] = t->x[imi];
        fieldxy[2*i + 1] = t->y[imi];
        refi = il_get(t->ref, i);
        radecdeg2xyzarr(t->a_ref[refi], t->d_ref[refi], starxyz + i*3);
        if (t->weighted_fit)
            weights[i] = dl_get(t->weight, i);
    }

    int doshift = 1;
    result = fit_sip_wcs(starxyz, fieldxy, weights, M,
                         &(t->sip->wcstan), t->sip->a_order, t->sip->ap_order, doshift,
                         &sipout);
    free(starxyz);
    free(fieldxy);
    free(weights);
    if (result) {
        ERROR("fit_sip_wcs failed\n");
        return;
    }
    memcpy(t->sip, &sipout, sizeof(sip_t));

    // recalc using new SIP
    tweak_clear_on_sip_change(t);
    tweak_go_to(t, TWEAK_HAS_IMAGE_AD);
    tweak_go_to(t, TWEAK_HAS_REF_XY);

    logverb("RMS error of correspondences: %g arcsec\n",
            correspondences_rms_arcsec(t, 0));
    if (t->weighted_fit)
        logverb("Weighted RMS error of correspondences: %g arcsec\n",
                correspondences_rms_arcsec(t, 1));
}

// Really what we want is some sort of fancy dependency system... DTDS!
// Duct-tape dependencey system (DTDS)
#define done(x) t->state |= x; return x;
#define want(x)                                 \
    if (flag == x && t->state & x)              \
        return x;                               \
    else if (flag == x)
#define ensure(x)                               \
    if (!(t->state & x)) {                      \
        return tweak_advance_to(t, x);          \
    }

unsigned int tweak_advance_to(tweak_t* t, unsigned int flag) {
    want(TWEAK_HAS_IMAGE_AD) {
        int jj;
        ensure(TWEAK_HAS_SIP);
        ensure(TWEAK_HAS_IMAGE_XY);
        debug("Satisfying TWEAK_HAS_IMAGE_AD\n");
        // Convert to ra dec
        assert(!t->a);
        assert(!t->d);
        t->a = malloc(sizeof(double) * t->n);
        t->d = malloc(sizeof(double) * t->n);
        for (jj = 0; jj < t->n; jj++)
            sip_pixelxy2radec(t->sip, t->x[jj], t->y[jj], t->a + jj, t->d + jj);
        done(TWEAK_HAS_IMAGE_AD);
    }

    want(TWEAK_HAS_REF_AD) {
        if (!(t->a_ref && t->d_ref)) {
            ensure(TWEAK_HAS_REF_XYZ);
            debug("Satisfying TWEAK_HAS_REF_AD\n");
            ref_ad_from_xyz(t);
        }
        assert(t->a_ref && t->d_ref);
        done(TWEAK_HAS_REF_AD);
    }

    want(TWEAK_HAS_REF_XYZ) {
        if (!t->xyz_ref) {
            ensure(TWEAK_HAS_REF_AD);
            debug("Satisfying TWEAK_HAS_REF_XYZ\n");
            ref_xyz_from_ad(t);
        }
        assert(t->xyz_ref);
        done(TWEAK_HAS_REF_XYZ);
    }

    want(TWEAK_HAS_REF_XY) {
        int jj;
        ensure(TWEAK_HAS_REF_AD);
        debug("Satisfying TWEAK_HAS_REF_XY\n");
        assert(t->state & TWEAK_HAS_REF_AD);
        assert(t->n_ref);
        assert(!t->x_ref);
        assert(!t->y_ref);
        t->x_ref = malloc(sizeof(double) * t->n_ref);
        t->y_ref = malloc(sizeof(double) * t->n_ref);
        for (jj = 0; jj < t->n_ref; jj++) {
            Unused anbool ok;
            ok = sip_radec2pixelxy(t->sip, t->a_ref[jj], t->d_ref[jj],
                                   t->x_ref + jj, t->y_ref + jj);
            assert(ok);
        }
        done(TWEAK_HAS_REF_XY);
    }

    want(TWEAK_HAS_IMAGE_XYZ) {
        int i;
        ensure(TWEAK_HAS_IMAGE_AD);
        debug("Satisfying TWEAK_HAS_IMAGE_XYZ\n");
        assert(!t->xyz);
        t->xyz = malloc(3 * t->n * sizeof(double));
        for (i = 0; i < t->n; i++)
            radecdeg2xyzarr(t->a[i], t->d[i], t->xyz + 3*i);
        done(TWEAK_HAS_IMAGE_XYZ);
    }

    want(TWEAK_HAS_COARSLY_SHIFTED) {
        ensure(TWEAK_HAS_REF_XY);
        ensure(TWEAK_HAS_IMAGE_XY);
        debug("Satisfying TWEAK_HAS_COARSLY_SHIFTED\n");
        get_dydx_range(t->x, t->y, t->n, t->x_ref, t->y_ref, t->n_ref,
                       &t->mindx, &t->mindy, &t->maxdx, &t->maxdy);
        do_entire_shift_operation(t, 1.0);
        tweak_clear_image_ad(t);
        tweak_clear_ref_xy(t);
        done(TWEAK_HAS_COARSLY_SHIFTED);
    }

    want(TWEAK_HAS_FINELY_SHIFTED) {
        ensure(TWEAK_HAS_REF_XY);
        ensure(TWEAK_HAS_IMAGE_XY);
        ensure(TWEAK_HAS_COARSLY_SHIFTED);
        debug("Satisfying TWEAK_HAS_FINELY_SHIFTED\n");
        // Shrink size of hough box
        do_entire_shift_operation(t, 0.3);
        tweak_clear_image_ad(t);
        tweak_clear_ref_xy(t);
        done(TWEAK_HAS_FINELY_SHIFTED);
    }

    want(TWEAK_HAS_REALLY_FINELY_SHIFTED) {
        ensure(TWEAK_HAS_REF_XY);
        ensure(TWEAK_HAS_IMAGE_XY);
        ensure(TWEAK_HAS_FINELY_SHIFTED);
        debug("Satisfying TWEAK_HAS_REALLY_FINELY_SHIFTED\n");
        // Shrink size of hough box
        do_entire_shift_operation(t, 0.03);
        tweak_clear_image_ad(t);
        tweak_clear_ref_xy(t);
        done(TWEAK_HAS_REALLY_FINELY_SHIFTED);
    }

    want(TWEAK_HAS_CORRESPONDENCES) {
        ensure(TWEAK_HAS_REF_XYZ);
        ensure(TWEAK_HAS_IMAGE_XYZ);
        debug("Satisfying TWEAK_HAS_CORRESPONDENCES\n");
        t->jitterd2 = arcsec2distsq(t->jitter);
        find_correspondences(t, 6.0 * arcsec2rad(t->jitter));
        done(TWEAK_HAS_CORRESPONDENCES);
    }

    want(TWEAK_HAS_LINEAR_CD) {
        ensure(TWEAK_HAS_SIP);
        ensure(TWEAK_HAS_REALLY_FINELY_SHIFTED);
        ensure(TWEAK_HAS_REF_XY);
        ensure(TWEAK_HAS_REF_AD);
        ensure(TWEAK_HAS_IMAGE_XY);
        ensure(TWEAK_HAS_CORRESPONDENCES);
        debug("Satisfying TWEAK_HAS_LINEAR_CD\n");
        do_sip_tweak(t);
        tweak_clear_on_sip_change(t);
        done(TWEAK_HAS_LINEAR_CD);
    }

    // small memleak -- but it's a major bug if this happens, so suck it up.
    logerr("die for dependence: %s\n", state_string(flag));
    assert(0);
    return -1;
}

void tweak_push_wcs_tan(tweak_t* t, const tan_t* wcs) {
    memcpy(&(t->sip->wcstan), wcs, sizeof(tan_t));
    t->state |= TWEAK_HAS_SIP;
}

void tweak_go_to(tweak_t* t, unsigned int dest_state) {
    while (!(t->state & dest_state))
        tweak_advance_to(t, dest_state);
}

#define SAFE_FREE(xx) {free((xx)); xx = NULL;}
void tweak_clear(tweak_t* t) {
    if (!t)
        return ;
    SAFE_FREE(t->a);
    SAFE_FREE(t->d);
    SAFE_FREE(t->x);
    SAFE_FREE(t->y);
    SAFE_FREE(t->xyz);
    SAFE_FREE(t->a_ref);
    SAFE_FREE(t->d_ref);
    SAFE_FREE(t->x_ref);
    SAFE_FREE(t->y_ref);
    SAFE_FREE(t->xyz_ref);
    if (t->sip) {
        sip_free(t->sip);
        t->sip = NULL;
    }
    il_free(t->image);
    il_free(t->ref);
    dl_free(t->dist2);
    if (t->weight)
        dl_free(t->weight);
    t->image = NULL;
    t->ref = NULL;
    t->dist2 = NULL;
    t->weight = NULL;
    kdtree_free(t->kd_image);
    kdtree_free(t->kd_ref);
}

void tweak_free(tweak_t* t) {
    tweak_clear(t);
    free(t);
}


#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#ifdef _MSC_VER				/* identifies this as a Microsoft compiler */
#define _USE_MATH_DEFINES	/* added RX2011 */
#endif
#include <math.h>
#include <time.h>
#include <limits.h>
#include <gsl/gsl_multimin.h>
//#include "/Users/tischler/dev/GNU/include/gsl/gsl_multimin.h"
#include "Euler.h"

#ifdef DEBUG_ON
#define DEBUG 1				/* a value of 5 gets everything */
#endif

#define MIN(A,B)	( (A)<(B)?(A):(B) )
#define MAX(A,B)	( (A)>(B)?(A):(B) )

double my_f (const gsl_vector *v, void *params);

/******************************************************************************************
 * return the error between a lattice rotated by Euler angles and the measured spots
 * thus the returned value is a minimum for the optimimal Euler angles
 */
double my_f(
const gsl_vector *v,	/* vector with current value of Euler angles */
void	*pattern)		/* pointer to the parameters, in this case a pointer to the pattern structure */
{
	double  G[3];						/* calculated G vector for each hkl */
	double  recip[3][3];				/* reciprocal space */
	double  M_Euler[3][3];				/* an Euler matrix */
	size_t  N;							/* local copy of number of measured spots to check */
	double a,b,g;						/* local copy of the Euler angles */
	struct patternOfOneGrain *p;		/* local value of pattern */
	double  rms;						/* rms value of error, the returned value */
	size_t  i;							/* loop index */
	double  hkl[3];						/* float value of hkl */
	double  dot;

	/* p = pattern; */
	p = (struct patternOfOneGrain *)pattern;
	N = p->Ni;											/* local value for convenience */
	a = gsl_vector_get(v,0);							/* the Euler angles */
	b = gsl_vector_get(v,1);
	g = gsl_vector_get(v,2);
	MatrixCopy33(recip,p->xtal.recip);					/* copy unrotated reciprocal lattice into recip */

	EulerMatrix(a,b,g,M_Euler);							/* make the rotation matrix M_Euler from Euler angles */
	MatrixMultiply33(M_Euler,recip,recip);				/* rotate recip by Euler angles */
	for (i=0,rms=0.0;i<N;i++) {
		VECTOR_COPY3(hkl,p->hkls[i]);					/* convert integer hkl to float for following MatrixMultiply31 */
if (hkl[0]!=hkl[0]) {
fprintf(stderr,"hkl[0] is NaN\n");
}
		MatrixMultiply31(recip,hkl,G);
if (G[0]!=G[0]) {
fprintf(stderr,"G[0] is NaN\n");
}
		normalize3(G);									/* normalized gvec of hkl[i] rotated by Euler angles */
		dot = dot3(G,p->Ghat[i]);
		dot = MIN(dot,1.0);
		rms += 1. - dot;								/* (1-dot) ~ 0.5*(delta angle)^2*/
	}
	rms = sqrt(rms/(double)N);							/* rms of angle errors */
	return rms;
}

/* When calling optimizeEulerAngles(), be sure to set pattern.xtal:

	e.g.
	pattern.xtal.a = pattern.xtal.b = pattern.xtal.c = 4.05;		// just default to Al for now
	pattern.xtal.alpha = pattern.xtal.beta = pattern.xtal.gamma = M_PI/2.0;

	then call:
		FillRecipInLattice(&(pattern.lattice));
	to set the reciprocal lattice in the lattice structure
*/
/* this routine optimizes approximate Euler angles using the gsl simplex method. */
int optimizeEulerAngles(
double  startStep,					/* starting step size (radians), make this a bit larger than the error */
double  epsAbs,						/* tolerance on the answer (=0.001) */
long	maxIter,					/* maximum number of iterations, stop after this many regardless, perhaps 100 */
struct	patternOfOneGrain *pattern) /* provides G^'s and hkl's for one fitted pattern, and the lattice parameters */
{
	size_t np = 3;					/* number of dimensions of the function, 3 Euler angles */

	/* T tells what kind of minimizer we are using, select the simplex method */
	const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
	gsl_multimin_fminimizer *s = NULL;		/* this is the minimizer */
	gsl_vector *ss, *x;						/* pointers to vectors,  ss is stepsize, x is point */
	gsl_multimin_function minex_func;		/* struct defining parts needed for minimizing */
	size_t iter=0, i;
	int status;								/* status of returned gsl function, 0 is OK */
	double size;							/* size of minimizer, sort of maximum error */

	double  G[3];							/* calculated G vector for each hkl */
	double  recip[3][3];					/* reciprocal space */
	double  M_Euler[3][3];					/* an Euler matrix */
	double  hkl[3];							/* float value of hkl */
	double  dot;

	if (!pattern) { fprintf(stderr,"pattern in NULL on entry to optimizeEulerAngles()\n"); return 1; }
	if (pattern->Ni<3) { fprintf(stderr,"less than 3 pairs of Ghat & hkls on entry to optimizeEulerAngles()\n"); return 1; }
	if (!(pattern->Ghat) || !(pattern->hkls)) { fprintf(stderr,"Ghat or hkls are NULL on entry to optimizeEulerAngles()\n"); return 1; }
	if ((pattern->xtal.a)<=0 || (pattern->xtal.b)<=0 || (pattern->xtal.c)<=0) { fprintf(stderr,"invalid lattice in optimizeEulerAngles()\n"); return 1; }

	/* Initial vertex step size vector */
	ss = gsl_vector_alloc(np);				/* step sizes in x and y */

	/* Set all step sizes to startStep, the starting step size */
	gsl_vector_set_all(ss, startStep);		/* set ss[0] = ss[1]  = ss[2] = startStep (radian) */

	/* Starting point */
	x = gsl_vector_alloc(np);				/* allocate space for a vector of length 3 */
	gsl_vector_set(x, 0, pattern->alpha);   /* set to starting guess, input (alpha,beta,gamma) */
	gsl_vector_set(x, 1, pattern->beta);
	gsl_vector_set(x, 2, pattern->gamma);

	/* Initialize the minimizer */
	minex_func.f = &my_f;					/* the function that returns the value */
	minex_func.n = np;						/* number of parameters */
	minex_func.params = (void *)pattern;	/* a generic pointer that points to something with info for my_f() */
	/* Note: my_func.df, and my_func.fdf are not needed since simplex method does not use derivatives */

	s = gsl_multimin_fminimizer_alloc(T, np);	/* allocate for a 2d simplex minimizer named 's' */

	/* Set starting point for the minimizer:
		for the minimizer 's',
		working on a function 'minex_func'
		starting from the initial guess x[2]
		using step sizes of ss[2]  */
	gsl_multimin_fminimizer_set(s, &minex_func, x, ss);

#if (DEBUG)		/******************************************************************************/
	fprintf(fout, "start minimization with Euler angles (%15.10f, %15.10f, %15.10f) (deg.)\n", \
		pattern->alpha*180/M_PI,pattern->beta*180/M_PI,pattern->gamma*180/M_PI);
#endif			/******************************************************************************/

	/* Iterate the minimizer.  This looop iterates the minimization method until it is good enough */
	iter = 0;
	do {
		iter++;
		status = gsl_multimin_fminimizer_iterate(s);	/* do one iteration of the minimizer */
		if (status) break;								/* status=0 is OK, check here for an error */

		size = gsl_multimin_fminimizer_size (s); 		/* get 'size' of minimizer */
		status = gsl_multimin_test_size (size, epsAbs);	/* test size against epsAbs */

#if (DEBUG>1)		/******************************************************************************/
		fprintf(fout,"%5ld  f(", iter);
		for (i=0; i<np; i++) {
			fprintf(fout,"%.8f", gsl_vector_get(s->x, i));
			if (i<np-1) fprintf(fout,", ");
		}
		fprintf(fout,") = %15.8g size = %.5g\n", s->fval, size);
#endif			/******************************************************************************/
	} while (status == GSL_CONTINUE && iter < (size_t)maxIter); /* stop after maxIter regardless of size */

#if (DEBUG)		/******************************************************************************/
	if (status == GSL_SUCCESS) fprintf(fout,"converged to minimum successfully\n");
	else fprintf(fout,"did not converge successfully, status = %d\n",status);
	fprintf(fout, "  optimization changed Euler angles by (%15.10f, %15.10f, %15.10f) (deg.)\n", \
		(gsl_vector_get(s->x,0)-pattern->alpha)*180/M_PI, (gsl_vector_get(s->x,1)-pattern->beta)*180/M_PI, \
		(gsl_vector_get(s->x,2)-pattern->gamma)*180/M_PI);
#endif			/******************************************************************************/

	pattern->alpha = gsl_vector_get(s->x,0);			/* set to final values, resultant (alpha,beta,gamma) returned */
	pattern->beta = gsl_vector_get(s->x,1);
	pattern->gamma = gsl_vector_get(s->x,2);

#if (DEBUG)		/******************************************************************************/
	fprintf(fout, "after minimization, Euler angles are (%15.10f, %15.10f, %15.10f) (deg.)\n", \
		pattern->alpha*180/M_PI,pattern->beta*180/M_PI,pattern->gamma*180/M_PI);
#endif			/******************************************************************************/

	/* recalculate the err[] list in pattern */
	MatrixCopy33(recip,pattern->xtal.recip);		/* copy unrotated reciprocal lattice into recip */
	EulerMatrix(pattern->alpha,pattern->beta,pattern->gamma,M_Euler);/* rotation matrix from Euler angles */
	MatrixMultiply33(M_Euler,recip,recip);			/* rotate recip by Euler angles */
	for (i=0;i<(size_t)(pattern->Ni);i++) {
		VECTOR_COPY3(hkl,pattern->hkls[i]);			/* convert integer hkl to float for following MatrixMultiply31 */
		MatrixMultiply31(recip,hkl,G);
		normalize3(G);								/* normalized gvec of hkl[i] rotated by Euler angles */
		dot = dot3(G,pattern->Ghat[i]);				/* note, Ghat here is the measured, not calculated G */
		(pattern->err)[i] = acos(MIN(dot,1.0));
	}

	gsl_vector_free(x);									/* free vector of x,y positions */
	gsl_vector_free(ss);								/* free vector of step sizes */
	gsl_multimin_fminimizer_free (s);					/* free the minimizer */
	return status;
}


#ifndef _PKMCMC_H_
#define _PKMCMC_H_

#include <vector>
#include <string>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_matrix.h>

class pkmcmc{
    int num_data, num_pars, workspace_size;
    std::vector<double> data, model, k;
    std::vector<double> theta_0, theta_i, param_vars, min, max;
    std::vector<std::vector<double>> cov, Psi;
    std::vector<bool> limit_pars;
    gsl_spline *Pk_bao, *Pk_nw;
    gsl_interp_accel *acc_bao, *acc_nw;
    double chisq_0, chisq_i, abs_err, rel_err, k_min, k_max;

    // The function to be integrated by GSL
    double model_func(std::vector<double> &pars, int j); //done

    // Performs the integral needed for the model value at each data point
    void model_calc(std::vector<double> &pars); //done

    // Randomly selects a new parameter realization for a trial
    void get_param_real(); //done

    // Compute the chi^2 value of the current model with the data
    double calc_chi_squared(); //done

    // Perform one Metropolis-Hastings trial
    bool trial(); //done

    // Write the current accepted parameter realization to the screen
    void write_theta_screen(); //done

    // Perform a number of trials to move from the initial parameter guess to a higher likelihood region
    void burn_in(int num_burn); //done

    // Tune the acceptance ratio to be about 0.234
    void tune_vars(); //done

    public:
        // Initialization function
        pkmcmc(std::string data_file, std::string cov_file, std::string pk_bao_file, std::string pk_nw_file,
               std::vector<double> &pars, std::vector<double> &vars, bool mock_avg); //done

        // Check that the vectors have the expected sizes
        void check_init(); //done

        // Set which parameters have priors and what those priors are.
        void set_param_limits(std::vector<bool> &lim_pars, std::vector<double> &min_in,
                              std::vector<double> &max_in); //done

        // Run a number of Metropolis-Hastings trials to form the MCMC chain
        void run_chain(int num_draws, std::string reals_file, bool new_chain);

        // Free the memory with various GSL class members
        void clean_up_gsl();

};

#endif


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>

#include "tests.h"

void
test_dot (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int N = 1;
   float alpha = 0.0f;
   float X[] = { 0.733f };
   float Y[] = { 0.825f };
   int incX = 1;
   int incY = -1;
   float expected = 0.604725f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 1)");
  };

  {
   int N = 1;
   float alpha = 0.1f;
   float X[] = { 0.733f };
   float Y[] = { 0.825f };
   int incX = 1;
   int incY = -1;
   float expected = 0.704725f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 2)");
  };

  {
   int N = 1;
   float alpha = 1.0f;
   float X[] = { 0.733f };
   float Y[] = { 0.825f };
   int incX = 1;
   int incY = -1;
   float expected = 1.604725f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 3)");
  };

  {
   int N = 1;
   float alpha = 0.0f;
   float X[] = { -0.812f };
   float Y[] = { -0.667f };
   int incX = -1;
   int incY = 1;
   float expected = 0.541604f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 4)");
  };

  {
   int N = 1;
   float alpha = 0.1f;
   float X[] = { -0.812f };
   float Y[] = { -0.667f };
   int incX = -1;
   int incY = 1;
   float expected = 0.641604f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 5)");
  };

  {
   int N = 1;
   float alpha = 1.0f;
   float X[] = { -0.812f };
   float Y[] = { -0.667f };
   int incX = -1;
   int incY = 1;
   float expected = 1.541604f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 6)");
  };

  {
   int N = 1;
   float alpha = 0.0f;
   float X[] = { 0.481f };
   float Y[] = { 0.523f };
   int incX = -1;
   int incY = -1;
   float expected = 0.251563f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 7)");
  };

  {
   int N = 1;
   float alpha = 0.1f;
   float X[] = { 0.481f };
   float Y[] = { 0.523f };
   int incX = -1;
   int incY = -1;
   float expected = 0.351563f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 8)");
  };

  {
   int N = 1;
   float alpha = 1.0f;
   float X[] = { 0.481f };
   float Y[] = { 0.523f };
   int incX = -1;
   int incY = -1;
   float expected = 1.251563f;
   float f;
   f = cblas_sdsdot (N, alpha, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdsdot(case 9)");
  };

  {
   int N = 1;
   float X[] = { 0.785f };
   float Y[] = { -0.7f };
   int incX = 1;
   int incY = -1;
   float expected = -0.5495f;
   float f;
   f = cblas_sdot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdot(case 10)");
  };

  {
   int N = 1;
   double X[] = { 0.79 };
   double Y[] = { -0.679 };
   int incX = 1;
   int incY = -1;
   double expected = -0.53641;
   double f;
   f = cblas_ddot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, dbleps, "ddot(case 11)");
  };

  {
   int N = 1;
   float X[] = { 0.474f, -0.27f };
   float Y[] = { -0.144f, -0.392f };
   int incX = 1;
   int incY = -1;
   float expected[2] = {-0.174096f, -0.146928f};
   float f[2];
   cblas_cdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotu(case 12) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotu(case 12) imag");
  };

  {
   int N = 1;
   float X[] = { 0.474f, -0.27f };
   float Y[] = { -0.144f, -0.392f };
   int incX = 1;
   int incY = -1;
   float expected[2] = {0.037584f, -0.224688f};
   float f[2];
   cblas_cdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotc(case 13) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotc(case 13) imag");
  };

  {
   int N = 1;
   double X[] = { -0.87, -0.631 };
   double Y[] = { -0.7, -0.224 };
   int incX = 1;
   int incY = -1;
   double expected[2] = {0.467656, 0.63658};
   double f[2];
   cblas_zdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotu(case 14) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotu(case 14) imag");
  };

  {
   int N = 1;
   double X[] = { -0.87, -0.631 };
   double Y[] = { -0.7, -0.224 };
   int incX = 1;
   int incY = -1;
   double expected[2] = {0.750344, -0.24682};
   double f[2];
   cblas_zdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotc(case 15) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotc(case 15) imag");
  };

  {
   int N = 1;
   float X[] = { -0.457f };
   float Y[] = { 0.839f };
   int incX = -1;
   int incY = 1;
   float expected = -0.383423f;
   float f;
   f = cblas_sdot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdot(case 16)");
  };

  {
   int N = 1;
   double X[] = { 0.949 };
   double Y[] = { -0.873 };
   int incX = -1;
   int incY = 1;
   double expected = -0.828477;
   double f;
   f = cblas_ddot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, dbleps, "ddot(case 17)");
  };

  {
   int N = 1;
   float X[] = { 0.852f, -0.045f };
   float Y[] = { 0.626f, -0.164f };
   int incX = -1;
   int incY = 1;
   float expected[2] = {0.525972f, -0.167898f};
   float f[2];
   cblas_cdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotu(case 18) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotu(case 18) imag");
  };

  {
   int N = 1;
   float X[] = { 0.852f, -0.045f };
   float Y[] = { 0.626f, -0.164f };
   int incX = -1;
   int incY = 1;
   float expected[2] = {0.540732f, -0.111558f};
   float f[2];
   cblas_cdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotc(case 19) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotc(case 19) imag");
  };

  {
   int N = 1;
   double X[] = { -0.786, -0.341 };
   double Y[] = { -0.271, -0.896 };
   int incX = -1;
   int incY = 1;
   double expected[2] = {-0.09253, 0.796667};
   double f[2];
   cblas_zdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotu(case 20) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotu(case 20) imag");
  };

  {
   int N = 1;
   double X[] = { -0.786, -0.341 };
   double Y[] = { -0.271, -0.896 };
   int incX = -1;
   int incY = 1;
   double expected[2] = {0.518542, 0.611845};
   double f[2];
   cblas_zdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotc(case 21) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotc(case 21) imag");
  };

  {
   int N = 1;
   float X[] = { -0.088f };
   float Y[] = { -0.165f };
   int incX = -1;
   int incY = -1;
   float expected = 0.01452f;
   float f;
   f = cblas_sdot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, flteps, "sdot(case 22)");
  };

  {
   int N = 1;
   double X[] = { -0.434 };
   double Y[] = { -0.402 };
   int incX = -1;
   int incY = -1;
   double expected = 0.174468;
   double f;
   f = cblas_ddot(N, X, incX, Y, incY);
   gsl_test_rel(f, expected, dbleps, "ddot(case 23)");
  };

  {
   int N = 1;
   float X[] = { -0.347f, 0.899f };
   float Y[] = { -0.113f, -0.858f };
   int incX = -1;
   int incY = -1;
   float expected[2] = {0.810553f, 0.196139f};
   float f[2];
   cblas_cdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotu(case 24) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotu(case 24) imag");
  };

  {
   int N = 1;
   float X[] = { -0.347f, 0.899f };
   float Y[] = { -0.113f, -0.858f };
   int incX = -1;
   int incY = -1;
   float expected[2] = {-0.732131f, 0.399313f};
   float f[2];
   cblas_cdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], flteps, "cdotc(case 25) real");
   gsl_test_rel(f[1], expected[1], flteps, "cdotc(case 25) imag");
  };

  {
   int N = 1;
   double X[] = { -0.897, -0.204 };
   double Y[] = { -0.759, 0.557 };
   int incX = -1;
   int incY = -1;
   double expected[2] = {0.794451, -0.344793};
   double f[2];
   cblas_zdotu_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotu(case 26) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotu(case 26) imag");
  };

  {
   int N = 1;
   double X[] = { -0.897, -0.204 };
   double Y[] = { -0.759, 0.557 };
   int incX = -1;
   int incY = -1;
   double expected[2] = {0.567195, -0.654465};
   double f[2];
   cblas_zdotc_sub(N, X, incX, Y, incY, &f);
   gsl_test_rel(f[0], expected[0], dbleps, "zdotc(case 27) real");
   gsl_test_rel(f[1], expected[1], dbleps, "zdotc(case 27) imag");
  };

}


/* Easel's interfaces to the GNU Scientific Library
 */
#ifdef HAVE_LIBGSL
#include "esl_config.h"

#include <stdlib.h>
#include "easel/easel.h"
#include "easel/dmatrix.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_eigen.h>

int
esl_GSL_MatrixInversion(ESL_DMATRIX *A, ESL_DMATRIX **ret_Ai)
{
  ESL_DMATRIX     *Ai;          /* RETURN: A^{-1}             */
  gsl_matrix_view  Av;          /* gsl view of matrix A       */
  gsl_matrix      *LU;          /* LU decomposition of A      */
  gsl_matrix      *Aiv;         /* gsl version of A^{-1}      */
  gsl_permutation *permute;
  int              signum;
  int              i,j;

  Ai = esl_dmx_Alloc(A->n, A->m);

  /* Invert U to get Ui, using LU decomposition.
   */
  Av      = gsl_matrix_view_array(A->mx[0], A->n, A->n);
  LU      = gsl_matrix_alloc(A->n, A->n);
  Aiv     = gsl_matrix_alloc(A->n, A->n); /* U^{-1}: inverse of U    */
  permute = gsl_permutation_alloc(A->n);
  gsl_matrix_memcpy(LU, &Av.matrix);

  if (gsl_linalg_LU_decomp(LU, permute, &signum) != 0) ESL_EXCEPTION(eslEUNKNOWN, "gsl failed");
  if (gsl_linalg_LU_invert(LU, permute, Aiv) != 0)     ESL_EXCEPTION(eslEUNKNOWN, "gsl failed");

  gsl_matrix_free(LU);
  gsl_permutation_free(permute);

  /* recover the matrix from gsl.
   */
  for (i = 0; i < A->n; i++)
    for (j = 0; j < A->n; j++)
      Ai->mx[i][j] = gsl_matrix_get(Aiv, i, j);
  gsl_matrix_free(Aiv);

  ret->Ai = Ai;
  return eslOK;
}

#endif /*HAVE_LIBGSL*/

/*****************************************************************
 * @LICENSE@
 *
 * SVN $Id$
 * SVN $URL$
 *****************************************************************/