#define _USE_MATH_DEFINES
#include <opencv2/core/core.hpp>

#include <stdint.h>

#include <string>
#include <vector>

#include "caffe/common.hpp"
#include "caffe/layers/tsv_box_data_layer.hpp"
#include "caffe/util/benchmark.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/random_helper.h"

#define BOOST_SPIRIT_THREADSAFE
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/foreach.hpp>

using boost::property_tree::ptree;
using boost::property_tree::read_json;

namespace caffe {

struct image {
    int h;
    int w;
    int c;
    float *data;
};

struct box_label {
    int id;
    float x, y, w, h;
    float left, right, top, bottom;
};

image make_empty_image(int w, int h, int c)
{
    image out;
    out.data = 0;
    out.h = h;
    out.w = w;
    out.c = c;
    return out;
}

image make_image(int w, int h, int c)
{
    image out = make_empty_image(w, h, c);
    out.data = new float[h*w*c];
    return out;
}

void free_image(image m)
{
    if (m.data) {
        delete[] m.data;
        m.data = 0;
    }
}

image cvmat_to_image(cv::Mat* src)
{
    int h = src->rows;
    int w = src->cols;
    int c = src->channels();
    image out = make_image(w, h, c);
    int i, j, k, count = 0;;

    for (k = 0; k < c; ++k) {
        for (i = 0; i < h; ++i) {
            const unsigned char* ptr = src->ptr<unsigned char>(i);
            for (j = 0; j < w; ++j) {
                out.data[count++] = ptr[j*c + k] / 255.;
            }
        }
    }
    return out;
}

void rgbgr_image(image im)
{
    int i;
    for (i = 0; i < im.w*im.h; ++i) {
        float swap = im.data[i];
        im.data[i] = im.data[i + im.w*im.h * 2];
        im.data[i + im.w*im.h * 2] = swap;
    }
}

float get_pixel(image m, int x, int y, int c)
{
    assert(x < m.w && y < m.h && c < m.c);
    return m.data[c*m.h*m.w + y*m.w + x];
}
float get_pixel_extend(image m, int x, int y, int c)
{
    if (x < 0) x = 0;
    if (x >= m.w) x = m.w - 1;
    if (y < 0) y = 0;
    if (y >= m.h) y = m.h - 1;
    if (c < 0 || c >= m.c) return 0;
    return get_pixel(m, x, y, c);
}
void set_pixel(image m, int x, int y, int c, float val)
{
    if (x < 0 || y < 0 || c < 0 || x >= m.w || y >= m.h || c >= m.c) return;
    assert(x < m.w && y < m.h && c < m.c);
    m.data[c*m.h*m.w + y*m.w + x] = val;
}
void add_pixel(image m, int x, int y, int c, float val)
{
    assert(x < m.w && y < m.h && c < m.c);
    m.data[c*m.h*m.w + y*m.w + x] += val;
}

image resize_image(image im, int w, int h)
{
    assert(w > 1 && h > 1 && im.h > 1 && im.w > 1);
    image resized = make_image(w, h, im.c);
    image part = make_image(w, im.h, im.c);
    int r, c, k;
    float w_scale = (float)(im.w - 1) / (w - 1);
    float h_scale = (float)(im.h - 1) / (h - 1);
    for (k = 0; k < im.c; ++k) {
        for (r = 0; r < im.h; ++r) {
            for (c = 0; c < w; ++c) {
                float val = 0;
                if (c == w - 1 || im.w == 1) {
                    val = get_pixel(im, im.w - 1, r, k);
                }
                else {
                    float sx = c*w_scale;
                    int ix = (int)sx;
                    float dx = sx - ix;
                    val = (1 - dx) * get_pixel(im, ix, r, k) + dx * get_pixel(im, ix + 1, r, k);
                }
                set_pixel(part, c, r, k, val);
            }
        }
    }
    for (k = 0; k < im.c; ++k) {
        for (r = 0; r < h; ++r) {
            float sy = r*h_scale;
            int iy = (int)sy;
            float dy = sy - iy;
            for (c = 0; c < w; ++c) {
                float val = (1 - dy) * get_pixel(part, c, iy, k);
                set_pixel(resized, c, r, k, val);
            }
            if (r == h - 1 || im.h == 1) continue;
            for (c = 0; c < w; ++c) {
                float val = dy * get_pixel(part, c, iy + 1, k);
                add_pixel(resized, c, r, k, val);
            }
        }
    }

    free_image(part);
    return resized;
}

image ipl_to_image(IplImage* src)
{
    unsigned char *data = (unsigned char *)src->imageData;
    int h = src->height;
    int w = src->width;
    int c = src->nChannels;
    int step = src->widthStep;
    image out = make_image(w, h, c);
    int i, j, k, count = 0;;

    for (k = 0; k < c; ++k) {
        for (i = 0; i < h; ++i) {
            for (j = 0; j < w; ++j) {
                out.data[count++] = data[i*step + j*c + k] / 255.;
            }
        }
    }
    return out;
}

// imbuf is image stream in memory
image load_image_cv(vector<unsigned char>& imbuf, int channels)
{
    int cv_read_flag = (channels == 3 ? CV_LOAD_IMAGE_COLOR : CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat src = cv::imdecode(cv::Mat(imbuf), cv_read_flag);

    image out = cvmat_to_image(&src);
    // Here we directly work in the BGR format, not as in https://github.com/pjreddie/darknet/blob/master/src/image.c#L526
    // rgbgr_image(out);
    return out;
}

// imbuf is image stream in memory
image load_image_cv(const string &filename, int channels)
{
    cv::Mat src = ReadImageToCVMat(filename, channels > 1);

    image out = cvmat_to_image(&src);
    // Here we directly work in the BGR format, not as in https://github.com/pjreddie/darknet/blob/master/src/image.c#L526
    // rgbgr_image(out);
    return out;
}

image load_image(vector<unsigned char>& imbuf, int w, int h, int c)
{
    image out = load_image_cv(imbuf, c);

    if ((h && w) && (h != out.h || w != out.w)) {
        image resized = resize_image(out, w, h);
        free_image(out);
        out = resized;
    }
    return out;
}

image load_image(const string &filename, int w, int h, int c)
{
    image out = load_image_cv(filename, c);

    if ((h && w) && (h != out.h || w != out.w)) {
        image resized = resize_image(out, w, h);
        free_image(out);
        out = resized;
    }
    return out;
}

image load_image_color(vector<unsigned char>& imbuf, int w, int h)
{
    return load_image(imbuf, w, h, 3);
}

image load_image_color(const string &filename, int w, int h)
{
    return load_image(filename, w, h, 3);
}

void fill_image(image m, float s)
{
    int i;
    for (i = 0; i < m.h*m.w*m.c; ++i) m.data[i] = s;
}

float bilinear_interpolate(image im, float x, float y, int c)
{
    int ix = (int)floorf(x);
    int iy = (int)floorf(y);

    float dx = x - ix;
    float dy = y - iy;

    float val = (1 - dy) * (1 - dx) * get_pixel_extend(im, ix, iy, c) +
        dy     * (1 - dx) * get_pixel_extend(im, ix, iy + 1, c) +
        (1 - dy) *   dx   * get_pixel_extend(im, ix + 1, iy, c) +
        dy     *   dx   * get_pixel_extend(im, ix + 1, iy + 1, c);
    return val;
}

void original_image_to_network_input(float x3, float y3,
        float cos_rad, float sin_rad,
        int orig_img_w, int orig_img_h,
        int nw, int nh,
        int network_w, int network_h,
        float dx, float dy,
        float &rotate_x3, float &rotate_y3) {
    float offset_x3 = x3 * nw / orig_img_w - 0.5 * nw;
    float offset_y3 = y3 * nh / orig_img_h - 0.5 * nh;
    rotate_x3 = cos_rad * offset_x3 - sin_rad * offset_y3;
    rotate_y3 = sin_rad * offset_x3 + cos_rad * offset_y3;
    rotate_x3 += (float)nw / 2. + dx;
    rotate_y3 += (float)nh / 2. + dy;
}

void network_input_to_original_image(float in_x, float in_y,
        float cos_rad, float sin_rad,
        int orig_img_w, int orig_img_h,
        int nw, int nh,
        int network_w, int network_h,
        float dx, float dy,
        float &out_x, float &out_y) {
    float rx = in_x - 0.5 * nw - dx;
    float ry = in_y - 0.5 * nh - dy;
    out_x = cos_rad * rx + sin_rad * ry;
    out_y = -sin_rad * rx + cos_rad * ry;
    out_x *= orig_img_w / (float)nw;
    out_y *= orig_img_h / (float)nh;
    out_x += orig_img_w * .5;
    out_y += orig_img_h * .5;
}

void place_image(image im, int w, int h, int dx, int dy, image canvas, float rad)
{
    if (rad) {
        const float fill_value = 0.5;
        float cos_rad = cos(rad);
        float sin_rad = sin(rad);
        for (int c = 0; c < im.c; c++) {
            for (int y1 = 0; y1 < canvas.h; y1++) {
                for (int x1 = 0; x1 < canvas.w; x1++) {
                    float m_x, m_y;
                    network_input_to_original_image((float)x1, (float)y1,
                            cos_rad, sin_rad,
                            im.w, im.h,
                            w, h,
                            canvas.w, canvas.h,
                            dx, dy,
                            m_x, m_y);
                    float val;
                    if (m_x < 0 || m_x >= im.w - 1 || m_y < 0 || m_y >= im.h - 1) {
                        val = fill_value;
                    } else {
                        val = bilinear_interpolate(im, m_x, m_y, c);
                    }
                    set_pixel(canvas, x1, y1, c, val);
                }
            }
        }
    } else {
        int x, y, c;
        for (c = 0; c < im.c; ++c) {
            for (y = 0; y < h; ++y) {
                for (x = 0; x < w; ++x) {
                    float rx = ((float)x / w) * im.w;
                    float ry = ((float)y / h) * im.h;
                    float val = bilinear_interpolate(im, rx, ry, c);
                    set_pixel(canvas, x + dx, y + dy, c, val);
                }
            }
        }
    }
}

float three_way_max(float a, float b, float c)
{
    return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c);
}

float three_way_min(float a, float b, float c)
{
    return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c);
}

// http://www.cs.rit.edu/~ncs/color/t_convert.html
void rgb_to_hsv(image im)
{
    assert(im.c == 3);
    int i, j;
    float r, g, b;
    float h, s, v;
    for (j = 0; j < im.h; ++j) {
        for (i = 0; i < im.w; ++i) {
            r = get_pixel(im, i, j, 2);
            g = get_pixel(im, i, j, 1);
            b = get_pixel(im, i, j, 0);
            float max = three_way_max(r, g, b);
            float min = three_way_min(r, g, b);
            float delta = max - min;
            v = max;
            if (max == 0) {
                s = 0;
                h = 0;
            }
            else {
                s = delta / max;
                if (max == min) {
                    h = 0;
                }
                else if (r == max) {
                    h = (g - b) / delta;
                }
                else if (g == max) {
                    h = 2 + (b - r) / delta;
                }
                else {
                    h = 4 + (r - g) / delta;
                }
                if (h < 0) h += 6;
                h = h / 6.;
            }
            set_pixel(im, i, j, 0, h);
            set_pixel(im, i, j, 1, s);
            set_pixel(im, i, j, 2, v);
        }
    }
}

void hsv_to_rgb(image im)
{
    assert(im.c == 3);
    int i, j;
    float r, g, b;
    float h, s, v;
    float f, p, q, t;
    for (j = 0; j < im.h; ++j) {
        for (i = 0; i < im.w; ++i) {
            h = 6 * get_pixel(im, i, j, 0);
            s = get_pixel(im, i, j, 1);
            v = get_pixel(im, i, j, 2);
            if (s == 0) {
                r = g = b = v;
            }
            else {
                int index = floor(h);
                f = h - index;
                p = v*(1 - s);
                q = v*(1 - s*f);
                t = v*(1 - s*(1 - f));
                if (index == 0) {
                    r = v; g = t; b = p;
                }
                else if (index == 1) {
                    r = q; g = v; b = p;
                }
                else if (index == 2) {
                    r = p; g = v; b = t;
                }
                else if (index == 3) {
                    r = p; g = q; b = v;
                }
                else if (index == 4) {
                    r = t; g = p; b = v;
                }
                else {
                    r = v; g = p; b = q;
                }
            }
            set_pixel(im, i, j, 2, r);
            set_pixel(im, i, j, 1, g);
            set_pixel(im, i, j, 0, b);
        }
    }
}

void scale_image_channel(image im, int c, float v)
{
    int i, j;
    for (j = 0; j < im.h; ++j) {
        for (i = 0; i < im.w; ++i) {
            float pix = get_pixel(im, i, j, c);
            pix = pix*v;
            set_pixel(im, i, j, c, pix);
        }
    }
}

void scale_image(image im, float v)
{
    int i;
    for (i = 0; i < im.w*im.h*im.c; ++i) {
        im.data[i] = im.data[i]*v;
    }
}

void constrain_image(image im)
{
    int i;
    for (i = 0; i < im.w*im.h*im.c; ++i) {
        if (im.data[i] < 0) im.data[i] = 0;
        if (im.data[i] > 1) im.data[i] = 1;
    }
}

void distort_image(image im, float hue, float sat, float val)
{
    rgb_to_hsv(im);
    scale_image_channel(im, 1, sat);
    scale_image_channel(im, 2, val);
    int i;
    for (i = 0; i < im.w*im.h; ++i) {
        im.data[i] = im.data[i] + hue;
        if (im.data[i] > 1) im.data[i] -= 1;
        if (im.data[i] < 0) im.data[i] += 1;
    }
    hsv_to_rgb(im);
    constrain_image(im);
}

float rand_uniform(float min, float max)
{
    if (min == max) {
        return min;
    }
    if (max < min) {
        float swap = min;
        min = max;
        max = swap;
    }
    return (float)random_helper::uniform_real(min, max);
}

float rand_scale(float s)
{
    float scale = rand_uniform(1, s);
    if (random_helper::uniform_int(0, 1)) return scale;
    return 1. / scale;
}

void random_distort_image(image im, float hue, float saturation, float exposure)
{
    float dhue = rand_uniform(-hue, hue);
    float dsat = rand_scale(saturation);
    float dexp = rand_scale(exposure);
    distort_image(im, dhue, dsat, dexp);
}

void flip_image(image a)
{
    int i, j, k;
    for (k = 0; k < a.c; ++k) {
        for (i = 0; i < a.h; ++i) {
            for (j = 0; j < a.w / 2; ++j) {
                int index = j + a.w*(i + a.h*(k));
                int flip = (a.w - j - 1) + a.w*(i + a.h*(k));
                float swap = a.data[flip];
                a.data[flip] = a.data[index];
                a.data[index] = swap;
            }
        }
    }
}

void image_subtract_mean(image im, float r, float g, float b)
{
    float *r_channel, *g_channel, *b_channel;
    b_channel = im.data;
    g_channel = im.data + im.w * im.h;
    r_channel = im.data + im.w * im.h * 2;
    for (int i = 0; i < im.w * im.h; ++i) {
        b_channel[i] -= b;
        g_channel[i] -= g;
        r_channel[i] -= r;
    }
}

vector<box_label> read_boxes(const string &input_label_data, map<string, int> &labelmap, int orig_img_w, int orig_img_h)
{
    vector<box_label> boxes;
    if (input_label_data.length() == 0)
        return boxes;

    // Read json.
    ptree pt;
    std::istringstream is(input_label_data);
    try {
        read_json(is, pt);
    } catch (std::exception &e) {
        LOG(FATAL) << "invalid json string: " << input_label_data << " error: " << e.what();
    }

    for (boost::property_tree::ptree::iterator it = pt.begin(); it != pt.end(); ++it)
    {
        boost::optional<int> diff = it->second.get_optional<int>("diff");         // 0: normal, 1: difficult
        if (diff && diff.get())
            continue;

        string cls = it->second.get<string>("class");
        CHECK(labelmap.find(cls) != labelmap.end()) << "class name not found in the labelmap: " << cls;
        std::vector<float> rect;
        ptree pt_rect = it->second.get_child("rect");   // format: x1, y1, x2, y2
        for (ptree::iterator iter = pt_rect.begin(); iter != pt_rect.end(); ++iter)
            rect.push_back(iter->second.get_value<float>());

        // https://github.com/pjreddie/darknet/blob/master/scripts/voc_label.py#L15
        // in yolo, x is decreased by 1 because coordinates in pascal voc are 1-based.
        // but in the tsv data, coordinates are already 0-based.
        float x = ((rect[0] + rect[2]) / 2) / orig_img_w;
        float y = ((rect[1] + rect[3]) / 2) / orig_img_h;
        float w = (rect[2] - rect[0]) / orig_img_w;
        float h = (rect[3] - rect[1]) / orig_img_h;

        if (x < -0.001 || y < -0.001 || x > 0.999 || y > 0.999 || w <= -0.001 || h <= -0.001) {
            LOG(ERROR) << "invalid bounding box detected and will be skipped: "
                << rect[0] << ", " << rect[1] << ", "
                << rect[2] << ", " << rect[3] << " string: " << input_label_data;
            continue;
        }

        box_label box;
        box.id = labelmap[cls];
        // https://github.com/pjreddie/darknet/blob/master/src/data.c#L149
        box.x = x;
        box.y = y;
        box.h = h;
        box.w = w;
        box.left = x - w / 2;
        box.right = x + w / 2;
        box.top = y - h / 2;
        box.bottom = y + h / 2;
        boxes.push_back(box);
    }

    return boxes;
}

void randomize_boxes(box_label *b, int n)
{
    int i;
    for (i = 0; i < n; ++i) {
        box_label swap = b[i];
        int index = rand() % n;
        b[i] = b[index];
        b[index] = swap;
    }
}

float constrain(float min, float max, float a)
{
    if (a < min) return min;
    if (a > max) return max;
    return a;
}

int correct_boxes(box_label *boxes, int n, float dx, float dy, float sx, float sy, int flip,
        float rad,
        int orig_img_w, int orig_img_h,
        int nw, int nh,
        int network_w, int network_h, float odx, float ody)
{
    int valid_boxes = 0;
    float cos_rad = cos(rad);
    float sin_rad = sin(rad);
    for (int i = 0; i < n; ++i) {
        if (boxes[i].x == 0 && boxes[i].y == 0) {
            boxes[i].x = 999999;
            boxes[i].y = 999999;
            boxes[i].w = 999999;
            boxes[i].h = 999999;
            continue;
        }
        if (rad) {
            float x0 = boxes[i].left * orig_img_w;
            float y0 = boxes[i].top * orig_img_h;
            float x3 = boxes[i].right * orig_img_w;
            float y3 = boxes[i].bottom * orig_img_h;
            float x1 = x3;
            float y1 = y0;
            float x2 = x0;
            float y2 = y3;
            float area_in_new = (boxes[i].right - boxes[i].left) *
                (boxes[i].bottom - boxes[i].top) * nw * nh;
            original_image_to_network_input(x0, y0, cos_rad, sin_rad, orig_img_w, orig_img_h,
                    nw, nh, network_w, network_h,
                    odx, ody,
                    x0, y0);
            original_image_to_network_input(x1, y1, cos_rad, sin_rad, orig_img_w, orig_img_h,
                    nw, nh, network_w, network_h,
                    odx, ody,
                    x1, y1);
            original_image_to_network_input(x2, y2, cos_rad, sin_rad, orig_img_w, orig_img_h,
                    nw, nh, network_w, network_h,
                    odx, ody,
                    x2, y2);
            original_image_to_network_input(x3, y3, cos_rad, sin_rad, orig_img_w, orig_img_h,
                    nw, nh, network_w, network_h,
                    odx, ody,
                    x3, y3);
            float left = fmin(fmin(fmin(x0, x1), x2), x3);
            float right = fmax(fmax(fmax(x0, x1), x2), x3);
            float top = fmin(fmin(fmin(y0, y1), y2), y3);
            float bottom = fmax(fmax(fmax(y0, y1), y2), y3);
            {
                // make the area the same with the aspect ratio held
                float shrink = sqrt(area_in_new / (right - left) / (bottom - top));
                float final_width = (right - left) * shrink;
                float final_height = (bottom - top) * shrink;
                float final_cx = (left + right) / 2.;
                float final_cy = (top + bottom) / 2.;
                boxes[i].left = (final_cx - final_width / 2.) / network_w;
                boxes[i].right = (final_cx + final_width / 2.) / network_w;
                boxes[i].top = (final_cy - final_height / 2.) / network_h;
                boxes[i].bottom = (final_cy + final_height / 2.) / network_h;

            }
        } else {
            boxes[i].left = boxes[i].left  * sx - dx;
            boxes[i].right = boxes[i].right * sx - dx;
            boxes[i].top = boxes[i].top   * sy - dy;
            boxes[i].bottom = boxes[i].bottom* sy - dy;
        }

        if (flip) {
            float swap = boxes[i].left;
            boxes[i].left = 1. - boxes[i].right;
            boxes[i].right = 1. - swap;
        }

        float area_old = (boxes[i].right - boxes[i].left) * (boxes[i].bottom - boxes[i].top);

        boxes[i].left = constrain(0, 1, boxes[i].left);
        boxes[i].right = constrain(0, 1, boxes[i].right);
        boxes[i].top = constrain(0, 1, boxes[i].top);
        boxes[i].bottom = constrain(0, 1, boxes[i].bottom);

        boxes[i].x = (boxes[i].left + boxes[i].right) / 2;
        boxes[i].y = (boxes[i].top + boxes[i].bottom) / 2;
        boxes[i].w = (boxes[i].right - boxes[i].left);
        boxes[i].h = (boxes[i].bottom - boxes[i].top);

        boxes[i].w = constrain(0, 1, boxes[i].w);
        boxes[i].h = constrain(0, 1, boxes[i].h);

        float area_new = boxes[i].w * boxes[i].h;

        if (area_old > 1e-6 && area_new / area_old > 0.5)
            valid_boxes++;
    }

    return valid_boxes;
}

int fill_truth_detection(const string &input_label_data, float *truth, int num_boxes, map<string, int> &labelmap,
        int orig_img_w, int orig_img_h, int flip, float dx, float dy, float sx, float sy,
        float rad,
        int nw, int nh, int network_w, int network_h, float odx, float ody)
{
    vector<box_label> boxes = read_boxes(input_label_data, labelmap, orig_img_w, orig_img_h);
    auto count = boxes.size();
    if (!count)
        return 0;
    randomize_boxes(&boxes[0], count);
    int valid_boxes = correct_boxes(&boxes[0], count, dx, dy, sx, sy, flip, rad,
            orig_img_w, orig_img_h, nw, nh, network_w, network_h, odx, ody);
    if (count > num_boxes)
        count = num_boxes;
    float x, y, w, h;
    int id;
    int i;

    for (i = 0; i < count; ++i) {
        x = boxes[i].x;
        y = boxes[i].y;
        w = boxes[i].w;
        h = boxes[i].h;
        id = boxes[i].id;

        if ((w < .001 || h < .001)) continue;

        truth[i * 5 + 0] = x;
        truth[i * 5 + 1] = y;
        truth[i * 5 + 2] = w;
        truth[i * 5 + 3] = h;
        truth[i * 5 + 4] = id;
    }

    return valid_boxes;
}

void load_data_detection(const string &input_b64coded_data, const string &input_label_data, float *output_image_data, float *output_label_data,
                         map<string, int> labelmap,
                         int w, int h, int boxes, float jitter, float hue, float saturation, float exposure,
                         float mean_r, float mean_g, float mean_b,
                         float pixel_value_scale,
                         const BoxDataParameter &box_param,
                         bool is_image_path)
{
    image orig;
    // load image in BGR format with values ranging in [0,1]
    if (is_image_path) {
        orig = load_image_color(input_b64coded_data, 0, 0);
    } else {
        vector<BYTE> imbuf = base64_decode(input_b64coded_data);
        orig = load_image_color(imbuf, 0, 0);
    }

    image sized = make_image(w, h, orig.c);
    fill_image(sized, .5);

    float dw = jitter * orig.w;
    float dh = jitter * orig.h;

    float nw, nh, dx, dy, rad;
    int flip;
    for (int tries = 0; tries < box_param.max_samples(); tries++)
    {
        float new_ar = (orig.w + rand_uniform(-dw, dw)) / (orig.h + rand_uniform(-dh, dh));
        float random_scale_min = box_param.random_scale_min();
        float random_scale_max = box_param.random_scale_max();
        float scale = rand_uniform(random_scale_min, random_scale_max);

        // float nw, nh;

        if (random_scale_min == random_scale_max && box_param.output_ssd_label())
        {
            // for ssd in test mode (random_scale_min == random_scale_max)
            // stretch image to 300x300 as specified by wxh
            nw = w;
            nh = h;
        }
        else if (new_ar < 1) {
            nh = scale * h;
            nw = nh * new_ar;
        }
        else {
            nw = scale * w;
            nh = nw / new_ar;
        }

        // float dx, dy;
        if (box_param.fix_offset()) {
            dx = (w - nw) / 2;
            dy = (h - nh) / 2;
        } else {
            dx = rand_uniform(0, w - nw);
            dy = rand_uniform(0, h - nh);
        }

        rad = rand_uniform(-box_param.rotate_max(),
                box_param.rotate_max());
        rad *= M_PI / 180.;

        flip = random_helper::uniform_int(0, 1) && !box_param.fix_offset();

        int valid_boxes = fill_truth_detection(input_label_data, output_label_data, boxes, labelmap, orig.w, orig.h, flip,
                -dx / w, -dy / h, nw / w, nh / h,
                rad,
                nw, nh, sized.w, sized.h, dx, dy);

        if (valid_boxes > 0)
            break;
    }

    place_image(orig, nw, nh, dx, dy, sized, rad);

    random_distort_image(sized, hue, saturation, exposure);
    if (flip) flip_image(sized);

    // scale values back to [0,255]
    scale_image(sized, pixel_value_scale);

    // mean subtraction
    image_subtract_mean(sized, mean_r, mean_g, mean_b);

    memcpy(output_image_data, sized.data, sizeof(float) * h * w * orig.c);

    free_image(sized);

    free_image(orig);
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::DataLayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
    TsvDataLayer<Dtype>::DataLayerSetUp(bottom, top);
    box_data_param_idx_ = 0;

    const BoxDataParameter &box_param = this->layer_param().box_data_param(
            box_data_param_idx_);

    iter_ = 0;
    dim_ = box_param.random_min();

    // reshape label
    vector<int> shape = top[1]->shape();
    iter_for_resize_ = box_param.iter_for_resize();
    CHECK(box_param.has_labelmap()) << "labelmap is expected in TsvBoxDataLayer";
    load_labelmap(box_param.labelmap());
    shape[1] = box_param.max_boxes() * 5;
    top[1]->Reshape(shape);
    for (int i = 0; i < this->prefetch_.size(); ++i) {
        this->prefetch_[i]->label_.Reshape(shape);
    }

    cum_tsv_box_weights_.clear();
    for (int i = 0; i < this->layer_param().box_data_param_size(); i++) {
        auto &box_data_param = this->layer_param().box_data_param(i);
        cum_tsv_box_weights_.push_back(box_data_param.weight());
    }

    for (size_t s = 0; s < cum_tsv_box_weights_.size(); s++) {
        CHECK_GT(cum_tsv_box_weights_[s], 0);
    }

    for (size_t s = 1; s < cum_tsv_box_weights_.size(); s++) {
        cum_tsv_box_weights_[s] += cum_tsv_box_weights_[s - 1];
    }

    CHECK_GT(cum_tsv_box_weights_.size(), 0);
    Dtype norm_factor = cum_tsv_box_weights_[cum_tsv_box_weights_.size() - 1];
    for (size_t s = 0; s < cum_tsv_box_weights_.size(); s++) {
        cum_tsv_box_weights_[s] /= norm_factor;
    }
}

// trim from end (in place)
static inline void rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(),
        std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::load_labelmap(const string &filename)
{
    std::ifstream labelmap_file;
    labelmap_file.open(filename.c_str());
    CHECK(!labelmap_file.fail()) << "Failed to open labelmap file: " << filename << ", error: " << strerror(errno);

    int id = 0;
    labelmap_.clear();
    while (!labelmap_file.eof())
    {
        std::string line;
        std::getline(labelmap_file, line);
        rtrim(line);
        if (line.length() == 0)
            break;

        labelmap_[line] = id++;
    }

    labelmap_file.close();
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::update_curr_box_data_param_idx() {

    Dtype prob = (Dtype)random_helper::uniform_real(0, 1);
    size_t idx = 0;
    for (; idx < cum_tsv_box_weights_.size(); idx++) {
        if (prob <= cum_tsv_box_weights_[idx]) {
            break;
        }
    }

    box_data_param_idx_ = idx;
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::on_load_batch_start(Batch<Dtype>* batch) {
    update_curr_box_data_param_idx();

    // change size
    if (iter_++ % this->iter_for_resize_ == 0)
    {
        const BoxDataParameter &box_param = this->layer_param().box_data_param(
                box_data_param_idx_);
        int rand_step = box_param.random_step();
        int rand_min = box_param.random_min() / rand_step;
        int rand_max = box_param.random_max() / rand_step;

        if (rand_min == rand_max)
        {
            dim_ = box_param.random_min();
        }
        else
        {
            dim_ = random_helper::uniform_int(rand_min, rand_max) * rand_step;
            LOG(INFO) << "Box data reshape to " << dim_ << "x" << dim_;
        }
    }
    vector<int> shape = batch->data_.shape();
    shape[2] = dim_;
    shape[3] = dim_;
    batch->data_.Reshape(shape);

    // reshape label as it may be changed due to output_ssd_label
    const BoxDataParameter &box_param = this->layer_param().box_data_param(
            box_data_param_idx_);
    vector<int> label_shape(2);
    label_shape[0] = shape[0];
    label_shape[1] = box_param.max_boxes() * 5;
    batch->label_.Reshape(label_shape);
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::on_load_batch_end(Batch<Dtype>* batch)
{
    const BoxDataParameter &box_param = this->layer_param().box_data_param(
            box_data_param_idx_);
    if (!box_param.output_ssd_label())
        return;

    // count the number of bboxes
    int num_boxes = 0;
    int batch_size = batch->label_.shape(0);
    const Dtype *label_data = batch->label_.cpu_data();
    for (int n = 0; n < batch_size; n++)
    {
        for (int i = 0; i < box_param.max_boxes(); i++)
        {
            if (label_data[(n * box_param.max_boxes() + i) * 5] > 0)
                num_boxes++;
        }
    }

    Blob<Dtype> ssd_label;
    vector<int> shape(4);
    shape[0] = 1;
    shape[1] = 1;
    shape[2] = std::max(num_boxes, 1);
    shape[3] = 8;
    ssd_label.Reshape(shape);

    Dtype *ssd_label_data = ssd_label.mutable_cpu_data();
    if (num_boxes == 0)
    {
        // Store all -1 in the label.
        caffe_set<Dtype>(8, -1, ssd_label_data);
    }
    else
    {
        int idx = 0;
        for (int n = 0; n < batch_size; n++)
        {
            for (int i = 0; i < box_param.max_boxes(); i++)
            {
                const Dtype *truth = label_data + (n * box_param.max_boxes() + i) * 5;
                if (truth[0] > 0)
                {
                    ssd_label_data[idx++] = n;
                    ssd_label_data[idx++] = truth[4]; // class label
                    ssd_label_data[idx++] = 0; // instance_id, not used for now
                    ssd_label_data[idx++] = truth[0] - truth[2] / 2; // xmin
                    ssd_label_data[idx++] = truth[1] - truth[3] / 2; // ymin
                    ssd_label_data[idx++] = truth[0] + truth[2] / 2; // xmax
                    ssd_label_data[idx++] = truth[1] + truth[3] / 2; // ymax
                    ssd_label_data[idx++] = 0; // difficult flag
                }
            }
        }
    }

    batch->label_.Reshape(shape);
    caffe_copy(ssd_label.count(), ssd_label.cpu_data(), batch->label_.mutable_cpu_data());
}

template <typename Dtype>
void TsvBoxDataLayer<Dtype>::process_one_image_and_label(const string &input_b64coded_data, const string &input_label_data, const TsvDataParameter &tsv_param, Dtype *output_image_data, Dtype *output_label_data)
{
    const BoxDataParameter &box_param = this->layer_param().box_data_param(
            box_data_param_idx_);
    float jitter = box_param.jitter();
    float exposure = box_param.exposure();
    float hue = box_param.hue();
    float saturation = box_param.saturation();
    int max_boxes = box_param.max_boxes();
    float pixel_value_scale = this->layer_param().tsv_data_param().pixel_value_scale();

    load_data_detection(input_b64coded_data, input_label_data, (float*)output_image_data, (float*)output_label_data,
        labelmap_,
        dim_, dim_, max_boxes, jitter, hue, saturation, exposure,
        this->mean_values_[2], this->mean_values_[1], this->mean_values_[0], pixel_value_scale,
        box_param,
        tsv_param.data_format() == TsvDataParameter_DataFormat_ImagePath);
}

INSTANTIATE_CLASS(TsvBoxDataLayer);
REGISTER_LAYER_CLASS(TsvBoxData);

}  // namespace caffe


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#include "slha_io.hpp"
#include "wrappers.hpp"
#include "lowe.h"
#include "linalg.h"
#include "ew_input.hpp"
#include "physical_input.hpp"
#include "spectrum_generator_settings.hpp"

#include <fstream>
#include <algorithm>
#include <string>
#include <boost/bind.hpp>

namespace flexiblesusy {

SLHA_io::SLHA_io()
   : data()
   , modsel()
{
}

void SLHA_io::clear()
{
   data.clear();
   modsel.clear();
}

void SLHA_io::convert_symmetric_fermion_mixings_to_slha(double&,
                                                        Eigen::Matrix<double, 1, 1>&)
{
}

/**
 * @param m mass
 * @param z 1x1 mixing matrix
 */
void SLHA_io::convert_symmetric_fermion_mixings_to_slha(double& m,
                                                        Eigen::Matrix<std::complex<double>, 1, 1>& z)
{
   // check if 1st row contains non-zero imaginary parts
   if (!is_zero(Abs(Im(z(0,0))))) {
      z(0,0) *= std::complex<double>(0.0,1.0);
      m *= -1;
#ifdef ENABLE_DEBUG
      if (!is_zero(Abs(Im(z(0,0))))) {
         WARNING("Element (0,0) of the following fermion mixing matrix"
                 " contains entries which have non-zero real and imaginary"
                 " parts:\nZ = " << z);
      }
#endif
   }
}

void SLHA_io::convert_symmetric_fermion_mixings_to_hk(double&,
                                                      Eigen::Matrix<double, 1, 1>&)
{
}

/**
 * @param m mass
 * @param z 1x1 mixing matrix
 */
void SLHA_io::convert_symmetric_fermion_mixings_to_hk(double& m,
                                                      Eigen::Matrix<std::complex<double>, 1, 1>& z)
{
   if (m < 0.) {
      z(0,0) *= std::complex<double>(0.0,1.0);
      m *= -1;
   }
}

bool SLHA_io::block_exists(const std::string& block_name) const
{
   return data.find(block_name) != data.cend();
}

std::string SLHA_io::to_lower(const std::string& str)
{
   std::string lower(str.size(), ' ');
   std::transform(str.begin(), str.end(), lower.begin(), ::tolower);
   return lower;
}

/**
 * @brief reads from source
 *
 * If source is "-", then read_from_stream() is called.  Otherwise,
 * read_from_file() is called.
 *
 * @param source string that specifies the source
 */
void SLHA_io::read_from_source(const std::string& source)
{
   if (source == "-")
      read_from_stream(std::cin);
   else
      read_from_file(source);
}

/**
 * @brief opens SLHA input file and reads the content
 * @param file_name SLHA input file name
 */
void SLHA_io::read_from_file(const std::string& file_name)
{
   std::ifstream ifs(file_name);
   if (ifs.good()) {
      data.clear();
      data.read(ifs);
   } else {
      std::ostringstream msg;
      msg << "cannot read SLHA file: \"" << file_name << "\"";
      throw ReadError(msg.str());
   }
}

/**
 * @brief reads SLHA data from a stream
 * @param istr input stream
 */
void SLHA_io::read_from_stream(std::istream& istr)
{
   data.read(istr);
}

void SLHA_io::read_modsel()
{
   SLHA_io::Tuple_processor modsel_processor
      = boost::bind(&SLHA_io::process_modsel_tuple, boost::ref(modsel), _1, _2);

   read_block("MODSEL", modsel_processor);
}

void SLHA_io::fill(softsusy::QedQcd& qedqcd) const
{
   CKM_wolfenstein ckm_wolfenstein;
   PMNS_parameters pmns_parameters;

   SLHA_io::Tuple_processor sminputs_processor
      = boost::bind(&SLHA_io::process_sminputs_tuple, boost::ref(qedqcd), _1, _2);

   read_block("SMINPUTS", sminputs_processor);

   if (modsel.quark_flavour_violated) {
      SLHA_io::Tuple_processor vckmin_processor
         = boost::bind(&SLHA_io::process_vckmin_tuple, boost::ref(ckm_wolfenstein), _1, _2);

      read_block("VCKMIN", vckmin_processor);
   }

   if (modsel.lepton_flavour_violated) {
      SLHA_io::Tuple_processor upmnsin_processor
         = boost::bind(&SLHA_io::process_upmnsin_tuple, boost::ref(pmns_parameters), _1, _2);

      read_block("UPMNSIN", upmnsin_processor);
   }

   // fill CKM parameters in qedqcd
   CKM_parameters ckm_parameters;
   ckm_parameters.set_from_wolfenstein(
      ckm_wolfenstein.lambdaW,
      ckm_wolfenstein.aCkm,
      ckm_wolfenstein.rhobar,
      ckm_wolfenstein.etabar);
   qedqcd.setCKM(ckm_parameters);

   // fill PMNS parameters in qedqcd
   qedqcd.setPMNS(pmns_parameters);
}

/**
 * Fill struct of extra physical input parameters from SLHA object
 * (FlexibleSUSYInput block)
 *
 * @param input struct of physical input parameters
 */
void SLHA_io::fill(Physical_input& input) const
{
   SLHA_io::Tuple_processor processor
      = boost::bind(&SLHA_io::process_flexiblesusyinput_tuple, boost::ref(input), _1, _2);

   read_block("FlexibleSUSYInput", processor);
}

/**
 * Fill struct of spectrum generator settings from SLHA object
 * (FlexibleSUSY block)
 *
 * @param settings struct of spectrum generator settings
 */
void SLHA_io::fill(Spectrum_generator_settings& settings) const
{
   SLHA_io::Tuple_processor flexiblesusy_processor
      = boost::bind(&SLHA_io::process_flexiblesusy_tuple, boost::ref(settings), _1, _2);

   read_block("FlexibleSUSY", flexiblesusy_processor);
}

/**
 * Applies processor to each (key, value) pair of a SLHA block.
 * Non-data lines are ignored.
 *
 * @param block_name block name
 * @param processor tuple processor to be applied
 *
 * @return scale (or 0 if no scale is defined)
 */
double SLHA_io::read_block(const std::string& block_name, const Tuple_processor& processor) const
{
   SLHAea::Coll::const_iterator block =
      data.find(data.cbegin(), data.cend(), block_name);

   double scale = 0.;

   while (block != data.cend()) {
      for (SLHAea::Block::const_iterator line = block->cbegin(),
              end = block->cend(); line != end; ++line) {
         if (!line->is_data_line()) {
            // read scale from block definition
            if (line->size() > 3 &&
                to_lower((*line)[0]) == "block" && (*line)[2] == "Q=")
               scale = convert_to<double>((*line)[3]);
            continue;
         }

         if (line->size() >= 2) {
            const int key = convert_to<int>((*line)[0]);
            const double value = convert_to<double>((*line)[1]);
            processor(key, value);
         }
      }

      ++block;
      block = data.find(block, data.cend(), block_name);
   }

   return scale;
}

/**
 * Fills an entry from a SLHA block
 *
 * @param block_name block name
 * @param entry entry to be filled
 *
 * @return scale (or 0 if no scale is defined)
 */
double SLHA_io::read_block(const std::string& block_name, double& entry) const
{
   SLHAea::Coll::const_iterator block =
      data.find(data.cbegin(), data.cend(), block_name);

   double scale = 0.;

   while (block != data.cend()) {
      for (SLHAea::Block::const_iterator line = block->cbegin(),
              end = block->cend(); line != end; ++line) {
         if (!line->is_data_line()) {
            // read scale from block definition
            if (line->size() > 3 &&
                to_lower((*line)[0]) == "block" && (*line)[2] == "Q=")
               scale = convert_to<double>((*line)[3]);
            continue;
         }

         if (line->size() >= 1)
            entry = convert_to<double>((*line)[0]);
      }

      ++block;
      block = data.find(block, data.cend(), block_name);
   }

   return scale;
}

double SLHA_io::read_entry(const std::string& block_name, int key) const
{
   SLHAea::Coll::const_iterator block =
      data.find(data.cbegin(), data.cend(), block_name);

   double entry = 0.;
   const SLHAea::Block::key_type keys(1, ToString(key));
   SLHAea::Block::const_iterator line;

   while (block != data.cend()) {
      line = block->find(keys);

      if (line != block->end() && line->is_data_line() && line->size() > 1)
         entry = convert_to<double>(line->at(1));

      ++block;
      block = data.find(block, data.cend(), block_name);
   }

   return entry;
}

/**
 * Reads scale definition from SLHA block.
 *
 * @param block_name block name
 *
 * @return scale (or 0 if no scale is defined)
 */
double SLHA_io::read_scale(const std::string& block_name) const
{
   if (!block_exists(block_name))
      return 0.;

   double scale = 0.;

   for (SLHAea::Block::const_iterator line = data.at(block_name).cbegin(),
        end = data.at(block_name).cend(); line != end; ++line) {
      if (!line->is_data_line()) {
         if (line->size() > 3 &&
             to_lower((*line)[0]) == "block" && (*line)[2] == "Q=")
            scale = convert_to<double>((*line)[3]);
         break;
      }
   }

   return scale;
}

void SLHA_io::set_block(const std::ostringstream& lines, Position position)
{
   SLHAea::Block block;
   block.str(lines.str());
   data.erase(block.name());
   if (position == front)
      data.push_front(block);
   else
      data.push_back(block);
}

void SLHA_io::set_block(const std::string& lines, Position position)
{
   set_block(std::ostringstream(lines), position);
}

void SLHA_io::set_blocks(const std::vector<std::string>& blocks, Position position)
{
   for (std::vector<std::string>::const_iterator it = blocks.begin(),
           end = blocks.end(); it != end; it++)
      set_block(*it, position);
}

/**
 * This function treats a given scalar as 1x1 matrix.  Such a case is
 * not defined in the SLHA standard, but we still handle it to avoid
 * problems.
 */
void SLHA_io::set_block(const std::string& name, double value,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n'
      << boost::format(mixing_matrix_formatter) % 1 % 1 % value % symbol;

   set_block(ss);
}

void SLHA_io::set_block(const std::string& name, const softsusy::DoubleMatrix& matrix,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= matrix.displayRows(); ++i)
      for (int k = 1; k <= matrix.displayCols(); ++k) {
         ss << boost::format(mixing_matrix_formatter) % i % k % matrix(i,k)
            % (symbol + "(" + ToString(i) + "," + ToString(k) + ")");
      }

   set_block(ss);
}

void SLHA_io::set_block(const std::string& name, const softsusy::ComplexMatrix& matrix,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= matrix.displayRows(); ++i)
      for (int k = 1; k <= matrix.displayCols(); ++k) {
         ss << boost::format(mixing_matrix_formatter) % i % k
            % Re(matrix(i,k))
            % ("Re(" + symbol + "(" + ToString(i) + "," + ToString(k) + "))");
      }

   set_block(ss);
}

void SLHA_io::set_sminputs(const softsusy::QedQcd& qedqcd_)
{
   using namespace softsusy;
   softsusy::QedQcd qedqcd(qedqcd_);
   std::ostringstream ss;

   const double alphaEmInv = 1./qedqcd.displayAlpha(ALPHA);

   ss << "Block SMINPUTS\n";
   ss << FORMAT_ELEMENT( 1, alphaEmInv                   , "alpha^(-1) SM MSbar(MZ)");
   ss << FORMAT_ELEMENT( 2, qedqcd.displayFermiConstant(), "G_Fermi");
   ss << FORMAT_ELEMENT( 3, qedqcd.displayAlpha(ALPHAS)  , "alpha_s(MZ) SM MSbar");
   ss << FORMAT_ELEMENT( 4, qedqcd.displayPoleMZ()       , "MZ(pole)");
   ss << FORMAT_ELEMENT( 5, qedqcd.displayMbMb()         , "mb(mb) SM MSbar");
   ss << FORMAT_ELEMENT( 6, qedqcd.displayPoleMt()       , "mtop(pole)");
   ss << FORMAT_ELEMENT( 7, qedqcd.displayPoleMtau()     , "mtau(pole)");
   ss << FORMAT_ELEMENT( 8, qedqcd.displayNeutrinoPoleMass(3), "mnu3(pole)");
   ss << FORMAT_ELEMENT( 9, qedqcd.displayPoleMW()       , "MW(pole)");
   ss << FORMAT_ELEMENT(11, qedqcd.displayPoleMel()      , "melectron(pole)");
   ss << FORMAT_ELEMENT(12, qedqcd.displayNeutrinoPoleMass(1), "mnu1(pole)");
   ss << FORMAT_ELEMENT(13, qedqcd.displayPoleMmuon()    , "mmuon(pole)");
   ss << FORMAT_ELEMENT(14, qedqcd.displayNeutrinoPoleMass(2), "mnu2(pole)");

   // recalculate mc(mc)^MS-bar
   double mc = qedqcd.displayMass(mCharm);
   qedqcd.runto(mc);
   mc = qedqcd.displayMass(mCharm);

   // recalculate mu(2 GeV)^MS-bar, md(2 GeV)^MS-bar, ms^MS-bar(2 GeV)
   qedqcd.runto(2.0);
   ss << FORMAT_ELEMENT(21, qedqcd.displayMass(mDown)    , "md");
   ss << FORMAT_ELEMENT(22, qedqcd.displayMass(mUp)      , "mu");
   ss << FORMAT_ELEMENT(23, qedqcd.displayMass(mStrange) , "ms");
   ss << FORMAT_ELEMENT(24, mc                           , "mc");

   set_block(ss);
}

void SLHA_io::write_to_file(const std::string& file_name)
{
   std::ofstream ofs(file_name);
   write_to_stream(ofs);
}

void SLHA_io::write_to_stream(std::ostream& ostr)
{
   if (ostr.good())
      ostr << data;
   else
      ERROR("cannot write SLHA file");
}

/**
 * fill Modsel struct from given key - value pair
 *
 * @param modsel MODSEL data
 * @param key SLHA key in MODSEL
 * @param value value corresponding to key
 */
void SLHA_io::process_modsel_tuple(Modsel& modsel, int key, double value)
{
   switch (key) {
   case 1: // SUSY breaking model (defined in FlexibleSUSY model file)
   case 3: // SUSY model (defined in SARAH model file)
   case 4: // R-parity violation (defined in SARAH model file)
   case 5: // CP-parity violation (defined in SARAH model file)
   case 11:
   case 21:
      WARNING("Key " << key << " in Block MODSEL currently not supported");
      break;
   case 6: // Flavour violation (defined in SARAH model file)
   {
      const int ivalue = Round(value);

      if (ivalue < 0 || ivalue > 3)
         WARNING("Value " << ivalue << " in MODSEL block entry 6 out of range");

      modsel.quark_flavour_violated = ivalue & 0x1;
      modsel.lepton_flavour_violated = ivalue & 0x2;
   }
      break;
   case 12:
      modsel.parameter_output_scale = value;
      break;
   default:
      WARNING("Unrecognized entry in block MODSEL: " << key);
      break;
   }
}

/**
 * fill qedqcd from given key - value pair
 *
 * @param qedqcd low-energy data set
 * @param key SLHA key in SMINPUTS
 * @param value value corresponding to key
 */
void SLHA_io::process_sminputs_tuple(softsusy::QedQcd& qedqcd, int key, double value)
{
   using namespace softsusy;

   switch (key) {
   case 1:
      qedqcd.setAlpha(ALPHA, 1.0 / value);
      qedqcd.setAlphaEmInput(1.0 / value);
      break;
   case 2:
      qedqcd.setFermiConstant(value);
      break;
   case 3:
      qedqcd.setAlpha(ALPHAS, value);
      qedqcd.setAlphaSInput(value);
      break;
   case 4:
      qedqcd.setPoleMZ(value);
      qedqcd.setMu(value);
      softsusy::MZ = value;
      break;
   case 5:
      qedqcd.setMass(mBottom, value);
      qedqcd.setMbMb(value);
      break;
   case 6:
      qedqcd.setPoleMt(value);
      break;
   case 7:
      qedqcd.setMass(mTau, value);
      qedqcd.setPoleMtau(value);
      break;
   case 8:
      qedqcd.setNeutrinoPoleMass(3, value);
      break;
   case 9:
      qedqcd.setPoleMW(value);
      break;
   case 11:
      qedqcd.setMass(mElectron, value);
      qedqcd.setPoleMel(value);
      break;
   case 12:
      qedqcd.setNeutrinoPoleMass(1, value);
      break;
   case 13:
      qedqcd.setMass(mMuon, value);
      qedqcd.setPoleMmuon(value);
      break;
   case 14:
      qedqcd.setNeutrinoPoleMass(2, value);
      break;
   case 21:
      qedqcd.setMass(mDown, value);
      qedqcd.setMd2GeV(value);
      break;
   case 22:
      qedqcd.setMass(mUp, value);
      qedqcd.setMu2GeV(value);
      break;
   case 23:
      qedqcd.setMass(mStrange, value);
      qedqcd.setMs2GeV(value);
      break;
   case 24:
      qedqcd.setMass(mCharm, value);
      qedqcd.setMcMc(value);
      break;
   default:
      WARNING("Unrecognized entry in block SMINPUTS: " << key);
      break;
   }
}

void SLHA_io::process_flexiblesusy_tuple(Spectrum_generator_settings& settings,
                                         int key, double value)
{
   if (0 <= key && key < static_cast<int>(Spectrum_generator_settings::NUMBER_OF_OPTIONS)) {
      settings.set((Spectrum_generator_settings::Settings)key, value);
   } else {
      WARNING("Unrecognized entry in block FlexibleSUSY: " << key);
   }
}

void SLHA_io::process_flexiblesusyinput_tuple(
   Physical_input& input,
   int key, double value)
{
   if (0 <= key && key < static_cast<int>(Physical_input::NUMBER_OF_INPUT_PARAMETERS)) {
      input.set((Physical_input::Input)key, value);
   } else {
      WARNING("Unrecognized entry in block FlexibleSUSYInput: " << key);
   }
}

/**
 * fill CKM_wolfenstein from given key - value pair
 *
 * @param ckm_wolfenstein Wolfenstein parameters
 * @param key SLHA key in SMINPUTS
 * @param value value corresponding to key
 */
void SLHA_io::process_vckmin_tuple(CKM_wolfenstein& ckm_wolfenstein, int key, double value)
{
   switch (key) {
   case 1:
      ckm_wolfenstein.lambdaW = value;
      break;
   case 2:
      ckm_wolfenstein.aCkm = value;
      break;
   case 3:
      ckm_wolfenstein.rhobar = value;
      break;
   case 4:
      ckm_wolfenstein.etabar = value;
      break;
   default:
      WARNING("Unrecognized entry in block VCKMIN: " << key);
      break;
   }
}

/**
 * fill PMNS_parameters from given key - value pair
 *
 * @param pmns_parameters PMNS matrix parameters
 * @param key SLHA key in SMINPUTS
 * @param value value corresponding to key
 */
void SLHA_io::process_upmnsin_tuple(PMNS_parameters& pmns_parameters, int key, double value)
{
   switch (key) {
   case 1:
      pmns_parameters.theta_12 = value;
      break;
   case 2:
      pmns_parameters.theta_23 = value;
      break;
   case 3:
      pmns_parameters.theta_13 = value;
      break;
   case 4:
      pmns_parameters.delta = value;
      break;
   case 5:
      pmns_parameters.alpha_1 = value;
   case 6:
      pmns_parameters.alpha_2 = value;
      break;
   default:
      WARNING("Unrecognized entry in block UPMNSIN: " << key);
      break;
   }
}

} // namespace flexiblesusy


/*=========================================================================
 *
 *  Copyright David Doria 2011 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

// Submodules
#include <Helpers/Helpers.h>
#include <Mask/Mask.h>

// ITK
#include "itkBinaryThresholdImageFilter.h"
#include "itkImageRegionConstIterator.h"
#include "itkImageFileWriter.h"
#include "itkRandomImageSource.h"
#include "itkTimeProbe.h"
#include "itkVectorImage.h"

// Boost
#include <boost/bind.hpp>

// STL
#include <cstdlib>
#include <ctime>
#include <vector>

int main(int argc, char *argv[])
{
  unsigned int t = time(NULL);
  srand(t);

  itk::Size<2> size;
  size.Fill(100);

  itk::Index<2> index;
  index.Fill(0);

  itk::ImageRegion<2> region(index, size);
/*
  // Generate a random image (this method doesn't work with VectorImage)
  itk::RandomImageSource<FloatVectorImageType>::Pointer imageSource =
    itk::RandomImageSource<FloatVectorImageType>::New();
  imageSource->SetNumberOfThreads(1); // to produce non-random results
  imageSource->SetSize(size);
  imageSource->SetMin(0);
  imageSource->SetMax(100);
  imageSource->Update();
  FloatVectorImageType::Pointer image = imageSource->GetOutput();
*/
  // Generate a random image
  typedef itk::VectorImage<float, 2> FloatVectorImageType;
  FloatVectorImageType::Pointer image = FloatVectorImageType::New();
  image->SetRegions(region);
  image->SetNumberOfComponentsPerPixel(3);
  image->Allocate();

  {
  itk::ImageRegionIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());

  while(!imageIterator.IsAtEnd())
  {
    FloatVectorImageType::PixelType pixel;
    pixel.SetSize(3);
    pixel[0] = drand48();
    pixel[1] = drand48();
    pixel[2] = drand48();
    imageIterator.Set(pixel);
    ++imageIterator;
  }
  }

  // Generate a random membership image
  typedef itk::Image<int, 2> IntImageType;
  IntImageType::Pointer membershipImage = IntImageType::New();
  membershipImage->SetRegions(region);
  membershipImage->Allocate();
  membershipImage->FillBuffer(0);

  {
  itk::ImageRegionIterator<IntImageType> membershipImageIterator(membershipImage, membershipImage->GetLargestPossibleRegion());

  while(!membershipImageIterator.IsAtEnd())
  {
    IntImageType::PixelType pixel;
    pixel = rand() / 1000;
    membershipImageIterator.Set(pixel);
    ++membershipImageIterator;
  }
  }

  // Write the image
  itk::ImageFileWriter<FloatVectorImageType>::Pointer imageWriter =
    itk::ImageFileWriter<FloatVectorImageType>::New();
  imageWriter->SetFileName("image.mha");
  imageWriter->SetInput(image);
  imageWriter->Update();

//   // Generate a random mask
//   itk::RandomImageSource<Mask>::Pointer maskSource = itk::RandomImageSource<Mask>::New();
//   maskSource->SetNumberOfThreads(1); // to produce non-random results
//   maskSource->SetSize(size);
//   maskSource->SetMin(0);
//   maskSource->SetMax(255);
//   maskSource->Update();
//
//   // Threshold the mask
//   //typedef itk::ThresholdImageFilter <UnsignedCharImageType> ThresholdImageFilterType;
//   typedef itk::BinaryThresholdImageFilter <Mask, Mask> ThresholdImageFilterType;
//   ThresholdImageFilterType::Pointer thresholdFilter = ThresholdImageFilterType::New();
//   thresholdFilter->SetInput(maskSource->GetOutput());
//   thresholdFilter->SetLowerThreshold(0);
//   thresholdFilter->SetUpperThreshold(122);
//   thresholdFilter->SetOutsideValue(1);
//   thresholdFilter->SetInsideValue(0);
//   thresholdFilter->Update();
//   Mask::Pointer mask = thresholdFilter->GetOutput();

  std::cout << "Creating mask..." << std::endl;
  Mask::Pointer mask = Mask::New();
  mask->SetRegions(region);
  mask->Allocate();

  {
  itk::ImageRegionIterator<Mask> maskIterator(mask, mask->GetLargestPossibleRegion());

  while(!maskIterator.IsAtEnd())
  {
    int randomNumber = rand()%10;
    //std::cout << "randomNumber: " << randomNumber << std::endl;
    if(randomNumber > 5)
    {
      maskIterator.Set(mask->GetHoleValue());
    }
    else
    {
      maskIterator.Set(mask->GetValidValue());
    }
    ++maskIterator;
  }
  }
  std::cout << "Writing mask..." << std::endl;
  // Write the mask
  itk::ImageFileWriter<Mask>::Pointer maskWriter = itk::ImageFileWriter<Mask>::New();
  maskWriter->SetFileName("mask.png");
  maskWriter->SetInput(mask);
  maskWriter->Update();

  std::cout << "Creating source patches..." << std::endl;
  unsigned int patchRadius = 10;
  // Create source patches
  itk::ImageRegionConstIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());
  // std::vector<Patch> sourcePatches;
//   while(!imageIterator.IsAtEnd())
//     {
//     itk::Index<2> currentPixel = imageIterator.GetIndex();
//     itk::ImageRegion<2> region = Helpers::GetRegionInRadiusAroundPixel(currentPixel, patchRadius);
//     if(image->GetLargestPossibleRegion().IsInside(region))
//       {
//       sourcePatches.push_back(Patch(region));
//       }
//     ++imageIterator;
//     }
//   std::cout << "Source patches: " << sourcePatches.size() << std::endl;
  itk::Size<2> targetSize;
  targetSize.Fill(patchRadius * 2 + 1);

  itk::Index<2> targetIndex;
  targetIndex.Fill(3);

//   itk::ImageRegion<2> targetRegion(targetIndex, targetSize);
//   Patch targetPatch(targetRegion);
//
//   CandidatePairs pairs(targetPatch);
//   pairs.AddPairFromPatch(targetPatch);

//   itk::ImageRegion<2> adjacentRegion = targetRegion;
//   itk::Index<2> adjacentIndex;
//   adjacentIndex[0] = targetIndex[0] + 1;
//   adjacentIndex[1] = targetIndex[1] + 1;
//   adjacentRegion.SetIndex(adjacentIndex);
//   Patch adjacentPatch(adjacentRegion);
//   pairs.AddPairFromPatch(adjacentPatch);
  //pairs.AddPairFromPatch(sourcePatches[0]);

//   SelfPatchCompare patchCompare;
//   patchCompare.SetPairs(&pairs);
//   patchCompare.SetImage(image);
//   patchCompare.SetMask(mask);
//   patchCompare.SetNumberOfComponentsPerPixel(3);
//   patchCompare.SetMembershipImage(membershipImage);
//
//   patchCompare.FunctionsToCompute.push_back(boost::bind(&SelfPatchCompare::SetPatchMembershipDifference,&patchCompare,_1));
//   patchCompare.ComputeAllSourceDifferences();
//
//   std::cout << "pairs: " << pairs.size() << std::endl;
//   for(unsigned int i = 0; i < pairs.size(); ++i)
//     {
//     std::cout << "MembershipDifference: " << pairs[i].DifferenceMap[PatchPair::MembershipDifference] << std::endl;
//     }

  //unsigned int bestMatchSourcePatchId = patchCompare.FindBestPatch();
  //std::cout << "bestMatchSourcePatchId: " << bestMatchSourcePatchId << std::endl;
/*
  unsigned int patchId = 1;
  float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
  std::cout << "slowPatchDifference: " << slowPatchDifference << std::endl;

  float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
  std::cout << "fastPatchDifference: " << fastPatchDifference << std::endl;

  unsigned int iterations = 1e6;

  itk::TimeProbe slowTimer;
  slowTimer.Start();

  for(unsigned int i = 0; i < iterations; ++i)
    {
    float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
    }

  slowTimer.Stop();
  std::cout << "Slow Total: " << slowTimer.GetTotal() << std::endl;

  itk::TimeProbe fastTimer;
  fastTimer.Start();

  for(unsigned int i = 0; i < iterations; ++i)
    {
    float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
    }

  fastTimer.Stop();
  std::cout << "Fast Total: " << fastTimer.GetTotal() << std::endl;*/

  return EXIT_SUCCESS;
}


// Boost.Maral library (Molecular Archiving, Retrieval & Algorithm Library)
//
// Copyright (C) 2014 Armin Madadkar Sobhani
//
// Distributed under the Boost Software License Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// $Id$

#include <fstream>

// due to inclusion of <windows.h> by header only boost::test, we need
// the following define to prevent problem with std::numeric_limits
#   if defined(_MSC_VER)
#       define NOMINMAX
#   endif  //_MSC_VER

#define BOOST_TEST_MAIN
#include <boost/mpl/list.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#   if (BOOST_VERSION >= 105900)
#       include <boost/test/unit_test_parameters.hpp>
#   else
#       include <boost/test/detail/unit_test_parameters.hpp>
#   endif  //BOOST_VERSION

#include <maral/mtl/mtl.hpp>
#include <maral/units.hpp>

#define PATTERNS_FOLDER "patterns/"

using boost::test_tools::output_test_stream;
using namespace maral::mtl;
namespace utrc = boost::unit_test::runtime_config;

typedef boost::mpl::list<int,float,double,long double> test_types;
typedef boost::mpl::list<float,double,long double> float_types;

template<typename T>
struct ETHAN
{
    T ethan[24];

    ETHAN()
    {
        // Ref: Snyder L.C., Basch H. Molecular wave functions and properties:
        // tabulated form SCF calculation in a Guassian basis set, John Wiley
        // & Sons, p T-74, 1972
        ethan [0] = T( 0.00000000); ethan [1] = T( 0.00000000);
        ethan [2] = T( 0.00000000); // [0]  C1       H6
        ethan [3] = T( 0.00000000); ethan [4] = T( 0.00000000);
        ethan [5] = T( 2.91589999); // [3]  C2       |
        ethan [6] = T( 1.69855762); ethan [7] = T(-0.98066292);
        ethan [8] = T(-0.69996772); // [6]  H1  H5 - C2 - H4
        ethan [9] = T(-1.69855762); ethan[10] = T(-0.98066792);
        ethan[11] = T(-0.69996772); // [9]  H2       |
        ethan[12] = T( 0.00000000); ethan[13] = T( 1.96132584);
        ethan[14] = T(-0.69996772); // [12] H3  H2 - C1 - H3
        ethan[15] = T( 1.69855762); ethan[16] = T( 0.98066292);
        ethan[17] = T( 3.61586168); // [15] H4       |
        ethan[18] = T( 0.00000000); ethan[19] = T(-1.96132579);
        ethan[20] = T( 3.61586168); // [18] H5       H1
        ethan[21] = T(-1.69855762); ethan[22] = T( 0.98066292);
        ethan[23] = T( 3.61586168); // [21] H6
    }
};

BOOST_AUTO_TEST_SUITE(Points)

BOOST_AUTO_TEST_SUITE( Pnt2 )

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_RvalueRef, T, test_types)
{
    point2<T> pnt = point2<T>().zero();
    BOOST_CHECK(pnt == point2<T>().zero());
    BOOST_CHECK_EQUAL(pnt[0], (T)0);
    BOOST_CHECK_EQUAL(pnt[1], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Constructors, T, test_types)
{
    point2<T> pnt((T)1, (T)2);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_VectorExplicit, T, test_types)
{
    vector2<T> vec((T)1, (T)2);
    point2<T> pnt((vector2<T>)vec);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_CopyConstruct, T, test_types)
{
    point2<T> pnt;

    pnt[0] = (T)2;
    pnt[1] = (T)4;

    point2<T> point_copy(pnt);

    BOOST_CHECK_EQUAL(point_copy[0], (T)2);
    BOOST_CHECK_EQUAL(point_copy[1], (T)4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_X, T, test_types)
{
    point2<T> pnt;

    pnt.x() = (T)1;
    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt.x(), (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Y, T, test_types)
{
    point2<T> pnt;

    pnt.y() = (T)2;
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt.y(), (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_GetX, T, test_types)
{
    point2<T> pnt((T)1, (T)2);

    BOOST_CHECK_EQUAL(pnt.get_x(), (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_GetY, T, test_types)
{
    point2<T> pnt((T)1, (T)2);

    BOOST_CHECK_EQUAL(pnt.get_y(), (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_SetX, T, test_types)
{
    point2<T> pnt;

    pnt.set_x((T)1);
    BOOST_CHECK_EQUAL(pnt[0], (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_SetY, T, test_types)
{
    point2<T> pnt;

    pnt.set_y((T)2);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Set, T, test_types)
{
    point2<T> pnt;
    pnt.set((T)1, (T)2);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_SetPtr, T, test_types)
{
    T data[2] = {(T)1, (T)2};

    point2<T> pnt;
    pnt.set(data);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Zero, T, test_types)
{
    point2<T> pnt;

    BOOST_CHECK_EQUAL(pnt.zero()[0], (T)0);
    BOOST_CHECK_EQUAL(pnt[1], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_GetData, T, test_types)
{
    T* data;
    point2<T> pnt((T)1, (T)2);
    data = pnt.get_data();

    BOOST_CHECK_EQUAL(data[0], (T)1);
    BOOST_CHECK_EQUAL(data[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_BracketOp, T, test_types)
{
    point2<T> ref((T)1, (T)2);
    point2<T> pnt;

    pnt[0] = (T)1;
    pnt[1] = (T)2;
    BOOST_CHECK(ref == pnt);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE( Pnt2Ops )

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpEqualityCompare, T, test_types)
{
    point2<T> point1((T)1, (T)2);
    point2<T> point2(point1);

    BOOST_CHECK(point1 == point2);
    BOOST_CHECK(!(point1 != point2));

    point2 = point1;
    point2[0] = (T)21.10f;
    BOOST_CHECK(point1 != point2);
    BOOST_CHECK(!(point1 == point2));

    point2 = point1;
    point2[1] = (T)21.10f;
    BOOST_CHECK(point1 != point2);
    BOOST_CHECK(!(point1 == point2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpIsEqual, T, float_types)
{
    point2<T> pnt1(1.0, 2.0);
    point2<T> pnt2(pnt1);
    T eps(0.0);

    for(eps=(T)0.0; eps<(T)10.0; eps += (T)0.05)
    {
        BOOST_CHECK(is_equal(pnt1, pnt2, eps));
        BOOST_CHECK_CLOSE(pnt1[0], pnt2[0], eps);
        BOOST_CHECK_CLOSE(pnt1[1], pnt2[1], eps);
    }

    pnt1.set(1.0, 1.0);
    for(unsigned elt=0; elt<2; elt++)
    {
        pnt2 = pnt1;
        pnt2[elt] = (T)21.0;
        BOOST_CHECK(!is_equal(pnt1, pnt2, T(10.0)));
        BOOST_CHECK(!is_equal(pnt1, pnt2, T(19.9)));
        BOOST_CHECK( is_equal(pnt1, pnt2, T(20.1)));
        BOOST_CHECK( is_equal(pnt1, pnt2, T(22.0)));
    }

    pnt1.set( T(3.141593), T(938.27231) );
    pnt2.set(  T(3.14159),   T(938.272) );

    BOOST_CHECK(!is_equal(pnt1, pnt2, T(0)) );
    BOOST_CHECK(!is_equal(pnt1, pnt2, T(0.0001)) );
    BOOST_CHECK( is_equal(pnt1, pnt2, T(0.0005)) );
    BOOST_CHECK( is_equal(pnt1, pnt2, T(0.001)) );
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpPlusEq, T, test_types)
{
    point2<T> p((T)1, (T)2);
    vector2<T> v((T)2, (T)2);

    p += v;
    BOOST_CHECK_EQUAL(p[0], (T)3);
    BOOST_CHECK_EQUAL(p[1], (T)4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpPlus, T, test_types)
{
    point2<T> p((T)2, (T)2);
    vector2<T> v((T)1, (T)2);

    point2<T> r = p + v;
    BOOST_CHECK_EQUAL(r[0], (T)3);
    BOOST_CHECK_EQUAL(r[1], (T)4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMinusEq, T, test_types)
{
    point2<T> p((T)1, (T)2);
    vector2<T> v((T)2, (T)2);

    p -= v;
    BOOST_CHECK_EQUAL(p[0], (T)-1);
    BOOST_CHECK_EQUAL(p[1], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMinusVector, T, test_types)
{
    point2<T> point1((T)2, (T)2);
    point2<T> point2((T)1, (T)2);

    vector2<T> vec = point1 - point2;
    BOOST_CHECK_EQUAL(vec[0], (T)-1);
    BOOST_CHECK_EQUAL(vec[1], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMinus, T, test_types)
{
    point2<T> p((T)2, (T)2);
    vector2<T> v((T)1, (T)2);

    point2<T> r = p - v;
    BOOST_CHECK_EQUAL(r[0], (T)1);
    BOOST_CHECK_EQUAL(r[1], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpNegate, T, test_types)
{
    point2<T> pnt2((T)2, (T)-3);

    point2<T> pnt1 = -pnt2;
    BOOST_CHECK_EQUAL(pnt1[0], (T)-2);
    BOOST_CHECK_EQUAL(pnt1[1], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpAddScaled, T, test_types)
{
    point2<T> p((T)1, (T)2);
    vector2<T> v((T)2, (T)2);

    add_scaled((T)4, p, v);
    BOOST_CHECK_EQUAL(p[0], (T)9);
    BOOST_CHECK_EQUAL(p[1], (T)10);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpSumScaled, T, test_types)
{
    point2<T> p((T)1, (T)2);
    vector2<T> v((T)2, (T)2);

    point2<T> result = sum_scaled((T)4, p, v);
    BOOST_CHECK_EQUAL(result[0], (T)9);
    BOOST_CHECK_EQUAL(result[1], (T)10);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMaximize, T, test_types)
{
    point2<T> pnt1;
    point2<T> pnt2((T)1, (T)4);
    point2<T> pnt3((T)3, (T)2);

    maximize(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)3);
    BOOST_CHECK_EQUAL(pnt1[1], (T)4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMinimize, T, test_types)
{
    point2<T> pnt1;
    point2<T> pnt2((T)1, (T)4);
    point2<T> pnt3((T)3, (T)2);

    minimize(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)1);
    BOOST_CHECK_EQUAL(pnt1[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpMiddle, T, float_types)
{
    point2<T> pnt1;
    point2<T> pnt2((T)1, (T)2);
    point2<T> pnt3((T)3, (T)4);

    middle(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)2);
    BOOST_CHECK_EQUAL(pnt1[1], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpDistance, T, float_types)
{
    point2<T> point1((T)3, (T)4);
    point2<T> point2((T)1, (T)2);

    BOOST_CHECK_CLOSE(distance(point1, point2), T(2.828427f), SMALL);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpDistanceSq, T, test_types)
{
    point2<T> point1((T)3, (T)4);
    point2<T> point2((T)1, (T)2);

    BOOST_CHECK_EQUAL(distance2(point1, point2), T(8));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_OpAngle, T, float_types)
{
/// [point2 angle]
    namespace mu = maral::units;

    point2<T> pnt1(1, 2);
    point2<T> pnt2(1, 1);
    point2<T> pnt3(2, 1);

    auto ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(90.0), SMALL);

    pnt1.set(2, 2);
    ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(45.0), SMALL);

    pnt1.set(1, 1);
    ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(0.0), SMALL);
/// [point2 angle]
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Inserter, T, float_types)
{
    output_test_stream cout(
        PATTERNS_FOLDER"pntvec2.txt",
        !utrc::save_pattern() );
/// [point2 inserter]
    point2<T> pnt( T(3.141593), T(938.27231) );

    cout << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::fixed << std::setprecision(3)
         << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('|') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << setew(7) << delimiters('[', ']')
         << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::setw(15) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << spaces << delimiters('|') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << horizontal << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << separator(',') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('{', '}') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << nospaces << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << setew(0) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::setw(20) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('\0') << pnt << std::endl;
    BOOST_CHECK( cout.match_pattern() );
/// [point2 inserter]
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point2_Extractor, T, float_types)
{
    std::ifstream cin(PATTERNS_FOLDER"pntvec2.txt");
/// [point2 extractor]
    point2<T> pnt( T(3.141593), T(938.27231) );
    point2<T> ext;

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('|') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('[', ']') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('|') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> horizontal >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> separator(',') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('{', '}') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('\0') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );
/// [point2 extractor]
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE( Pnt3 )

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Constructors, T, test_types)
{
    point3<T> pnt((T)1, (T)2, (T)3);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_VectorExplicit, T, test_types)
{
    vector3<T> vec((T)1, (T)2, (T)3);
    point3<T> pnt((vector3<T>)vec);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_CopyConstruct, T, test_types)
{
    point3<T> pnt;

    pnt[0] = (T)2;
    pnt[1] = (T)4;
    pnt[2] = (T)6;

    point3<T> point_copy(pnt);

    BOOST_CHECK_EQUAL(point_copy[0], (T)2);
    BOOST_CHECK_EQUAL(point_copy[1], (T)4);
    BOOST_CHECK_EQUAL(point_copy[2], (T)6);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_X, T, test_types)
{
    point3<T> pnt;

    pnt.x() = (T)1;
    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt.x(), (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Y, T, test_types)
{
    point3<T> pnt;

    pnt.y() = (T)2;
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt.y(), (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Z, T, test_types)
{
    point3<T> pnt;

    pnt.z() = (T)3;
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
    BOOST_CHECK_EQUAL(pnt.z(), (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_GetX, T, test_types)
{
    point3<T> pnt((T)1, (T)2, (T)3);

    BOOST_CHECK_EQUAL(pnt.get_x(), (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_GetY, T, test_types)
{
    point3<T> pnt((T)1, (T)2, (T)3);

    BOOST_CHECK_EQUAL(pnt.get_y(), (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_GetZ, T, test_types)
{
    point3<T> pnt((T)1, (T)2, (T)3);

    BOOST_CHECK_EQUAL(pnt.get_z(), (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_SetX, T, test_types)
{
    point3<T> pnt;

    pnt.set_x((T)1);
    BOOST_CHECK_EQUAL(pnt[0], (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_SetY, T, test_types)
{
    point3<T> pnt;

    pnt.set_y((T)2);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_SetZ, T, test_types)
{
    point3<T> pnt;

    pnt.set_z((T)3);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Set, T, test_types)
{
    point3<T> pnt;
    pnt.set((T)1, (T)2, (T)3);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_SetPtr, T, test_types)
{
    T data[3] = {(T)1, (T)2, (T)3};

    point3<T> pnt;
    pnt.set(data);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Zero, T, test_types)
{
    point3<T> pnt;

    BOOST_CHECK_EQUAL(pnt.zero()[0], (T)0);
    BOOST_CHECK_EQUAL(pnt[1], (T)0);
    BOOST_CHECK_EQUAL(pnt[2], (T)0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_GetData, T, test_types)
{
    T* data;
    point3<T> pnt((T)1, (T)2, (T)3);
    data = pnt.get_data();

    BOOST_CHECK_EQUAL(data[0], (T)1);
    BOOST_CHECK_EQUAL(data[1], (T)2);
    BOOST_CHECK_EQUAL(data[2], (T)3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_BracketOp, T, test_types)
{
    point3<T> ref((T)1, (T)2, (T)3);
    point3<T> pnt;

    pnt[0] = (T)1;
    pnt[1] = (T)2;
    pnt[2] = (T)3;
    BOOST_CHECK(ref == pnt);

    BOOST_CHECK_EQUAL(pnt[0], (T)1);
    BOOST_CHECK_EQUAL(pnt[1], (T)2);
    BOOST_CHECK_EQUAL(pnt[2], (T)3);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE( Pnt3Ops )

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpEqualityCompare, T, test_types)
{
    point3<T> point1((T)1, (T)2, T(3));
    point3<T> point2(point1);

    BOOST_CHECK(point1 == point2);
    BOOST_CHECK(!(point1 != point2));

    point2 = point1;
    point2[0] = (T)21.10f;
    BOOST_CHECK(point1 != point2);
    BOOST_CHECK(!(point1 == point2));

    point2 = point1;
    point2[1] = (T)21.10f;
    BOOST_CHECK(point1 != point2);
    BOOST_CHECK(!(point1 == point2));

    point2 = point1;
    point2[2] = (T)21.10f;
    BOOST_CHECK(point1 != point2);
    BOOST_CHECK(!(point1 == point2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpIsEqual, T, float_types)
{
    point3<T> pnt1(1.0, 2.0, 3.0);
    point3<T> pnt2(pnt1);
    T eps(0.0);

    for(eps=(T)0.0; eps<(T)10.0; eps += (T)0.05)
    {
        BOOST_CHECK(is_equal(pnt1, pnt2, eps));
        BOOST_CHECK_CLOSE(pnt1[0], pnt2[0], eps);
        BOOST_CHECK_CLOSE(pnt1[1], pnt2[1], eps);
        BOOST_CHECK_CLOSE(pnt1[2], pnt2[2], eps);
    }

    pnt1.set(1.0f, 1.0f, 1.0f);
    for(unsigned elt=0; elt<3; elt++)
    {
        pnt2 = pnt1;
        pnt2[elt] = (T)21.0;
        BOOST_CHECK(!is_equal(pnt1, pnt2, T(10.0)));
        BOOST_CHECK(!is_equal(pnt1, pnt2, T(19.9)));
        BOOST_CHECK( is_equal(pnt1, pnt2, T(20.1)));
        BOOST_CHECK( is_equal(pnt1, pnt2, T(22.0)));
    }

    pnt1.set( T(3.141593), T(938.27231), T(2.718282) );
    pnt2.set(  T(3.14159),   T(938.272),  T(2.71828) );

    BOOST_CHECK(!is_equal(pnt1, pnt2, T(0)) );
    BOOST_CHECK(!is_equal(pnt1, pnt2, T(0.0001)) );
    BOOST_CHECK( is_equal(pnt1, pnt2, T(0.0005)) );
    BOOST_CHECK( is_equal(pnt1, pnt2, T(0.001)) );
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpPlusEq, T, test_types)
{
    point3<T> p((T)1, (T)2, (T)3);
    vector3<T> v((T)2, (T)2, (T)2);

    p += v;
    BOOST_CHECK_EQUAL(p[0], (T)3);
    BOOST_CHECK_EQUAL(p[1], (T)4);
    BOOST_CHECK_EQUAL(p[2], (T)5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpPlus, T, test_types)
{
    vector3<T> v((T)2, (T)2, (T)3);
    point3<T> p((T)1, (T)2, (T)3);

    point3<T> r = p + v;
    BOOST_CHECK_EQUAL(r[0], (T)3);
    BOOST_CHECK_EQUAL(r[1], (T)4);
    BOOST_CHECK_EQUAL(r[2], (T)6);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpMinusEq, T, test_types)
{
    point3<T> p((T)1, (T)2, (T)3);
    vector3<T> v((T)2, (T)2, (T)2);

    p -= v;
    BOOST_CHECK_EQUAL(p[0], (T)-1);
    BOOST_CHECK_EQUAL(p[1], (T)0);
    BOOST_CHECK_EQUAL(p[2], (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Vector3_OpMinusVector, T, test_types)
{
    point3<T> p1((T)2, (T)2, (T)4);
    point3<T> p2((T)1, (T)2, (T)5);

    vector3<T> vec = p1 - p2;
    BOOST_CHECK_EQUAL(vec[0], (T)-1);
    BOOST_CHECK_EQUAL(vec[1], (T)0);
    BOOST_CHECK_EQUAL(vec[2], (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpMinus, T, test_types)
{
    vector3<T> v((T)2, (T)2, (T)2);
    point3<T> p((T)1, (T)2, (T)3);

    point3<T> r = p - v;
    BOOST_CHECK_EQUAL(r[0], (T)-1);
    BOOST_CHECK_EQUAL(r[1], (T)0);
    BOOST_CHECK_EQUAL(r[2], (T)1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpNegate, T, test_types)
{
    point3<T> point2((T)2, (T)-3, (T)5);

    point3<T> point1 = -point2;
    BOOST_CHECK_EQUAL(point1[0], (T)-2);
    BOOST_CHECK_EQUAL(point1[1], (T)3);
    BOOST_CHECK_EQUAL(point1[2], (T)-5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpAddScaled, T, test_types)
{
    point3<T> p((T)1, (T)2, (T)3);
    vector3<T> v((T)2, (T)2, (T)2);

    add_scaled((T)4, p, v);
    BOOST_CHECK_EQUAL(p[0], (T)9);
    BOOST_CHECK_EQUAL(p[1], (T)10);
    BOOST_CHECK_EQUAL(p[2], (T)11);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpSumScaled, T, test_types)
{
    point3<T> p((T)1, (T)2, (T)3);
    vector3<T> v((T)2, (T)2, (T)2);

    point3<T> result = sum_scaled((T)4, p, v);
    BOOST_CHECK_EQUAL(result[0], (T)9);
    BOOST_CHECK_EQUAL(result[1], (T)10);
    BOOST_CHECK_EQUAL(result[2], (T)11);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpMaximize, T, test_types)
{
    point3<T> pnt1;
    point3<T> pnt2((T)1, (T)-4, (T)-1);
    point3<T> pnt3((T)3, (T)2, (T)-2);

    maximize(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)3);
    BOOST_CHECK_EQUAL(pnt1[1], (T)2);
    BOOST_CHECK_EQUAL(pnt1[2], (T)-1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpMinimize, T, test_types)
{
    point3<T> pnt1;
    point3<T> pnt2((T)1, (T)-4, (T)-1);
    point3<T> pnt3((T)3, (T)2, (T)-2);

    minimize(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)1);
    BOOST_CHECK_EQUAL(pnt1[1], (T)-4);
    BOOST_CHECK_EQUAL(pnt1[2], (T)-2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpMiddle, T, float_types)
{
    point3<T> pnt1;
    point3<T> pnt2((T)1, (T)2, (T)3);
    point3<T> pnt3((T)3, (T)4, (T)5);

    middle(pnt1, pnt2, pnt3);
    BOOST_CHECK_EQUAL(pnt1[0], (T)2);
    BOOST_CHECK_EQUAL(pnt1[1], (T)3);
    BOOST_CHECK_EQUAL(pnt1[2], (T)4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpDistance, T, float_types)
{
    point3<T> pnt1((T)3, (T)4, (T)5);
    point3<T> pnt2((T)1, (T)2, (T)5);

    BOOST_CHECK_CLOSE(distance(pnt1, pnt2), T(2.828427f), SMALL);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_OpDistanceSq, T, test_types)
{
    point3<T> pnt1((T)3, (T)4, (T)3);
    point3<T> pnt2((T)1, (T)2, (T)5);

    BOOST_CHECK_EQUAL(distance2(pnt1, pnt2), T(12));
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(Point3_OpAngle, T, float_types, ETHAN<T>)
{
/// [point3 angle]
    namespace mu = maral::units;

    point3<T> pnt1(1, 1, 0);
    point3<T> pnt2(1, 0, 0);
    point3<T> pnt3(2, 0, 1);

    auto ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(90.0), SMALL);

//    std::cout << ang << " ("
//              << mu::to_degrees(ang) << ", "
//              << mu::to_gradians(ang)
//              << ')' << std::endl;

    pnt1.set(2, 0, 0);
    ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(45.0), SMALL);

    pnt1.set(0, 0, 0);
    ang = mu::radians( angle(pnt1, pnt2, pnt3) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(135.0), SMALL);

    point3<T> C1;
    C1.set(&ETHAN<T>::ethan[0]);
    point3<T> C2;
    C2.set(&ETHAN<T>::ethan[3]);
    point3<T> H1;
    H1.set(&ETHAN<T>::ethan[6]);
    ang = mu::radians( angle(C2, C1, H1) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), 109.640722, SMALL);
    ang = mu::radians( angle(H1, C1, C2) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), 109.640722, SMALL);
/// [point3 angle]
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(Point3_OpTorsionAngle, T, float_types, ETHAN<T>)
{
/// [point3 torsion angle]
    namespace mu = maral::units;

    point3<T> C1;
    C1.set(&ETHAN<T>::ethan[0]);
    point3<T> C2;
    C2.set(&ETHAN<T>::ethan[3]);
    point3<T> H1;
    H1.set(&ETHAN<T>::ethan[6]);
    point3<T> H4;
    H4.set(&ETHAN<T>::ethan[15]);
    point3<T> H5;
    H5.set(&ETHAN<T>::ethan[18]);
    point3<T> H6;
    H6.set(&ETHAN<T>::ethan[21]);

    auto ang = mu::radians( torsion_angle(H1, C1, C2, H4) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(-60.0), T(0.001));

    ang = mu::radians( torsion_angle(H4, C2, C1, H1) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(-60.0), T(0.001));

    ang = mu::radians( torsion_angle(H1, C1, C2, H5) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(60.0), T(0.001));

    ang = mu::radians( torsion_angle(H5, C2, C1, H1) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(60.0), T(0.001));

    ang = mu::radians( torsion_angle(H1, C1, C2, H6) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(-180.0), T(0.001));

    ang = mu::radians( torsion_angle(H6, C2, C1, H1) );
    BOOST_CHECK_CLOSE(mu::to_degrees(ang).value(), T(-180.0), T(0.001));
/// [point3 torsion angle]
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Inserter, T, float_types)
{
    output_test_stream cout(
        PATTERNS_FOLDER"pntvec3.txt",
        !utrc::save_pattern() );
/// [point3 inserter]
    point3<T> pnt( T(3.141593), T(938.27231), T(2.718282) );

    cout << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::fixed << std::setprecision(3)
         << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('|') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << setew(7) << delimiters('[', ']')
         << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::setw(15) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << spaces << delimiters('|') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << horizontal << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << separator(',') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('{', '}') << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << nospaces << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << setew(0) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << std::setw(25) << pnt << std::endl << std::endl;
    BOOST_CHECK( cout.match_pattern() );

    cout << delimiters('\0') << pnt << std::endl;
    BOOST_CHECK( cout.match_pattern() );
/// [point3 inserter]
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Point3_Extractor, T, float_types)
{
    std::ifstream cin(PATTERNS_FOLDER"pntvec3.txt");
/// [point3 extractor]
    point3<T> pnt( T(3.141593), T(938.27231), T(2.718282) );
    point3<T> ext;

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('|') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('[', ']') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('|') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> horizontal >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> separator(',') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('{', '}') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );

    cin >> delimiters('\0') >> ext.zero();
    BOOST_CHECK( is_equal(pnt, ext) );
/// [point3 extractor]
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/*
    Copyright 2016 Emanuele Vespa, Imperial College London
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    3. Neither the name of the copyright holder nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   POSSIBILITY OF SUCH DAMAGE.

*/
#ifndef OCTANT_OPS_HPP
#define OCTANT_OPS_HPP
#include "octree_defines.h"
#include "utils/math_utils.h"
#include "utils/morton_utils.hpp"

#include <supereight/shared/commons.h>

#include <Eigen/Dense>
#include <bitset>
#include <iostream>

namespace se {
namespace keyops {

SE_DEVICE_FUNC
inline se::key_t code(const se::key_t key) { return key & ~SCALE_MASK; }

SE_DEVICE_FUNC
inline int level(const se::key_t key) { return key & SCALE_MASK; }

SE_DEVICE_ONLY_FUNC
inline se::key_t encode(const int x, const int y, const int z, const int level,
    const int max_depth) {
    const int offset = MAX_BITS - max_depth + level - 1;
    return (compute_morton(x, y, z) & MASK[offset] & ~SCALE_MASK) | level;
}

SE_DEVICE_FUNC
inline Eigen::Vector3i decode(const se::key_t key) {
    return unpack_morton(key & ~SCALE_MASK);
}
} // namespace keyops

/*
 * Algorithm 5 of p4est paper: https://epubs.siam.org/doi/abs/10.1137/100791634
 */
SE_DEVICE_FUNC
inline Eigen::Vector3i face_neighbour(const se::key_t o,
    const unsigned int face, const unsigned int l,
    const unsigned int max_depth) {
    Eigen::Vector3i coords  = se::keyops::decode(o);
    const unsigned int side = 1 << (max_depth - l);
    coords(0) = coords(0) + ((face == 0) ? -side : (face == 1) ? side : 0);
    coords(1) = coords(1) + ((face == 2) ? -side : (face == 3) ? side : 0);
    coords(2) = coords(2) + ((face == 4) ? -side : (face == 5) ? side : 0);
    return {coords(0), coords(1), coords(2)};
}

/*
 * \brief Return true if octant is a descendant of ancestor
 * \param octant
 * \param ancestor
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_ONLY_FUNC
inline bool descendant(
    se::key_t octant, se::key_t ancestor, const int max_depth) {
    const int level = se::keyops::level(ancestor);
    const int idx   = MAX_BITS - max_depth + level - 1;
    ancestor        = se::keyops::code(ancestor);
    octant          = se::keyops::code(octant) & MASK[idx];
    return (ancestor ^ octant) == 0;
}

/*
 * \brief Computes the parent's morton code of a given octant
 * \param octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_ONLY_FUNC
inline se::key_t parent(const se::key_t& octant, const int max_depth) {
    const int level = se::keyops::level(octant) - 1;
    const int idx   = MAX_BITS - max_depth + level - 1;
    return (octant & MASK[idx]) | level;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_FUNC
inline int child_id(se::key_t octant, const int level, const int max_depth) {
    int shift = max_depth - level;
    octant    = se::keyops::code(octant) >> shift * 3;
    int idx   = (octant & 0x01) | (octant & 0x02) | (octant & 0x04);
    return idx;
}

/*
 * \brief Computes the octants's corner which is not shared with its siblings
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_FUNC
inline Eigen::Vector3i far_corner(
    const se::key_t octant, const int level, const int max_depth) {
    const unsigned int side           = 1 << (max_depth - level);
    const int idx                     = child_id(octant, level, max_depth);
    const Eigen::Vector3i coordinates = se::keyops::decode(octant);
    return Eigen::Vector3i(coordinates(0) + (idx & 1) * side,
        coordinates(1) + ((idx & 2) >> 1) * side,
        coordinates(2) + ((idx & 4) >> 2) * side);
}

/*
 * \brief Computes the non-sibling neighbourhood around an octants. In the
 * special case in which the octant lies on an edge, neighbour are duplicated
 * as movement outside the enclosing cube is forbidden.
 * \param result 7-vector containing the neighbours
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_ONLY_FUNC
inline void exterior_neighbours(se::key_t result[7], const se::key_t octant,
    const int level, const int max_depth) {
    const int idx       = child_id(octant, level, max_depth);
    Eigen::Vector3i dir = Eigen::Vector3i(
        (idx & 1) ? 1 : -1, (idx & 2) ? 1 : -1, (idx & 4) ? 1 : -1);
    Eigen::Vector3i base = far_corner(octant, level, max_depth);
    dir(0) =
        se::math::in(base(0) + dir(0), 0, (1 << max_depth) - 1) ? dir(0) : 0;
    dir(1) =
        se::math::in(base(1) + dir(1), 0, (1 << max_depth) - 1) ? dir(1) : 0;
    dir(2) =
        se::math::in(base(2) + dir(2), 0, (1 << max_depth) - 1) ? dir(2) : 0;

    result[0] = se::keyops::encode(
        base(0) + dir(0), base(1) + 0, base(2) + 0, level, max_depth);
    result[1] = se::keyops::encode(
        base(0) + 0, base(1) + dir(1), base(2) + 0, level, max_depth);
    result[2] = se::keyops::encode(
        base(0) + dir(0), base(1) + dir(1), base(2) + 0, level, max_depth);
    result[3] = se::keyops::encode(
        base(0) + 0, base(1) + 0, base(2) + dir(2), level, max_depth);
    result[4] = se::keyops::encode(
        base(0) + dir(0), base(1) + 0, base(2) + dir(2), level, max_depth);
    result[5] = se::keyops::encode(
        base(0) + 0, base(1) + dir(1), base(2) + dir(2), level, max_depth);
    result[6] = se::keyops::encode(
        base(0) + dir(0), base(1) + dir(1), base(2) + dir(2), level, max_depth);
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, max_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant octant coordinates
 * \param level level of the octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_FUNC
static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const Eigen::Vector3i& octant, const int level, const int max_depth) {
    const Eigen::Vector3i base = octant;
    const int size             = 1 << max_depth;
    const int step             = 1 << (max_depth - level);
    Eigen::Matrix<int, 4, 6> cross;
    res << -step, step, 0, 0, 0, 0, 0, 0, -step, step, 0, 0, 0, 0, 0, 0, -step,
        step, 0, 0, 0, 0, 0, 0;
    res.colwise() += base.homogeneous();
    res = res.unaryExpr(
        [size](const int a) { return std::max(std::min(a, size - 1), 0); });
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, max_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant octant key
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_FUNC
static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const se::key_t octant, const int max_depth) {
    one_neighbourhood(
        res, se::keyops::decode(octant), se::keyops::level(octant), max_depth);
}

/*
 * \brief Computes the morton number of all siblings around an octant,
 * including itself.
 * \param result 8-vector containing the neighbours
 * \param octant
 * \param max_depth max depth of the tree on which the octant lives
 */
SE_DEVICE_ONLY_FUNC
inline void siblings(
    se::key_t result[8], const se::key_t octant, const int max_depth) {
    const int level   = (octant & SCALE_MASK);
    const int shift   = 3 * (max_depth - level);
    const se::key_t p = parent(octant, max_depth) + 1; // set-up next level
    for (int i = 0; i < 8; ++i) { result[i] = p | (i << shift); }
}
} // namespace se
#endif


#include <boost/units/systems/information/bit.hpp>


#include <boost/units/systems/angle/revolutions.hpp>


#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/comparison.hpp>
#include <nt2/sdk/aligned/align_on.hpp>

using boost::mpl::int_;
using boost::mpl::equal_to;
using nt2::meta::align_on;

int main()
{
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<0>,int_<4> >::type,int_<0> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<1>,int_<4> >::type,int_<4> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<1055>,int_<4> >::type,int_<1056> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<1055> >::type,int_<1088> >::type ));
}


#pragma once

#include <xtensor/xtensor.hpp>
#include <boost/iterator/counting_iterator.hpp>

namespace nifty{
    namespace graph{

        template<class NODE_INTEGER_TYPE, bool SELF_LOOPS = false>
        class DenseUGraph{
        public:

            typedef NODE_INTEGER_TYPE   NodeType;
            typedef std::pair<NodeType, NodeType> EdgeType;

            typedef NodeType NodeConstRef;
            typedef EdgeType EdgeConstRef;

            boost::counting_iterator<NodeType> NodeIter;
            //boost::counting_iterator<EdgeType> EdgeIter;

            DenseUGraph(const NodeType & n_nodes = 0)
            :   n_nodes_(n_nodes),
                n_edges_(0),
                edge_matrix_({n_nodes, n_nodes})
            {
                edge_matrix_ = false;
            }

            NodeType n_edges()const{
                return n_edges_;
            }
            NodeType n_nodes()const{
                return n_nodes_;
            }

            NodeIter nodes_begin()const{
                return NodeIter(NodeType(0));
            }
            NodeIter nodes_end()const{
                return NodeIter(NodeType(n_nodes_));
            }
            EdgeIter edges_begin()const{
                return EdgeIter(EdgeType(0));
            }
            EdgeIter edges_end()const{
                return EdgeIter(EdgeType(n_edges_));
            }

            std::pair<EdgeType, bool> add_edge(const NodeType & u, const NodeType & v){
                const auto edge = EdgeType(std::min(u,v), std::max(u,v));
                if(!edges_(u, v)){
                    edges_(u,v) = true;
                    edges_(v,u) = true;
                    return std::make_pair(edge, true);
                }
                else{
                    return std::make_pair(edge, false);
                }
            }

            std::pair<EdgeType, bool> find_edge(const NodeType & u, const NodeType & v){
                if(!edges_(u, v)){
                    return std::make_pair(EdgeType(), true);
                }
                else{
                    const auto edge = EdgeType(std::min(u,v), std::max(u,v));
                    return std::make_pair(edge, false);
                }
            }

            bool contains_node(const NodeType & node)const{
                return node < n_nodes_;
            }
            bool contains_edge(const EdgeType & edge)const{
                return edges_(edge.first, edge.second);
            }

        private:

            NodeType n_nodes_;
            EdgeType n_edges_;

            xtensor::xtensor<2, bool> edges_;

        };

    }
}