/*
imgalt - Image Alignment Tool
Author: GreatAttractor

version 0.5
2014/05/22

This code can be freely distributed and used for any purpose.

File description:
    Utility functions implementation.

*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <stdexcept>
#include <fstream>
#include <cctype>
#include <algorithm>
#include <boost/format.hpp>
#include "comp.h"
using namespace boost;

int GetBytesPerPixel(PixelFormat_t pixFmt)
{
    switch (pixFmt)
    {
    case PIX_PAL8:
    case PIX_MONO8:
        return 1;
    case PIX_MONO16: return 2;
    case PIX_RGB24: return 3;
    case PIX_RGB48: return 3*2;
    case PIX_MONO32F: return sizeof(float);
    default: return -1;
    }
}

/// Returns the smallest power of 2 which is > n
unsigned GetClosestGPowerOf2(unsigned n)
{
    int msb = 0;
    while (n != 0)
    {
        n >>= 1;
        msb++;
    }

    return ((unsigned)1 << msb);
}

/// Returns the greatest power of 2 which is <= n
unsigned GetClosestLEPowerOf2(unsigned n)
{
    int msb = 0;
    while (n != 0)
    {
        n >>= 1;
        msb++;
    }

    return ((unsigned)1 << (msb-1));
}

/// Returns 0 for x=0, 1 for x=1
inline float CosineWindow(float x)
{
    return 0.5f*(1.0f - cosf(3.1415926535f*x));
}

/// Returns 0 for x=0, 1 for x=1
inline float BlackmanWindow(float x)
{
    const float A0 = 7938.0f/18608,
                A1 = 9240.0f/18608,
                A2 = 1430.0f/18608;

    return A0 - A1*cosf(3.1415926535f*x) + A2*cosf(2*3.1415926535f*x);
}

#define SQR(x) ((x)*(x))

/// Calculates window function and writes its values into 'buf' (a square array)
void CalcWindowFunction(
        int wndSize, ///< Window size
        float buf[] ///< Destination buffer, wndSize*wndSize elements
)
{
    // The window function is rotationally symmetrical, so calculate it only in a quarter of 'buf'
    #pragma omp parallel for
    for (int y = 0; y < wndSize/2; y++)
        for (int x = 0; x < wndSize/2; x++)
        {
            float value = 0.0f;
            float dist = sqrtf(SQR(wndSize/2 - x) + SQR(wndSize/2 - y));
            if (dist < wndSize/2)
                value = BlackmanWindow(1.0f-dist/(wndSize/2));

            // upper left
            buf[x + y*wndSize] = value;
            // upper right
            buf[wndSize-1-x + y*wndSize] = value;
            // lower right
            buf[wndSize-1-x + (wndSize-1-y)*wndSize] = value;
            // lower left
            buf[x + (wndSize-1-y)*wndSize] = value;
        }
}

/// Multiplies input image by window function; input and output images may be the same
void ApplyWindowFunction(float img[], ///< Input image, size*size elements; may be the same pointer as 'dest'
        float windowFunc[], ///< Window function values, size*size elements
        float dest[], ///< Output buffer, size*size elements; may be the same pointer as 'img'
        int size ///< Number of rows and columns in each array
)
{
    #pragma omp parallel for
    for (int y = 0; y < size; y++)
        for (int x = 0; x < size; x++)
            dest[x + y*size] = img[x + y*size] * windowFunc[x + y*size];
}

/// Resizes and translates image (or its fragment) by cropping and/or padding (with zeros) to the destination size and offset (there is no scaling)
void ResizeAndTranslate(
        void *input,          ///< Input buffer
        PixelFormat_t pixFmt, ///< Format of data in 'input' and 'output'
        int srcWidth,         ///< Width of input image
        int srcHeight,        ///< Height of input image
        int srcXmin,          ///< X min of input data in input image
        int srcYmin,          ///< Y min of input data in input image
        int srcXmax,          ///< X max of input data in input image
        int srcYmax,          ///< Y max of input data in input image
        void *output,         ///< Output buffer
        int destWidth,        ///< Width of output image
        int destHeight,       ///< Height of output image
        int xOfs,             ///< X offset of input data in output buffer
        int yOfs              ///< Y offset of input data in output buffer
)
{
    int bytesPP = GetBytesPerPixel(pixFmt);
    memset(output, 0, destWidth*destHeight*bytesPP); // Works also if 'dest' points to an array of floats; 32 zero bits represent a floating-point 0.0f

    // start and end (inclusive) coordinates to fill in the output buffer
    unsigned Xstart = (xOfs < 0) ? 0 : xOfs;
    unsigned Ystart = (yOfs < 0) ? 0 : yOfs;

    unsigned Xend = std::max(0, std::min((int)xOfs + srcXmax, destWidth-1));
    unsigned Yend = std::max(0, std::min((int)yOfs + srcYmax, destHeight-1));

    for (int y = Ystart; y <= Yend; y++)
    {
        memcpy((uint8_t *)output + (Xstart + y*destWidth)*bytesPP,
               (uint8_t *)input + (Xstart-xOfs+srcXmin + (y-yOfs + srcYmin)*srcWidth)*bytesPP,
               (Xend - Xstart + 1)*bytesPP);
    }
}

/// Blurs 'src' image using a 5x5 Gaussian kernel and writes the result to 'dest'; 2 pixel-wide borders are not blurred
void BlurImage(
    uint8_t *src, ///< Source image (8-bit luminance)
    int width,    ///< Image width
    int height,   ///< Image height
    uint8_t *dest ///< Destination image (same dimensions as 'src')
)
{
    const int KERNEL_SIZE = 5;
    const int kernel[KERNEL_SIZE][KERNEL_SIZE] =
    {
        { 2, 4,  5,  4,  2 },
        { 4, 9,  12, 9,  4 },
        { 5, 12, 15, 12, 5 },
        { 4, 9,  12, 9,  4 },
        { 2, 4,  5,  4,  2 }
    };

    for (int y = KERNEL_SIZE/2; y <= height - KERNEL_SIZE/2 - 1; y++)
        for (int x = KERNEL_SIZE/2; x <= width - KERNEL_SIZE/2 - 1; x++)
        {
            int sum = 0;
            for (int yofs = -KERNEL_SIZE/2; yofs <= KERNEL_SIZE/2; yofs++)
                for (int xofs = -KERNEL_SIZE/2; xofs <= KERNEL_SIZE/2; xofs++)
                    sum += (int)src[(x + xofs) + (y + yofs) * width] * kernel[xofs + KERNEL_SIZE/2][yofs + KERNEL_SIZE/2];

            dest[x + y*width] = sum/159;
        }
}

/// Calculates squared gradient lengths in the source image; 3 pixel-wide borders are skipped
void CalcGradients(
    uint8_t *src,  ///< Source image
    int width,     ///< Image width
    int height,    ///< Image height
    uint32_t *dest ///< Destination buffer (same dimensions as 'src')
)
{
    const int KERNEL_SIZE = 3;
    const int kernelX[KERNEL_SIZE][KERNEL_SIZE] =
    {
        { -1, 0, 1 },
        { -2, 0, 2 },
        { -1, 0, 1 }
    };

    const int kernelY[KERNEL_SIZE][KERNEL_SIZE] =
    {
        { 1,   2,  1 },
        { 0,   0,  0 },
        { -1, -2, -1 }
    };

    /// By skipping 3-pixel borders we skip the pixels that are also skipped by BlurImage()
    for (int y = 3; y <= height - 4; y++)
        for (int x = 3; x <= width - 4; x++)
        {
            int gradX = 0, gradY = 0;
            for (int yofs = -KERNEL_SIZE/2; yofs <= KERNEL_SIZE/2; yofs++)
                for (int xofs = -KERNEL_SIZE/2; xofs <= KERNEL_SIZE/2; xofs++)
                {
                    uint8_t srcVal = src[(x + xofs) + (y + yofs) * width];
                    gradX += srcVal * kernelX[xofs + KERNEL_SIZE/2][yofs + KERNEL_SIZE/2];
                    gradY += srcVal * kernelY[xofs + KERNEL_SIZE/2][yofs + KERNEL_SIZE/2];
                }

            dest[x + y*width] = gradX*gradX + gradY*gradY; // this is at most 2*(4*255)^2, so fits easily in uint32_t
        }
}

namespace BMP
{

#pragma pack(push)

#pragma pack(1)
typedef struct
{
    uint16_t bfType;
    uint32_t bfSize;
    uint16_t bfReserved1;
    uint16_t bfReserved2;
    uint32_t bfOffBits;
} BITMAPFILEHEADER_t; ///< BMP file header

#pragma pack(1)
typedef struct
{
   uint32_t biSize;
   int32_t biWidth;
   int32_t biHeight;
   uint16_t biPlanes;
   uint16_t biBitCount;
   uint32_t biCompression;
   uint32_t biSizeImage;
   int32_t biXPelsPerMeter;
   int32_t biYPelsPerMeter;
   uint32_t biClrUsed;
   uint32_t biClrImportant;
} BITMAPINFOHEADER_t; ///< BMP info header

#pragma pack(pop)

const uint32_t BMP_NO_COMPRESSION = 0;
const int BMP_PALETTE_SIZE = 256*4;

// returns the least multiple of 4 which is >= x
#define UP4MULT(x) (((x)+3)/4*4)

/// Reads a BMP image and returns pointer to the newly allocated buffer with pixel contents or 0 on error
void *ReadBmp(const char *fileName,
        int &imgWidth,  ///< Receives image width
        int &imgHeight, ///< Receives image height
        PixelFormat_t &pixFmt, ///< Receives the pixel format
        uint8_t palette[]    ///< If not NULL and reading from an 8-bit file, receives the palette (1024 bytes)
)
{
    std::ifstream file(fileName, std::ios_base::in | std::ios_base::binary);
    if (file.fail())
        return 0;

    BITMAPFILEHEADER_t bmpFileHdr;
    BITMAPINFOHEADER_t bmpInfoHdr;

    file.read((char *)&bmpFileHdr, sizeof(bmpFileHdr));
    file.read((char *)&bmpInfoHdr, sizeof(bmpInfoHdr));
    if (file.eof())
        return 0;

    imgWidth = bmpInfoHdr.biWidth;
    imgHeight = bmpInfoHdr.biHeight;
    if (imgWidth == 0 || imgHeight == 0 ||
        bmpFileHdr.bfType != 'B'+((int)'M'<<8) ||
        bmpInfoHdr.biPlanes != 1 ||
        bmpInfoHdr.biBitCount != 8 && bmpInfoHdr.biBitCount != 24 ||
        bmpInfoHdr.biCompression != BMP_NO_COMPRESSION)
    {
        return 0;
    }

    if (bmpInfoHdr.biBitCount == 8)
        pixFmt = PIX_PAL8;
    else if (bmpInfoHdr.biBitCount == 24)
        pixFmt = PIX_RGB24;

    int bytesPP = GetBytesPerPixel(pixFmt);

    void *pixels = malloc(imgWidth * imgHeight * bytesPP);

    if (bmpInfoHdr.biBitCount == 8)
    {
        unsigned bmpStride = UP4MULT(imgWidth); // line length in bytes in the BMP file's pixel data
        unsigned skip = bmpStride - imgWidth; // number of padding bytes at the end of a line

        int actualPalSize = bmpInfoHdr.biClrUsed == 0 ? BMP_PALETTE_SIZE : bmpInfoHdr.biClrUsed*4;

        // seek to the beginning of palette
        file.seekg(sizeof(bmpFileHdr) + bmpInfoHdr.biSize, std::ios_base::beg);

        if (palette != 0)
            file.read((char *)palette, actualPalSize);

        // Seek to the beginning of pixel values
        file.seekg(bmpFileHdr.bfOffBits, std::ios_base::beg);

        for (int y = imgHeight - 1; y >= 0; y--) // lines in BMP are stored bottom to top
        {
            file.read((char *)((uint8_t *)pixels + y*imgWidth), imgWidth);
            if (skip > 0)
                file.seekg(skip, std::ios_base::cur);
        }
    }
    else if (bmpInfoHdr.biBitCount == 24)
    {
        unsigned bmpStride = UP4MULT(imgWidth*3); // line length in bytes in the BMP file's pixel data
        unsigned skip = bmpStride - imgWidth*3; // number of padding bytes at the end of a row

        // Seek to the beginning of pixel values
        file.seekg(bmpFileHdr.bfOffBits, std::ios_base::beg);

        // read the lines directly into the buffer
        for (int y = imgHeight - 1; y >= 0; y--) // lines in BMP are stored bottom to top
        {
            file.read((char *)((uint8_t *)pixels + y*imgWidth*3), imgWidth*3);
            if (skip > 0)
                file.seekg(skip, std::ios_base::cur);
        }
    }

    return pixels;
}

/// Saves image in BMP format; returns 'false' on error
bool SaveBmp(const char *fileName, ///< Output file name
        int imgWidth,              ///< Image width
        int imgHeight,             ///< Image height
        PixelFormat_t pixFmt,      ///< Pixel format; has to be PIX_PAL8 or PIX_RGB24
        void *pixels,              ///< Pixel contents in 'pixFmt' format
        uint8_t palette[]          ///< Points to the palette (1024 bytes) to be saved if pixFmt is PIX_PAL8
)
{
    BITMAPFILEHEADER_t bmfh;
    BITMAPINFOHEADER_t bmih;
    int i;

    int bytesPP = GetBytesPerPixel(pixFmt);
    unsigned bmpLineWidth = UP4MULT(imgWidth * bytesPP);

    bmfh.bfType = 'B'+((int)'M'<<8);
    bmfh.bfSize = sizeof(bmfh) + sizeof(bmih) + imgHeight*bmpLineWidth;
    if (pixFmt == PIX_PAL8)
        bmfh.bfSize += BMP_PALETTE_SIZE;
    bmfh.bfReserved1 = 0;
    bmfh.bfReserved2 = 0;
    bmfh.bfOffBits = sizeof(bmih) + sizeof(bmfh);
    if (pixFmt == PIX_PAL8)
        bmfh.bfOffBits += BMP_PALETTE_SIZE;

    bmih.biSize = sizeof(bmih);
    bmih.biWidth = imgWidth;
    bmih.biHeight = imgHeight;
    bmih.biPlanes = 1;
    bmih.biBitCount = bytesPP * 8;
    bmih.biCompression = BMP_NO_COMPRESSION;
    bmih.biSizeImage = 0;
    bmih.biXPelsPerMeter = 1000;
    bmih.biYPelsPerMeter = 1000;
    bmih.biClrUsed = 0;
    bmih.biClrImportant = 0;

    std::ofstream file(fileName, std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
    if (file.fail())
        return false;

    file.write((const char *)&bmfh, sizeof(bmfh));
    file.write((const char *)&bmih, sizeof(bmih));
    if (pixFmt == PIX_PAL8)
        file.write((const char *)palette, BMP_PALETTE_SIZE);

    int skip = bmpLineWidth - imgWidth*bytesPP;

    for (i = imgHeight - 1; i >= 0; i--) // lines in BMP are stored bottom to top
    {
        file.write((const char*)pixels + i * imgWidth * bytesPP, imgWidth*bytesPP);
        if (skip > 0)
            file.write((const char *)pixels, skip); //this is just padding, so write anything
    }

    file.close();

    return true;
}

bool GetBmpDimensions(const char *fileName, unsigned &imgWidth, unsigned &imgHeight)
{
    std::ifstream file(fileName, std::ios_base::in | std::ios_base::binary);
    if (file.fail())
        return false;

    BITMAPFILEHEADER_t bmpFileHdr;
    BITMAPINFOHEADER_t bmpInfoHdr;

    file.read((char *)&bmpFileHdr, sizeof(bmpFileHdr));
    file.read((char *)&bmpInfoHdr, sizeof(bmpInfoHdr));
    if (file.eof())
        return false;

    imgWidth = bmpInfoHdr.biWidth;
    imgHeight = bmpInfoHdr.biHeight;

    return true;
}

} // namespace BMP

namespace TIFF
{

#pragma pack(push, 1)
typedef struct
{
    uint16_t tag;
    uint16_t type;
    uint32_t count;
    uint32_t value;
} TiffField_t;

typedef struct
{
    uint16_t id;
    uint16_t version;
    uint32_t dirOffset; // = offset of 'numDirEntries'
} TiffHeader_t;

#pragma pack(pop)

typedef enum { ttByte = 1, ttAscii, ttWord, ttDWord, ttRational } TagType_t;

const int TIFF_VERSION = 42;
const int TAG_IMAGE_WIDTH =                0x100;
const int TAG_IMAGE_HEIGHT =               0x101;
const int TAG_BITS_PER_SAMPLE =            0x102;
const int TAG_COMPRESSION =                0x103;
const int TAG_PHOTOMETRIC_INTERPRETATION = 0x106;
const int TAG_STRIP_OFFSETS =              0x111;
const int TAG_SAMPLES_PER_PIXEL =          0x115;
const int TAG_ROWS_PER_STRIP =             0x116;
const int TAG_STRIP_BYTE_COUNTS =          0x117;
const int TAG_PLANAR_CONFIGURATION =       0x11C;

const uint16_t NO_COMPRESSION = 1;
const uint16_t PLANAR_CONFIGURATION_CHUNKY = 1;
const uint16_t INTEL_BYTE_ORDER = ((uint16_t)'I' << 8) + 'I'; // little-endian
const uint16_t MOTOROLA_BYTE_ORDER = ((uint16_t)'M' << 8) + 'M'; // big-endian
const int PHMET_WHITE_IS_ZERO = 0;
const int PHMET_BLACK_IS_ZERO = 1;
const int PHMET_RGB = 2;

inline unsigned GetFieldTypeLength(TagType_t ttt)
{
    switch (ttt)
    {
    case ttByte: return 1; break;
    case ttAscii: return 1; break;
    case ttWord: return 2; break;
    case ttDWord: return 4; break;
    case ttRational: return 8; break;
    }
}

/// Conditionally swaps a 32-bit value
uint32_t inline SWAP32cnd(uint32_t x, bool swap)
{
    if (swap) return (x << 24) | ((x & 0x00FF0000) >> 8) | ((x & 0x0000FF00) << 8) | (x >> 24);
    else return x;
}

/// Conditionally swaps two lower bytes of a 32-bit value
uint32_t inline SWAP16in32cnd(uint32_t x, bool swap)
{
    if (swap) return ((x & 0xFF) << 8) | (x >> 8);
    else return x;
}

uint16_t inline SWAP16cnd(uint16_t x, bool swap)
{
    if (swap) return (x << 8) | (x >> 8);
    else return x;
}

/// Changes endianess of 16-bit words in the specified buffer
void SwapBufferWords(uint16_t *buf, int numWords)
{
    for (unsigned i = 0; i < numWords; i++)
        buf[i] = (buf[i] << 8) | (buf[i] >> 8);
}

/// Reverses values of an 8-bit grayscale buffer
void NegateGrayscale8(uint8_t *buf, int length)
{
    for (int i = 0; i < length; i++)
        buf[i] = 0xFF - buf[i];
}

/// Reverses values of a 16-bit grayscale buffer
void NegateGrayscale16(uint16_t *buf, int length)
{
    for (int i = 0; i < length; i++)
        buf[i] = 0xFFFF - buf[i];
}

bool SaveTiff(const char *fileName, ///< Output file name
              void *pixels,   ///< Pointer to the buffer with pixel data (left to right, top to bottom, no padding)
              PixelFormat_t pixFmt, ///< Pixel format of 'pixels'
              int imgWidth,
              int imgHeight
)
{
    if (pixFmt != PIX_MONO8 && pixFmt != PIX_MONO16 && pixFmt != PIX_RGB24 && pixFmt != PIX_RGB48)
        throw std::runtime_error("SaveTiff(): only grayscale and RGB, 8- and 16-bit formats are supported.");

    std::ofstream file(fileName, std::ios_base::trunc | std::ios_base::binary);

    if (file.fail())
        return false;

    TiffHeader_t tiffHeader;
    tiffHeader.id = INTEL_BYTE_ORDER;
    tiffHeader.version = TIFF_VERSION;
    tiffHeader.dirOffset = sizeof(tiffHeader);
    file.write((const char *)&tiffHeader, sizeof(tiffHeader));

    uint16_t numDirEntries = 10;
    file.write((const char *)&numDirEntries, sizeof(numDirEntries));

    uint32_t nextDirOffset = 0;

    TiffField_t field;

    field.tag = TAG_IMAGE_WIDTH;
    field.type = ttWord;
    field.count = 1;
    field.value = imgWidth;
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_IMAGE_HEIGHT;
    field.type = ttWord;
    field.count = 1;
    field.value = imgHeight;
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_BITS_PER_SAMPLE;
    field.type = ttWord;
    field.count = 1;
    switch (pixFmt)
    {
    case PIX_MONO8:
    case PIX_RGB24:
        field.value = 8; break;
    case PIX_MONO16:
    case PIX_RGB48:
        field.value = 16; break;
    }
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_COMPRESSION;
    field.type = ttWord;
    field.count = 1;
    field.value = NO_COMPRESSION;
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_PHOTOMETRIC_INTERPRETATION;
    field.type = ttWord;
    field.count = 1;
    switch (pixFmt)
    {
    case PIX_MONO8:
    case PIX_MONO16:
        field.value = PHMET_BLACK_IS_ZERO; break;
    case PIX_RGB24:
    case PIX_RGB48:
        field.value = PHMET_RGB; break;
    }
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_STRIP_OFFSETS;
    field.type = ttDWord;
    field.count = 1;
    // we write the header, num. of directory entries, 10 fields and a next directory offset (==0); pixel data starts next
    field.value = sizeof(tiffHeader) + sizeof(numDirEntries) + 10*sizeof(field) + sizeof(nextDirOffset);
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_SAMPLES_PER_PIXEL;
    field.type = ttWord;
    field.count = 1;
    switch (pixFmt)
    {
    case PIX_MONO8:
    case PIX_MONO16:
        field.value = 1; break;
    case PIX_RGB24:
    case PIX_RGB48:
        field.value = 3; break;
    }
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_ROWS_PER_STRIP;
    field.type = ttWord;
    field.count = 1;
    field.value = imgHeight; // there is only one strip for the whole image
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_STRIP_BYTE_COUNTS;
    field.type = ttDWord;
    field.count = 1;
    field.value = imgWidth * imgHeight * GetBytesPerPixel(pixFmt); // there is only one strip for the whole image
    file.write((const char *)&field, sizeof(field));

    field.tag = TAG_PLANAR_CONFIGURATION;
    field.type = ttWord;
    field.count = 1;
    field.value = PLANAR_CONFIGURATION_CHUNKY; // there is only one strip for the whole image
    file.write((const char *)&field, sizeof(field));

    // write the next directory offset (0 = no other directories)
    file.write((const char *)&nextDirOffset, sizeof(nextDirOffset));

    file.write((const char *)pixels, imgWidth * imgHeight * GetBytesPerPixel(pixFmt));

    file.close();
    return true;
}

/// Returns newly allocated buffer with contents of the specified TIFF file (returns 0 on error)
void *ReadTiff(const char *fileName, ///< Input file name
              PixelFormat_t &pixFmt, ///< Receives the pixel format
              int &imgWidth, ///< Receives image width
              int &imgHeight, ///< Receives image height
              std::string *errorMsg ///< If not null, receives error message (if any)
)
{
    std::ifstream file(fileName, std::ios_base::binary);

    if (file.fail())
        return 0;

    TiffHeader_t tiffHeader;
    file.read((char *)&tiffHeader, sizeof(tiffHeader));
    if (file.gcount() != sizeof(tiffHeader))
    {
        if (errorMsg) *errorMsg = "File header is incomplete.";
        return 0;
    }

    bool isBE = tiffHeader.id == MOTOROLA_BYTE_ORDER; // true if the file has big endian data

    if (SWAP16cnd(tiffHeader.version, isBE) != TIFF_VERSION)
    {
        if (errorMsg) *errorMsg = "Unknown TIFF version.";
        return 0;
    }

    // Seek to the first TIFF directory
    file.seekg(SWAP32cnd(tiffHeader.dirOffset, isBE), std::ios_base::beg);

    uint16_t numDirEntries;
    file.read((char *)&numDirEntries, sizeof(numDirEntries));
    numDirEntries = SWAP16cnd(numDirEntries, isBE);
    if (file.gcount() != sizeof(numDirEntries))
    {
        if (errorMsg) *errorMsg = "The number of TIFF directory entries tag is incomplete.";
        return 0;
    }

    unsigned numStrips = 0;
    unsigned bitsPerSample = 0;
    unsigned *stripOffsets = 0;
    unsigned *stripByteCounts = 0;
    unsigned rowsPerStrip = 0;
    int photometricInterpretation = -1;
    int samplesPerPixel = 0;

    std::fstream::pos_type nextFieldPos = file.tellg();
    for (unsigned i = 0; i < numDirEntries; i++)
    {
        TiffField_t tiffField;

        file.seekg(nextFieldPos, std::ios_base::beg);
        file.read((char *)&tiffField, sizeof(tiffField));
        if (file.gcount() != sizeof(tiffField))
        {
            if (errorMsg) *errorMsg = "TIFF field is incomplete.";
            return 0;
        }
        nextFieldPos = file.tellg();

        tiffField.tag = SWAP16cnd(tiffField.tag, isBE);
        tiffField.type = SWAP16cnd(tiffField.type, isBE);
        tiffField.count = SWAP32cnd(tiffField.count, isBE);
        if (tiffField.count > 1 || tiffField.type == ttDWord)
            tiffField.value = SWAP32cnd(tiffField.value, isBE);
        else if (tiffField.count == 1 && tiffField.type == ttWord)
            tiffField.value = SWAP16in32cnd(tiffField.value, isBE);

        switch (tiffField.tag)
        {
        case TAG_IMAGE_WIDTH: imgWidth = tiffField.value; break;

        case TAG_IMAGE_HEIGHT: imgHeight = tiffField.value; break;

        case TAG_BITS_PER_SAMPLE:
            if (tiffField.count == 1)
                bitsPerSample = tiffField.value;
            else
            {
                // Some files may have as many "bits per sample" values specified
                // as there are channels. Make sure they are all the same.

                file.seekg(tiffField.value, std::ios_base::beg);

                uint16_t *fieldBuf = new uint16_t[tiffField.count];
                file.read((char *)fieldBuf, tiffField.count * sizeof(uint16_t));

                bool allEqual = true;
                uint16_t first = fieldBuf[0];
                for (unsigned j = 1; j < tiffField.count; j++)
                    if (fieldBuf[j] != first)
                    {
                        allEqual = false;
                        break;
                    }

                 if (!allEqual)
                 {
                    if (errorMsg) *errorMsg = "Files with differing bit depts per channel are not supported.";
                    return 0;
                 }

                 bitsPerSample = SWAP16cnd(first, isBE);
            }

            if (bitsPerSample != 8 && bitsPerSample != 16)
            {
                if (errorMsg) *errorMsg = "Only 8 and 16 bits per channel files are supported.";
                return 0;
            }
            break;

        case TAG_COMPRESSION:
            if (tiffField.value != NO_COMPRESSION)
            {
                if (errorMsg) *errorMsg = "Compression is not supported.";
                return 0;
            }
            break;

        case TAG_PHOTOMETRIC_INTERPRETATION: photometricInterpretation = tiffField.value; break;

        case TAG_STRIP_OFFSETS:
            numStrips = tiffField.count;
            stripOffsets = new unsigned[numStrips];
            if (numStrips == 1)
                stripOffsets[0] = tiffField.value;
            else
            {
                file.seekg(tiffField.value, std::ios_base::beg);
                for (unsigned i = 0; i < numStrips; i++)
                {
                    file.read((char *)&stripOffsets[i], sizeof(stripOffsets[i]));
                    stripOffsets[i] = SWAP32cnd(stripOffsets[i], isBE);
                }
            }
            break;

        case TAG_SAMPLES_PER_PIXEL: samplesPerPixel = tiffField.value; break;

        case TAG_ROWS_PER_STRIP: rowsPerStrip = tiffField.value; break;

        case TAG_STRIP_BYTE_COUNTS:
            stripByteCounts = new unsigned[tiffField.count];
            if (tiffField.count == 1)
                stripByteCounts[0] = tiffField.value;
            else
            {
                file.seekg(tiffField.value, std::ios_base::beg);
                for (unsigned i = 0; i < tiffField.count; i++)
                {
                    file.read((char *)&stripByteCounts[i], sizeof(stripByteCounts[i]));
                    stripByteCounts[i] = SWAP32cnd(stripByteCounts[i], isBE);
                }
            }
            break;

        case TAG_PLANAR_CONFIGURATION:
            if (tiffField.value != PLANAR_CONFIGURATION_CHUNKY)
            {
                if (errorMsg) *errorMsg = "Files with planar configuration other than packed (chunky) are not supported.";
                return 0;
            }
            break;
        }
    }

    if (rowsPerStrip == 0 && numStrips == 1)
        // If there is only 1 strip, it contains all the rows
        rowsPerStrip = imgHeight;

    // Validate the values

    if (samplesPerPixel == 1 && photometricInterpretation != PHMET_BLACK_IS_ZERO && photometricInterpretation != PHMET_WHITE_IS_ZERO ||
        samplesPerPixel == 3 && photometricInterpretation != PHMET_RGB ||
        samplesPerPixel != 1 && samplesPerPixel != 3)
    {
        if (errorMsg) *errorMsg = "Only RGB and grayscale images are supported.";
        return 0;
    }

    if (samplesPerPixel == 1)
    {
        if (bitsPerSample == 8)
            pixFmt = PIX_MONO8;
        else if (bitsPerSample == 16)
            pixFmt = PIX_MONO16;
    }
    else if (samplesPerPixel == 3)
    {
        if (bitsPerSample == 8)
            pixFmt = PIX_RGB24;
        else if (bitsPerSample == 16)
            pixFmt = PIX_RGB48;
    }

    // Buffer with all image pixel values, left to right, top to bottom, without any padding
    void *pixels = malloc(imgWidth * imgHeight * GetBytesPerPixel(pixFmt));

    int bufOfs = 0;
    for (unsigned i = 0; i < numStrips; i++)
    {
        file.seekg(stripOffsets[i], std::ios_base::beg);
        file.read((char *)pixels + bufOfs, stripByteCounts[i]);
        bufOfs += stripByteCounts[i];
        if (file.gcount() != stripByteCounts[i])
        {
            if (errorMsg) *errorMsg = boost::str(boost::format("The file is incomplete: pixel data in strip %d is too short. Expected %d bytes, but read only %d.") % i % stripByteCounts[i] % file.gcount());
            free(pixels);
            return 0;
        }
    }

    if ((pixFmt == PIX_MONO16 || pixFmt == PIX_RGB48) && isBE)
        SwapBufferWords((uint16_t *)pixels, imgWidth*imgHeight*GetBytesPerPixel(pixFmt)/2);

    if (photometricInterpretation == PHMET_WHITE_IS_ZERO)
    {
        // Reverse the values so that "black" is zero, "white" is 255 or 65535.
        if (pixFmt == PIX_MONO8)
            NegateGrayscale8((uint8_t *)pixels, imgWidth*imgHeight);
        else if (pixFmt == PIX_MONO16)
            NegateGrayscale16((uint16_t *)pixels, imgWidth*imgHeight);
    }

    file.close();

    return pixels;
}

bool GetTiffDimensions(const char *fileName, unsigned &imgWidth, unsigned &imgHeight)
{
    std::ifstream file(fileName, std::ios_base::binary);

    if (file.fail())
        return false;

    TiffHeader_t tiffHeader;
    file.read((char *)&tiffHeader, sizeof(tiffHeader));
    if (file.gcount() != sizeof(tiffHeader))
        return false;

    bool isBE = tiffHeader.id == MOTOROLA_BYTE_ORDER; // true if the file has big endian data

    if (SWAP16cnd(tiffHeader.version, isBE) != TIFF_VERSION)
        return false;

    // Seek to the first TIFF directory
    file.seekg(SWAP32cnd(tiffHeader.dirOffset, isBE), std::ios_base::beg);

    uint16_t numDirEntries;
    file.read((char *)&numDirEntries, sizeof(numDirEntries));
    numDirEntries = SWAP16cnd(numDirEntries, isBE);
    if (file.gcount() != sizeof(numDirEntries))
        return false;

    imgWidth = imgHeight = -1;

    std::fstream::pos_type nextFieldPos = file.tellg();
    for (unsigned i = 0; i < numDirEntries; i++)
    {
        TiffField_t tiffField;

        file.seekg(nextFieldPos, std::ios_base::beg);
        file.read((char *)&tiffField, sizeof(tiffField));
        if (file.gcount() != sizeof(tiffField))
            return false;
        nextFieldPos = file.tellg();

        tiffField.tag = SWAP16cnd(tiffField.tag, isBE);
        tiffField.type = SWAP16cnd(tiffField.type, isBE);
        tiffField.count = SWAP32cnd(tiffField.count, isBE);
        if (tiffField.count > 1 || tiffField.type == ttDWord)
            tiffField.value = SWAP32cnd(tiffField.value, isBE);
        else if (tiffField.count == 1 && tiffField.type == ttWord)
            tiffField.value = SWAP16in32cnd(tiffField.value, isBE);

        switch (tiffField.tag)
        {
        case TAG_IMAGE_WIDTH: imgWidth = tiffField.value; break;
        case TAG_IMAGE_HEIGHT: imgHeight = tiffField.value; break;
        }

        if (imgWidth != -1 && imgHeight != -1)
            break;
    }

    return true;
}

} // namespace TIFF

/// Converts data in input buffer to the specified pixel format and writes it to the destination buffer; if formats are the same, does nothing
void ConvertPixelFormat(
    void *srcBuf,             ///< Source (input) buffer (pixels stored left to right, top to bottom, no padding)
    void *destBuf,            ///< Destination (output) buffer
    int width,                ///< Image width (number of columns in the buffers)
    int height,               ///< Image height (number of rows in the buffers)
    PixelFormat_t srcPixFmt,  ///< Pixel format in 'srcBuf'
    PixelFormat_t destPixFmt, ///< Desired pixel format in 'destBuf'; PIX_PAL8 is not supported
    uint8_t palette[]         ///< Pointer to 256-element RGB(+1) palette (256*4 bytes); required if 'srcPixFmt' or 'destPixFmt' is PIX_PAL8
)
{
    if (srcPixFmt == PIX_UNCHANGED || destPixFmt == PIX_UNCHANGED)
        throw std::runtime_error("ConvertPixelFormat(): specifying PIX_UNCHANGED is not allowed.");
    if (destPixFmt == PIX_PAL8 && srcPixFmt != PIX_PAL8)
        throw std::runtime_error("ConvertPixelFormat(): cannot convert to PIX_PAL8");
    if (srcPixFmt == PIX_PAL8 && !palette)
        throw std::runtime_error("ConvertPixelFormat(): palette required when converting from PIX_PAL8");

    if (srcPixFmt == destPixFmt)
        return;

    uint8_t *inpPtr = (uint8_t*)srcBuf,
            *outPtr = (uint8_t*)destBuf;

    int inpPtrStep = GetBytesPerPixel(srcPixFmt),
        outPtrStep = GetBytesPerPixel(destPixFmt);

    for (int i = 0; i < width*height; i++)
    {
        if (srcPixFmt == PIX_MONO8)
        {
            uint8_t src = *inpPtr;
            switch (destPixFmt)
            {
            case PIX_MONO16: *(uint16_t *)outPtr = (uint16_t)src << 8; break;
            case PIX_MONO32F: *(float *)outPtr = src * 1.0f/0xFF; break;
            case PIX_RGB24: outPtr[0] = outPtr[1] = outPtr[2] = src; break;
            case PIX_RGB48:
                ((uint16_t *)outPtr)[0] =
                    ((uint16_t *)outPtr)[1] =
                    ((uint16_t *)outPtr)[2] =  (uint16_t)src << 8;
                break;
            }
        }
        else if (srcPixFmt == PIX_MONO16)
        {
            uint16_t src = *(uint16_t *)inpPtr;
            switch (destPixFmt)
            {
            case PIX_MONO8: *outPtr = (uint8_t)(src >> 8); break;
            case PIX_MONO32F: *(float *)outPtr = src * 1.0f/0xFFFF; break;
            case PIX_RGB24: outPtr[0] = outPtr[1] = outPtr[2] = (uint8_t)(src >> 8); break;
            case PIX_RGB48:
                ((uint16_t *)outPtr)[0] =
                    ((uint16_t *)outPtr)[1] =
                    ((uint16_t *)outPtr)[2] = src;
                break;
            }
        }
        else if (srcPixFmt == PIX_MONO32F)
        {
            float src = *(float *)inpPtr;
            switch (destPixFmt)
            {
            case PIX_MONO8: *outPtr = (uint8_t)(src * 0xFF); break;
            case PIX_MONO16: *(uint16_t *)outPtr = (uint16_t)(src * 0xFFFF); break;
            case PIX_RGB24: outPtr[0] = outPtr[1] = outPtr[2] = (uint8_t)(src * 0xFF); break;
            case PIX_RGB48:
                ((uint16_t *)outPtr)[0] =
                    ((uint16_t *)outPtr)[1] =
                    ((uint16_t *)outPtr)[2] = (uint16_t)(src * 0xFFFF); break;
            }
        }
        // When converting from a color format to mono, use sum (scaled) of all channels as the pixel brightness.
        else if (srcPixFmt == PIX_PAL8)
        {
            uint8_t src = *inpPtr;
            switch (destPixFmt)
            {
            case PIX_MONO8: *outPtr = (uint8_t)(((int)palette[4*src] + palette[4*src+1] + palette[4*src+2])/3); break;
            case PIX_MONO16: *(uint16_t *)outPtr = ((uint16_t)palette[4*src] + palette[4*src+1] + palette[4*src+2])/3; break;
            case PIX_MONO32F: *(float *)outPtr = ((int)palette[4*src] + palette[4*src+1] + palette[4*src+2]) * 1.0f/(3*0xFF); break;
            case PIX_RGB24:
                outPtr[0] = palette[4*src];
                outPtr[1] = palette[4*src+1];
                outPtr[2] = palette[4*src+2];
                break;
            case PIX_RGB48:
                ((uint16_t *)outPtr)[0] = (uint16_t)palette[4*src] << 8;
                ((uint16_t *)outPtr)[1] = (uint16_t)palette[4*src+1] << 8;
                ((uint16_t *)outPtr)[2] = (uint16_t)palette[4*src+2] << 8;
                break;
            }
        }
        else if (srcPixFmt == PIX_RGB24)
        {
            switch (destPixFmt)
            {
            case PIX_MONO8: *outPtr = (uint8_t)(((int)inpPtr[0] + inpPtr[1] + inpPtr[2])/3); break;
            case PIX_MONO16: *(uint16_t *)outPtr = ((uint16_t)inpPtr[0] + inpPtr[1] + inpPtr[2])/3; break;
            case PIX_MONO32F: *(float *)outPtr = ((int)inpPtr[0] + inpPtr[1] + inpPtr[2]) * 1.0f/(3*0xFF); break;
            case PIX_RGB48:
                ((uint16_t *)outPtr)[0] = (uint16_t)inpPtr[0] << 8;
                ((uint16_t *)outPtr)[1] = (uint16_t)inpPtr[1] << 8;
                ((uint16_t *)outPtr)[2] = (uint16_t)inpPtr[2] << 8;
                break;
            }
        }
        else if (srcPixFmt == PIX_RGB48)
        {
            uint16_t *inpPtr16 = (uint16_t *)inpPtr;
            switch (destPixFmt)
            {
            case PIX_MONO8: *outPtr = (uint8_t)(((int)inpPtr16[0] + inpPtr16[1] + inpPtr16[2])/3); break;
            case PIX_MONO16: *(uint16_t *)outPtr = (uint16_t)(((int)inpPtr16[0] + inpPtr16[1] + inpPtr16[2])/3); break;
            case PIX_MONO32F: *(float *)outPtr = ((int)inpPtr16[0] + inpPtr16[1] + inpPtr16[2]) * 1.0f/(3*0xFFFF); break;
            case PIX_RGB24:
                outPtr[0] = (uint8_t)(inpPtr16[0] >> 8);
                outPtr[1] = (uint8_t)(inpPtr16[1] >> 8);
                outPtr[2] = (uint8_t)(inpPtr16[2] >> 8);
                break;
            }
        }

        inpPtr += inpPtrStep;
        outPtr += outPtrStep;
    }
}

/// Reads an image and returns pointer to the newly allocated buffer with pixel contents or 0 on error
void *ReadImageFile(std::string fileName,
        PixelFormat_t destPixFmt, ///< Desired pixel format of data in the returned buffer
        int &imgWidth,  ///< Receives image width
        int &imgHeight, ///< Receives image height
        PixelFormat_t *receivedPixFmt, ///< If not null and destPixFmt==PIX_UNCHANGED, receives the pixel format of the source image
        uint8_t palette[],     ///< If not null and reading from an 8-bit file with palette, receives the palette (1024 bytes)
        std::string *errorMsg  ///< If not null, receives error message (if any)
)
{
    std::string ext = fileName.substr(fileName.find_last_of('.'));
    std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);

    void *pixels = 0;

    PixelFormat_t dummy; // Used when the caller passed null as 'receivedPixFmt'
    if (receivedPixFmt == 0) receivedPixFmt = &dummy;

    uint8_t localPalette[256*4];
    uint8_t *palPtr = (palette != 0 ? palette : localPalette);

    if (ext == ".bmp")
        pixels = BMP::ReadBmp(fileName.c_str(), imgWidth, imgHeight, *receivedPixFmt, palPtr);
    else if (ext == ".tif" || ext == ".tiff")
        pixels = TIFF::ReadTiff(fileName.c_str(), *receivedPixFmt, imgWidth, imgHeight, errorMsg);
    else
        return 0;

    if (pixels && destPixFmt != PIX_UNCHANGED && destPixFmt != *receivedPixFmt)
    {
        void *convertedPixels = malloc(imgWidth * imgHeight * GetBytesPerPixel(destPixFmt));
        ConvertPixelFormat(pixels, convertedPixels, imgWidth, imgHeight, *receivedPixFmt, destPixFmt, palPtr);
        free(pixels);
        return convertedPixels;
    }
    else
        return pixels;
}

bool GetImageDimensions(std::string fileName, unsigned &imgWidth, unsigned &imgHeight)
{
    std::string ext = fileName.substr(fileName.find_last_of('.'));
    std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);

    if (ext == ".bmp")
        return BMP::GetBmpDimensions(fileName.c_str(), imgWidth, imgHeight);
    else if (ext == ".tif" || ext == ".tiff")
        return TIFF::GetTiffDimensions(fileName.c_str(), imgWidth, imgHeight);
    else
        return false;
}

/// Saves image; returns 'false' on error
bool SaveImageFile(std::string fileName, ///< Output file name
        int imgWidth,              ///< Image width
        int imgHeight,             ///< Image height
        PixelFormat_t pixFmt,      ///< Pixel format
        void *pixels,              ///< Pixel contents in 'pixFmt' format
        uint8_t palette[]          ///< Points to the palette (1024 bytes) to be saved if pixFmt is PIX_PAL8;
)
{
    std::string ext = fileName.substr(fileName.find_last_of('.'));
    std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
    if (ext == ".bmp")
        return BMP::SaveBmp(fileName.c_str(), imgWidth, imgHeight, pixFmt, pixels, palette);
    else if (ext == ".tif" || ext == ".tiff")
        return TIFF::SaveTiff(fileName.c_str(), pixels, pixFmt, imgWidth, imgHeight);
    else
        return false;
}

inline float ClampLuminance(float val, float maxVal)
{
    if (val < 0.0f)
        return 0.0f;
    else if (val > maxVal)
        return maxVal;
    else
        return val;
}

/// Cubic (Hermite) interpolation of 4 subsequent values fm1, f0, f1, f2 at location 0<=t<=1 between the middle elements (f0 and f1)
template<typename T>
inline float InterpolateCubic(float t, T fm1, T f0, T f1, T f2)
{
    float delta_k = (float)f1 - f0;
    float dk = ((float)f1 - fm1)*0.5f, dk1 = ((float)f2 - f0)*0.5f;

    float a0 = f0, a1 = dk, a2 = 3.0f*delta_k - 2.0f*dk - dk1,
        a3 = (float)dk + dk1 - 2.0f*delta_k;

    return t*(t*(a3*t + a2)+a1)+a0;
}

/// Performs sub-pixel image translation using cubic interpolation
template<typename Lum_t>
void SubpixelTranslationImpl(
        Lum_t *src,  ///< Source image
        Lum_t *dest, ///< Destination image
        int width,   ///< Image width
        int height,  ///< Image height
        int numChannels, ///< Number of channels (number of 'Lum_t' samples per pixel)
        float maxLum, ///< Max value to clamp the output values to
        float dx,   ///< Translation in X, |dx| < 1
        float dy    ///< Translation in Y, |dy| < 1
)
{
    if (fabs(dx) >= 1.0 || fabs(dy) >= 1.0)
        throw std::runtime_error("SubpixelTranslation(): the translation must be by less than 1 pixel in each direction.");

    int idx = dx < 0.0f ? 1 : -1;
    int idy = dy < 0.0f ? 1 : -1;

    dx = fabs(dx);
    dy = fabs(dy);

    // Skip 2-pixels borders on each side of the image
    #pragma omp parallel for
    for (int row = 2; row < height-2; row++)
    {
        for (int col = 2; col < width-2; col++)
        {
            for (int ch = 0; ch < numChannels; ch++)
            {
                float yvals[4];

                // Perform 4 interpolations at 4 adjacent rows, using X offsets -1, 0, 1, 2 (*idx)
                int y = row - idy;
                for (int relY = -1; relY <= 2; relY++)
                {
                    yvals[relY+1] = InterpolateCubic(dx,
                                     src[(col-idx     + y*width)*numChannels + ch],
                                     src[(col         + y*width)*numChannels + ch],
                                     src[(col+idx     + y*width)*numChannels + ch],
                                     src[(col+idx+idx + y*width)*numChannels + ch]);
                    y += idy;
                }

                // Perform the final vertical (column) interpolation of the 4 horizontal (row) values interpolated previously
                dest[(col + row*width)*numChannels + ch] = (Lum_t)ClampLuminance(InterpolateCubic(dy, yvals[0], yvals[1], yvals[2], yvals[3]), maxLum);
            }
        }
    }

    // Copy the 2-pixel borders without changes

    // 2 top rows
    memcpy(dest + (0 + 0*width) * numChannels, src + (0 + 0*width) * numChannels, width * numChannels * sizeof(Lum_t));
    memcpy(dest + (0 + 1*width) * numChannels, src + (0 + 1*width) * numChannels, width * numChannels * sizeof(Lum_t));
    // 2 bottom rows
    memcpy(dest + (0 + (height-1)*width) * numChannels, src + (0 + (height-1)*width) * numChannels, width * numChannels * sizeof(Lum_t));
    memcpy(dest + (0 + (height-2)*width) * numChannels, src + (0 + (height-2)*width) * numChannels, width * numChannels * sizeof(Lum_t));
    // 2 leftmost and 2 rightmost columns
    for (int row = 0; row < height; row++)
    {
        // 2 leftmost columns
        memcpy(dest + (0 + row*width) * numChannels, src + (0 + row*width) * numChannels,
                2  *numChannels * sizeof(Lum_t)); // copying 2 pixels, each is 'numChannels' elements
        // 2 rightmost columns
        memcpy(dest + (width-2 + row*width) * numChannels, src + (width-2 + row*width) * numChannels,
                2 * numChannels * sizeof(Lum_t)); // copying 2 pixels, each is 'numChannels' elements
    }
}

/// Performs a sub-pixel translation of an image using bicubic interpolation; PIX_PAL8 pixel format is not supported
void SubpixelTranslation(
    void *src,  ///< Source image
    void *dest, ///< Destination image
    int width,  ///< Image width
    int height, ///< Image height
    PixelFormat_t pixFmt, ///< Pixel format; PIX_PAL8 is not supported
    float dx,   ///< Translation in X, |dx| < 1
    float dy    ///< Translation in Y, |dy| < 1
)
{
    if (pixFmt == PIX_PAL8)
        throw std::runtime_error("SubpixelTranslation(): PIX_PAL8 is not supported.");

    switch (pixFmt)
    {
    case PIX_MONO8:   SubpixelTranslationImpl<uint8_t> ( (uint8_t *)src,  (uint8_t *)dest, width, height, 1,   (float)0xFF, dx, dy); break;
    case PIX_MONO16:  SubpixelTranslationImpl<uint16_t>((uint16_t *)src, (uint16_t *)dest, width, height, 1, (float)0xFFFF, dx, dy); break;
    case PIX_MONO32F: SubpixelTranslationImpl<float>   (   (float *)src,    (float *)dest, width, height, 1,          1.0f, dx, dy); break;
    case PIX_RGB24:   SubpixelTranslationImpl<uint8_t> ( (uint8_t *)src,  (uint8_t *)dest, width, height, 3,   (float)0xFF, dx, dy); break;
    case PIX_RGB48:   SubpixelTranslationImpl<uint16_t>((uint16_t *)src, (uint16_t *)dest, width, height, 3, (float)0xFFFF, dx, dy); break;
    }
}


#define BOOST_TEST_MODULE "test_3spn2_bond_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/3SPN2/ThreeSPN2BondPotential.hpp>

BOOST_AUTO_TEST_CASE(potential_3spn2_bond_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;

    const real_type k  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::ThreeSPN2BondPotential<real_type> potential(k, r0);

    const real_type x_min = 0.5 * r0;
    const real_type x_max = 1.5 * r0;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(potential_3spn2_bond_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;
    constexpr real_type tol = 1e-3f;

    const real_type k  = 1.0f;
    const real_type r0 = 5.0f;

    mjolnir::ThreeSPN2BondPotential<real_type> potential(k, r0);

    const real_type x_min = 0.5 * r0;
    const real_type x_max = 1.5 * r0;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);
}


/*
2014, the dachshund authors.
*/

#include <cmath>
#include <cstdio>

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

#if defined(_OPENMP)
#include <omp.h>
#endif

#include "timer.h"
#include "linalg.h"

#include "map.h"

void smp_product(const int num_pixel_points, const Point* const pixel_coords,
    const int num_map_points, const Point* const map_coords,
    const SignalCovarParams* const s_params,
    const double* const x, double* const m) {
  int i, j;
#if defined(_OPENMP)
  #pragma omp parallel for private(i, j)
#endif
  for (i = 0; i < num_map_points; ++i) {
    m[i] = 0.0;
    const double x_i = map_coords[i].x;
    const double y_i = map_coords[i].y;
    const double z_i = map_coords[i].z;
    for (j = 0; j < num_pixel_points; ++j) {
      const double dx = x_i - pixel_coords[j].x;
      const double dy = y_i - pixel_coords[j].y;
      const double dz = z_i - pixel_coords[j].z;
      const double x_perp_2 = (dx*dx + dy*dy);
      const double x_para_2 = dz*dz;
      const double S_ij = signal_covar(x_perp_2, x_para_2, s_params);
      m[i] += S_ij * x[j];
    }
  }
}


#pragma once

#include <cmath>
#include <iostream>
#include <Eigen/Core>
#include <string>

class GridFilter
{
public:
    GridFilter(const Eigen::MatrixXd & mat, double xStep, double yStep,
               int initDeviation = 1, double gradInfl = 1.0, double allowErr = 0.0)
        : m_ParaboloidMatrix(mat), m_InitialDeviation(initDeviation), m_GradientInfluence(gradInfl), m_AllowableError(allowErr) {
            m_PMrows = mat.rows();
            m_PMcols = mat.cols();
            ComputeGradient(xStep, yStep);
            m_CrossSectionModified = Eigen::MatrixXd::Zero(m_PMrows, m_PMcols);
        }

    GridFilter(int rows, int columns, double* mat, double xStep, double yStep,
               int initDeviation = 1, double gradInfl = 1.0, double allowErr = 0.0)
        : m_ParaboloidMatrix(Eigen::Map<Eigen::MatrixXd>(mat, rows, columns)),
        m_InitialDeviation(initDeviation), m_GradientInfluence(gradInfl), m_AllowableError(allowErr) {
            m_PMrows = rows;
            m_PMcols = columns;
            ComputeGradient(xStep, yStep);
            m_CrossSectionModified = Eigen::MatrixXd::Zero(m_PMrows, m_PMcols);
        }

    void GetCrossSectionOriginal(Eigen::MatrixXd & CSOmatTarget, double value, bool isCScomputed = false);

    void GetCrossSectionExtended(Eigen::MatrixXd & CSEmatTarget,
                                 double value, int deviation, bool isCScomputed = false, bool isDevConst = true);

    void GetCrossSectionExtendedAutoDev(Eigen::MatrixXd& CSEADmatTarget, double value);

    void GetCrossSectionExtendedIrregular(Eigen::MatrixXd& CSEImatTarget, double value);

    /**
     * Getter for GridFilter#InterestingPoints matrix
     * \warning Should be computed at least once by addPoints() function before getting
     * \return SpectrumCrossSection#InterestingPoints matrix
     */
    inline void GetInterestingPoints(Eigen::MatrixXd & IPTarget)   {  IPTarget = m_InterestingPoints; }

    /**
     * Getter for GridFilter#CrossSectionModified matrix
     * \warning Should be computed at least once by addPoints() function before getting
     * \return GridFilter#CrossSectionModified matrix
     */
    inline void GetModifiedCrossSection(Eigen::MatrixXd & CSMTarget) { CSMTarget = m_CrossSectionModified; }

protected:

    void ComputeGradient(double xStep, double yStep);
    void addPoints(int deviation, bool isDevConst = true);
    void ComputeCrossSectionOriginal(double value);
    int ComputeCurrentDeviation();
    void makeCorridor(int curr_x, int curr_y, int deviation);
    void ComputeAbsGradMatrix();

    Eigen::MatrixXd m_ParaboloidMatrix; //!< Full values matrix (2D and unknown size NxM)
    Eigen::MatrixXd m_CrossSecOriginal; //!< Cross-section z=value, is not set at initial moment, can be recomputed
    Eigen::MatrixXd m_CrossSectionModified;
    Eigen::MatrixXd  m_AbsGrad;
    Eigen::MatrixXd m_dxPM,         //!< x-component of gradient for ParaboloidMatrix size of [NxM-1]
                    m_dyPM;         //!< y-omponents of gradient for ParaboloidMatrix size od [N-1xM]
    Eigen::Matrix2Xd m_InterestingPoints;   //!< Found points
    int m_InitialDeviation;         //!< Multiplier for deviation value, can be set at constructor, default is 1
    double m_GradientInfluence;     //!< Multiplier for gradient component of deviation value, default is 1.0
    double m_AllowableError;        //!< In cross-section z = value finding there is z = value+-AllowableError is found in fact
    int m_PMcols,                   //!< The number of columns of ParaboloidMatrix, computed in constructor, can't be changed after
        m_PMrows;                   //!< The number of rows of ParaboloidMatrix, computed in constructor, can't be changed after

};


#ifndef MATHTOOLBOX_MATRIX_INVERSION_HPP
#define MATHTOOLBOX_MATRIX_INVERSION_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    Eigen::MatrixXd GetInverseUsingUpperLeftBlockInverse(const Eigen::MatrixXd& matrix,
                                                         const Eigen::MatrixXd& upper_left_block_inverse);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_MATRIX_INVERSION_HPP


#ifndef MJOLNIR_TEST_UTIL_CHECK_FORCE_HPP
#define MJOLNIR_TEST_UTIL_CHECK_FORCE_HPP
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/System.hpp>

#include <mjolnir/core/LocalInteractionBase.hpp>
#include <mjolnir/core/GlobalInteractionBase.hpp>
#include <type_traits>
#include <test/util/clear_system.hpp>

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

namespace mjolnir
{
namespace test
{

// Check if the sum of the forces is zero, only if the interaction is internal.

template<typename traitsT, typename Interaction>
void check_net_force(System<traitsT> sys,
                     const Interaction& interaction,
                     const typename traitsT::real_type tol)
{
    using real_type       = typename traitsT::real_type;
    using coordinate_type = typename traitsT::coordinate_type;
    clear_force(sys);

    sys.preprocess_forces();
    interaction.calc_force(sys);
    sys.postprocess_forces();

    coordinate_type f_tot = math::make_coordinate<coordinate_type>(0,0,0);
    for(const auto& f : sys.forces())
    {
        f_tot += f;
    }

    BOOST_TEST(mjolnir::math::X(f_tot) == real_type(0), boost::test_tools::tolerance(tol));
    BOOST_TEST(mjolnir::math::Y(f_tot) == real_type(0), boost::test_tools::tolerance(tol));
    BOOST_TEST(mjolnir::math::Z(f_tot) == real_type(0), boost::test_tools::tolerance(tol));
    return;
}

// This checks force applied to each particle and the numerical difference of
// the corresponding energy

template<typename traitsT, typename Interaction>
void check_force(const System<traitsT>& init,
                 const Interaction& interaction,
                 const typename traitsT::real_type tol,
                 const typename traitsT::real_type dr,
                 const bool zero_net_force = true) // sum of external force (e.g. wall potential) is non-zero
{
    using real_type = typename traitsT::real_type;

    for(const auto& f : init.forces())
    {
        BOOST_TEST_REQUIRE(mjolnir::math::X(f) == real_type(0));
        BOOST_TEST_REQUIRE(mjolnir::math::Y(f) == real_type(0));
        BOOST_TEST_REQUIRE(mjolnir::math::Z(f) == real_type(0));
    }
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST_REQUIRE(init.virial()[i] == real_type(0));
    }

    System<traitsT> sys(init);

    for(std::size_t idx=0; idx<sys.size(); ++idx)
    {
        {
            // ----------------------------------------------------------------
            // reset positions
            sys = init;

            // calc U(x-dx)
            const auto E0 = interaction.calc_energy(sys);

            mjolnir::math::X(sys.position(idx)) += dr;

            // calc F(x)
            sys.preprocess_forces();
            interaction.calc_force(sys);
            sys.postprocess_forces();

            mjolnir::math::X(sys.position(idx)) += dr;

            // calc U(x+dx)
            const auto E1 = interaction.calc_energy(sys);

            // central difference
            const auto dE = (E1 - E0) * 0.5;

            BOOST_TEST(-dE == dr * mjolnir::math::X(sys.force(idx)),
                       boost::test_tools::tolerance(tol));
        }
        {
            // ----------------------------------------------------------------
            // reset positions
            sys = init;

            // calc U(x-dx)
            const auto E0 = interaction.calc_energy(sys);

            mjolnir::math::Y(sys.position(idx)) += dr;

            // calc F(x)
            sys.preprocess_forces();
            interaction.calc_force(sys);
            sys.postprocess_forces();

            mjolnir::math::Y(sys.position(idx)) += dr;

            // calc U(x+dx)
            const auto E1 = interaction.calc_energy(sys);

            // central difference
            const auto dE = (E1 - E0) * 0.5;

            BOOST_TEST(-dE == dr * mjolnir::math::Y(sys.force(idx)),
                       boost::test_tools::tolerance(tol));
        }
        {
            // ----------------------------------------------------------------
            // reset positions
            sys = init;

            // calc U(x-dx)
            const auto E0 = interaction.calc_energy(sys);

            mjolnir::math::Z(sys.position(idx)) += dr;

            // calc F(x)
            sys.preprocess_forces();
            interaction.calc_force(sys);
            sys.postprocess_forces();

            mjolnir::math::Z(sys.position(idx)) += dr;

            // calc U(x+dx)
            const auto E1 = interaction.calc_energy(sys);

            // central difference
            const auto dE = (E1 - E0) * 0.5;

            BOOST_TEST(-dE == dr * mjolnir::math::Z(sys.force(idx)),
                       boost::test_tools::tolerance(tol));
        }
    }

    // check if virial is not calculated in calc_force
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST_REQUIRE(init.virial()[i] == real_type(0));
    }

    if(zero_net_force)
    {
        check_net_force(sys, interaction, tol);
    }
}

} // test
} // mjolnir
#endif// MJOLNIR_TEST_UTIL_CHECK_FORCE_HPP


/* compile_time_factorial.hpp
 *
 * Calculating factorial at compile time using recursive templates instantiation
 * Requires Boost.Multiprecision library
*/

#ifndef _COMPILE_TIME_FACTORIAL_HPP_
#define _COMPILE_TIME_FACTORIAL_HPP_

#include <type_traits>

#include <boost/multiprecision/cpp_int.hpp>

#ifdef __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING
#warning "Be careful when using large numbers!"
#warning "Add the -ftemplate-depth=N flag to increase the depth of template instantiation!"
#endif // __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING

namespace alex::utils::math
{
    namespace concepts
    {
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        template<typename T>
        concept IntegerType =
#if defined(__RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING) || defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) ||   \
    defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
            std::is_integral_v<T> || boost::multiprecision::is_number<T>::value;
#else
            std::is_integral_v<T>;
#endif
#else
    template<typename T>
    struct IsIntegerType
    {
        static constexpr bool value {
#if defined(__RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING) || defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) ||   \
    defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
            std::is_integral_v<T> || boost::multiprecision::is_number<T>::value
#else
            std::is_integral_v<T>
#endif
        };
    };
#endif
    }; // namespace alex::utils::math::concepts

#if defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) || defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
    namespace detail
    {
        template<size_t N>
        constexpr size_t cached_factorial_value {
            [] {
                if constexpr (N <= 1)       return 1;
                else if constexpr (N == 2)  return 2;
                else if constexpr (N == 3)  return 6;
                else if constexpr (N == 4)  return 24;
                else if constexpr (N == 5)  return 120;
            }()
        };
    } // namespace alex::utils::math::ct_factorial::detail
#endif

#ifdef __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    struct ct_factorial { static constexpr integer_type value{N * ct_factorial<integer_type, N - 1>::value}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type
#else
        typename integer_type
#endif
    >
    struct ct_factorial<integer_type, 0> { static constexpr integer_type value{1}; };

#elif defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    constexpr integer_type ct_factorial()
    {
        if constexpr (N <= 5) return detail::cached_factorial_value<N>;
        else return N * ct_factorial<integer_type, N - 1>();
    }

#elif defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    constexpr integer_type ct_factorial()
    {
        if constexpr (N <= 5) return detail::cached_factorial_value<N>;
        else {
            integer_type result{1};
            for (size_t i{1}; i <= N; ++i) {
                result *= i;
            }
            return result;
        }
    }

#elif defined(__SFINAE_TEMLATE_INSTANTIATION_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    struct ct_factorial : std::integral_constant<integer_type, N * ct_factorial<integer_type, N - 1>{}> {};

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type
#else
        typename integer_type
#endif
    >
    struct ct_factorial<integer_type, 0> : std::integral_constant<integer_type, 1> {};

#endif

} // namespace alex::utils::math

#endif // _COMPILE_TIME_FACTORIAL_HPP_


#pragma once

#include <LayoutEmbedding/Embedding.hh>

#include <Eigen/Dense>

namespace LayoutEmbedding {

Eigen::MatrixXd compute_vertex_repulsive_energy(const Embedding& _em);

}


/* ----------------------------------------------------------------------------

* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

* See LICENSE for the license information
* -------------------------------------------------------------------------- */

/**
 * @file    ShonanAveragingCLI.cpp
 * @brief   Run Shonan Rotation Averaging Algorithm on a file or example dataset
 * @author  Frank Dellaert
 * @date    August, 2020
 *
 * Example usage:
 *
 * Running without arguments will run on tiny 3D example pose3example-grid
 * ./ShonanAveragingCLI
 *
 * Read 2D dataset w10000 (in examples/data) and output to w10000-rotations.g2o
 * ./ShonanAveragingCLI -d 2 -n w10000 -o w10000-rotations.g2o
 *
 * Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
 * ./ShonanAveragingCLI -i spere2500.txt
 *
 */

#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/dataset.h>

#include <boost/program_options.hpp>

using namespace std;
using namespace gtsam;
namespace po = boost::program_options;

/* ************************************************************************* */
int main(int argc, char* argv[]) {
  string datasetName;
  string inputFile;
  string outputFile;
  int d, seed;
  po::options_description desc(
      "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
      "rotation constraints, and runs the Shonan algorithm.");
  desc.add_options()("help", "Print help message")(
      "named_dataset,n",
      po::value<string>(&datasetName)->default_value("pose3example-grid"),
      "Find and read frome example dataset file")(
      "input_file,i", po::value<string>(&inputFile)->default_value(""),
      "Read pose constraints graph from the specified file")(
      "output_file,o",
      po::value<string>(&outputFile)->default_value("shonan.g2o"),
      "Write solution to the specified file")(
      "dimension,d", po::value<int>(&d)->default_value(3),
      "Optimize over 2D or 3D rotations")(
      "seed,s", po::value<int>(&seed)->default_value(42),
      "Random seed for initial estimate");
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  // Get input file
  if (inputFile.empty()) {
    if (datasetName.empty()) {
      cout << "You must either specify a named dataset or an input file\n"
           << desc << endl;
      return 1;
    }
    inputFile = findExampleDataFile(datasetName);
  }

  // Seed random number generator
  static std::mt19937 rng(seed);

  NonlinearFactorGraph::shared_ptr inputGraph;
  Values::shared_ptr posesInFile;
  Values poses;
  if (d == 2) {
    cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
    ShonanAveraging2 shonan(inputFile);
    auto initial = shonan.initializeRandomly(rng);
    auto result = shonan.run(initial);

    // Parse file again to set up translation problem, adding a prior
    boost::tie(inputGraph, posesInFile) = load2D(inputFile);
    auto priorModel = noiseModel::Unit::Create(3);
    inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);

    cout << "recovering 2D translations" << endl;
    auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
    poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
  } else if (d == 3) {
    cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
    ShonanAveraging3 shonan(inputFile);
    auto initial = shonan.initializeRandomly(rng);
    auto result = shonan.run(initial);

    // Parse file again to set up translation problem, adding a prior
    boost::tie(inputGraph, posesInFile) = load3D(inputFile);
    auto priorModel = noiseModel::Unit::Create(6);
    inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);

    cout << "recovering 3D translations" << endl;
    auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
    poses = initialize::computePoses<Pose3>(result.first, &poseGraph);
  } else {
    cout << "Can only run SO(2) or SO(3) averaging\n" << desc << endl;
    return 1;
  }
  cout << "Writing result to " << outputFile << endl;
  writeG2o(NonlinearFactorGraph(), poses, outputFile);
  return 0;
}

/* ************************************************************************* */


//  (C) Copyright John Maddock 2008.
//  Use, modification and distribution are subject to 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)

#include <pch.hpp>

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/tools/test.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/ulp.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <iostream>
#include <iomanip>

#ifdef BOOST_MSVC
#pragma warning(disable:4127)
#endif

#if !defined(_CRAYC) && !defined(__CUDACC__) && (!defined(__GNUC__) || (__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ > 3)))
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(__SSE2__) || defined(TEST_SSE2)
#include <float.h>
#include "xmmintrin.h"
#define TEST_SSE2
#endif
#endif

template <class T>
void test_value(const T& val, const char* name)
{
   using namespace boost::math;
   T upper = tools::max_value<T>();
   T lower = -upper;

   std::cout << "Testing type " << name << " with initial value " << val << std::endl;

   BOOST_CHECK_EQUAL(float_distance(float_next(val), val), -1);
   BOOST_CHECK(float_next(val) > val);
   BOOST_CHECK_EQUAL(float_distance(float_prior(val), val), 1);
   BOOST_CHECK(float_prior(val) < val);
   BOOST_CHECK_EQUAL(float_distance((boost::math::nextafter)(val, upper), val), -1);
   BOOST_CHECK((boost::math::nextafter)(val, upper) > val);
   BOOST_CHECK_EQUAL(float_distance((boost::math::nextafter)(val, lower), val), 1);
   BOOST_CHECK((boost::math::nextafter)(val, lower) < val);
   BOOST_CHECK_EQUAL(float_distance(float_next(float_next(val)), val), -2);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_prior(val)), val), 2);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_prior(val)), float_next(float_next(val))), 4);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_next(val)), val), 0);
   BOOST_CHECK_EQUAL(float_distance(float_next(float_prior(val)), val), 0);
   BOOST_CHECK_EQUAL(float_prior(float_next(val)), val);
   BOOST_CHECK_EQUAL(float_next(float_prior(val)), val);

   BOOST_CHECK_EQUAL(float_distance(float_advance(val, 4), val), -4);
   BOOST_CHECK_EQUAL(float_distance(float_advance(val, -4), val), 4);
   if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present))
   {
      BOOST_CHECK_EQUAL(float_distance(float_advance(float_next(float_next(val)), 4), float_next(float_next(val))), -4);
      BOOST_CHECK_EQUAL(float_distance(float_advance(float_next(float_next(val)), -4), float_next(float_next(val))), 4);
   }
   if(val > 0)
   {
      T n = val + ulp(val);
      T fn = float_next(val);
      if(n > fn)
      {
         BOOST_CHECK_LE(ulp(val), boost::math::tools::min_value<T>());
      }
      else
      {
         BOOST_CHECK_EQUAL(fn, n);
      }
   }
   else if(val == 0)
   {
      BOOST_CHECK_GE(boost::math::tools::min_value<T>(), ulp(val));
   }
   else
   {
      T n = val - ulp(val);
      T fp = float_prior(val);
      if(n < fp)
      {
         BOOST_CHECK_LE(ulp(val), boost::math::tools::min_value<T>());
      }
      else
      {
         BOOST_CHECK_EQUAL(fp, n);
      }
   }
}

template <class T>
void test_values(const T& val, const char* name)
{
   static const T a = static_cast<T>(1.3456724e22);
   static const T b = static_cast<T>(1.3456724e-22);
   static const T z = 0;
   static const T one = 1;
   static const T two = 2;

   std::cout << "Testing type " << name << std::endl;

   T den = (std::numeric_limits<T>::min)() / 4;
   if(den != 0)
   {
      std::cout << "Denormals are active\n";
   }
   else
   {
      std::cout << "Denormals are flushed to zero.\n";
   }

   test_value(a, name);
   test_value(-a, name);
   test_value(b, name);
   test_value(-b, name);
   test_value(boost::math::tools::epsilon<T>(), name);
   test_value(-boost::math::tools::epsilon<T>(), name);
   test_value(boost::math::tools::min_value<T>(), name);
   test_value(-boost::math::tools::min_value<T>(), name);
   if (std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present) && ((std::numeric_limits<T>::min)() / 2 != 0))
   {
      test_value(z, name);
      test_value(-z, name);
   }
   test_value(one, name);
   test_value(-one, name);
   test_value(two, name);
   test_value(-two, name);
#if defined(TEST_SSE2)
   if((_mm_getcsr() & (_MM_FLUSH_ZERO_ON | 0x40)) == 0)
   {
#endif
      if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present) && ((std::numeric_limits<T>::min)() / 2 != 0))
      {
         test_value(std::numeric_limits<T>::denorm_min(), name);
         test_value(-std::numeric_limits<T>::denorm_min(), name);
         test_value(2 * std::numeric_limits<T>::denorm_min(), name);
         test_value(-2 * std::numeric_limits<T>::denorm_min(), name);
      }
#if defined(TEST_SSE2)
   }
#endif
   static const int primes[] = {
      11,     13,     17,     19,     23,     29,
      31,     37,     41,     43,     47,     53,     59,     61,     67,     71,
      73,     79,     83,     89,     97,    101,    103,    107,    109,    113,
      127,    131,    137,    139,    149,    151,    157,    163,    167,    173,
      179,    181,    191,    193,    197,    199,    211,    223,    227,    229,
      233,    239,    241,    251,    257,    263,    269,    271,    277,    281,
      283,    293,    307,    311,    313,    317,    331,    337,    347,    349,
      353,    359,    367,    373,    379,    383,    389,    397,    401,    409,
      419,    421,    431,    433,    439,    443,    449,    457,    461,    463,
   };

   for(unsigned i = 0; i < sizeof(primes)/sizeof(primes[0]); ++i)
   {
      T v1 = val;
      T v2 = val;
      for(int j = 0; j < primes[i]; ++j)
      {
         v1 = boost::math::float_next(v1);
         v2 = boost::math::float_prior(v2);
      }
      BOOST_CHECK_EQUAL(boost::math::float_distance(v1, val), -primes[i]);
      BOOST_CHECK_EQUAL(boost::math::float_distance(v2, val), primes[i]);
      BOOST_CHECK_EQUAL(boost::math::float_advance(val, primes[i]), v1);
      BOOST_CHECK_EQUAL(boost::math::float_advance(val, -primes[i]), v2);
   }
   if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_infinity))
   {
      BOOST_CHECK_EQUAL(boost::math::float_prior(std::numeric_limits<T>::infinity()), (std::numeric_limits<T>::max)());
      BOOST_CHECK_EQUAL(boost::math::float_next(-std::numeric_limits<T>::infinity()), -(std::numeric_limits<T>::max)());
      BOOST_MATH_CHECK_THROW(boost::math::float_prior(-std::numeric_limits<T>::infinity()), std::domain_error);
      BOOST_MATH_CHECK_THROW(boost::math::float_next(std::numeric_limits<T>::infinity()), std::domain_error);
      if(boost::math::policies:: BOOST_MATH_OVERFLOW_ERROR_POLICY == boost::math::policies::throw_on_error)
      {
         BOOST_MATH_CHECK_THROW(boost::math::float_prior(-(std::numeric_limits<T>::max)()), std::overflow_error);
         BOOST_MATH_CHECK_THROW(boost::math::float_next((std::numeric_limits<T>::max)()), std::overflow_error);
      }
      else
      {
         BOOST_CHECK_EQUAL(boost::math::float_prior(-(std::numeric_limits<T>::max)()), -std::numeric_limits<T>::infinity());
         BOOST_CHECK_EQUAL(boost::math::float_next((std::numeric_limits<T>::max)()), std::numeric_limits<T>::infinity());
      }
   }
   //
   // We need to test float_distance over mulyiple orders of magnitude,
   // the only way to get an accurate true result is to count the representations
   // between the two end points, but we can only really do this for type float:
   //
   if (std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::digits < 30) && (std::numeric_limits<T>::radix == 2))
   {
      T left, right, dist, fresult;
      boost::uintmax_t result;

      left = static_cast<T>(0.1);
      right = left * static_cast<T>(4.2);
      dist = boost::math::float_distance(left, right);
      // We have to use a wider integer type for the accurate count, since there
      // aren't enough bits in T to get a true result if the values differ
      // by more than a factor of 2:
      result = 0;
      for (; left != right; ++result, left = boost::math::float_next(left));
      fresult = static_cast<T>(result);
      BOOST_CHECK_EQUAL(fresult, dist);

      left = static_cast<T>(-0.1);
      right = left * static_cast<T>(4.2);
      dist = boost::math::float_distance(right, left);
      result = 0;
      for (; left != right; ++result, left = boost::math::float_prior(left));
      fresult = static_cast<T>(result);
      BOOST_CHECK_EQUAL(fresult, dist);

      left = static_cast<T>(-1.1) * (std::numeric_limits<T>::min)();
      right = static_cast<T>(-4.1) * left;
      dist = boost::math::float_distance(left, right);
      result = 0;
      for (; left != right; ++result, left = boost::math::float_next(left));
      fresult = static_cast<T>(result);
      BOOST_CHECK_EQUAL(fresult, dist);
   }
}

BOOST_AUTO_TEST_CASE( test_main )
{
   test_values(1.0f, "float");
   test_values(1.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_values(1.0L, "long double");
   test_values(boost::math::concepts::real_concept(0), "real_concept");
#endif

   //
   // Test some multiprecision types:
   //
   test_values(boost::multiprecision::cpp_bin_float_quad(0), "cpp_bin_float_quad");
   // This is way to slow to test routinely:
   //test_values(boost::multiprecision::cpp_bin_float_single(0), "cpp_bin_float_single");
   test_values(boost::multiprecision::cpp_bin_float_50(0), "cpp_bin_float_50");

#if defined(TEST_SSE2)

#ifdef _MSC_VER
#  pragma message("Compiling SSE2 test code")
#endif
#ifdef __GNUC__
#  pragma message "Compiling SSE2 test code"
#endif

   int mmx_flags = _mm_getcsr(); // We'll restore these later.

#ifdef _WIN32
   // These tests fail pretty badly on Linux x64, especially with Intel-12.1
   _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
   std::cout << "Testing again with Flush-To-Zero set" << std::endl;
   std::cout << "SSE2 control word is: " << std::hex << _mm_getcsr() << std::endl;
   test_values(1.0f, "float");
   test_values(1.0, "double");
   _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_OFF);
#endif
   BOOST_ASSERT((_mm_getcsr() & 0x40) == 0);
   _mm_setcsr(_mm_getcsr() | 0x40);
   std::cout << "Testing again with Denormals-Are-Zero set" << std::endl;
   std::cout << "SSE2 control word is: " << std::hex << _mm_getcsr() << std::endl;
   test_values(1.0f, "float");
   test_values(1.0, "double");

   // Restore the MMX flags:
   _mm_setcsr(mmx_flags);
#endif

}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/comparable.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/tuple.hpp>

#include <stdexcept>

namespace boost { namespace hana {
    struct Function {
        struct hana {
            struct operators
                : boost::hana::operators::of<Comparable>
            { };
        };
    };

    template <typename Domain, typename Codomain, typename F, typename = operators::adl>
    struct function_type {
        struct hana { using datatype = Function; };

        Domain dom;
        Codomain cod;
        F def;

        friend constexpr auto domain(function_type f)
        { return f.dom; }

        friend constexpr auto codomain(function_type f)
        { return f.cod; }

        template <typename X>
        constexpr auto operator()(X x) const {
            if (!elem(domain(*this), x))
                throw std::domain_error{"use of a hana::function with an argument out of the domain"};
            return def(x);
        }
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto function = [](auto domain, auto codomain) {
        return [=](auto definition) {
            return function_type<decltype(domain), decltype(codomain), decltype(definition)>{
                domain, codomain, definition
            };
        };
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto frange = [](auto f) {
        // Note: that would be better handled by a set data structure, but
        // whatever for now.
        return foldl(transform(domain(f), f), make<Tuple>(), [](auto xs, auto x) {
            return if_(elem(xs, x), xs, prepend(x, xs));
        });
    };

    template <>
    struct equal_impl<Function, Function> {
        template <typename F, typename G>
        static constexpr auto apply(F f, G g) {
            return domain(f) == domain(g) && all_of(domain(f), demux(equal)(f, g));
        }
    };
}} // end namespace boost::hana

// BOOST_HANA_CONSTEXPR_LAMBDA auto is_injective = [](auto f) {
//     auto check = [](auto x, auto y) {
//         return (x != y)     ^implies^   (f(x) != f(y));
//     };
//     return all_of(product(domain(f), domain(f)), check);
// };

// BOOST_HANA_CONSTEXPR_LAMBDA auto is_onto = [](auto f) {
//     return codomain(f) == range(g);
// };

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;
using namespace literals;

int main() {
    auto f = function(make<Tuple>(1_c, 2_c, 3_c), make<Tuple>(1_c, 2_c, 3_c, 4_c, 5_c, 6_c))(
        [](auto x) { return x + 1_c; }
    );

    auto g = function(make<Tuple>(1_c, 2_c, 3_c), make<Tuple>(2_c, 3_c, 4_c))(
        [](auto x) { return x + 1_c; }
    );

    auto h = function(make<Tuple>(1_c, 2_c, 3_c), make<Tuple>(0_c, 1_c, 2_c))(
        [](auto x) { return x - 1_c; }
    );

    BOOST_HANA_CONSTANT_CHECK(f == g);
    BOOST_HANA_CONSTANT_CHECK(f != h);
    BOOST_HANA_CONSTEXPR_CHECK(f(1) == 2);
    try { f(6); throw; } catch (std::domain_error) { }

    BOOST_HANA_CONSTANT_CHECK(frange(f) == make<Tuple>(4_c, 3_c, 2_c));
    (void)frange;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COSPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COSPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing cospi capabilities

    cosine of angle in \f$\pi\f$ multiples.

    @par Semantic:

    For every parameter of floating or integral type T

    @code
    T r = cospi(x);
    @endcode

    is similar to:

    @code
    T r = cos(Pi<T>()*x);
    @endcode

    @par Note

    As other cosine functions cospi can be used with two parameters as
    @code
    T r = cos(x, range_);
    @endcode

    see @ref cos for further details

    As it conveys a peculiar meaning,  unlike the orher cosine, cospi is defined
    for integral types and the result of cospi(n) coincides with \f$(-1)^n\f$.

    Take care that large floating entries are always integral and even !

    @see sincospi, cos, cosd

  **/
  const boost::dispatch::functor<tag::cospi_> cospi = {};
} }
#endif

#include <boost/simd/function/scalar/cospi.hpp>
#include <boost/simd/function/simd/cospi.hpp>

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/multiplies.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" bs::saturated_(bs::multiplies) signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using r_t = decltype(bs::saturated_(bs::multiplies)(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Mone<T>(), bs::Mone<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmax<T>(), bs::Valmax<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmax<T>(),T(2)), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmax<T>(),bs::Mone<T>()), bs::Valmin<T>()+bs::One<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmax<T>(),bs::One<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmin<T>(),bs::Mone<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_

STF_CASE_TPL (" bs::saturated_(bs::multiplies)unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using r_t = decltype(bs::saturated_(bs::multiplies)(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Valmax<T>(),T(2)), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE("mul sspecial")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::splat;
  using bs::Valmin;

  typedef short int T1;
  STF_EQUAL(bs::saturated_(bs::multiplies)(splat<T1>(-5165), splat<T1>(23258)), Valmin<T1>());

  typedef int T2;
  STF_EQUAL(bs::saturated_(bs::multiplies)(splat<T2>(-1306766858), splat<T2>(1550772331)), Valmin<T2>());
  STF_EQUAL(bs::saturated_(bs::multiplies)(splat<T2>(1467238299), splat<T2>(-900961598)), Valmin<T2>());
}


#include <iostream>                          // std::cout/endl/dec/hex
#include <boost/multiprecision/cpp_int.hpp>  // boost::multiprecision::cpp_int

using namespace std;

int main()
{
    using namespace boost::multiprecision::literals;
    using boost::multiprecision::cpp_int;

    cpp_int a = 0x123456789abcdef0_cppi;
    cpp_int b = 16;
    cpp_int c{"0400"};
    cpp_int result = a * b / c;
    cout << hex << result << endl;
    cout << dec << result << endl;
}


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  // Idea: iterate over entities in the mesh and get interior number of dofs for
  // each
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  for (std::size_t codim = 0; codim <= 2; ++codim) {
    entityDofs[codim] = 0;
    for (const auto *el : mesh->Entities(codim)) {
      if (el->RefEl() == lf::base::RefEl::kQuad()) {
        throw "Only triangular meshes are allowed!";
      }
      entityDofs[codim] += dofhandler.NumInteriorDofs(*el);
    }
  }
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  // Edges and nodes can be on the boundary
  for (const auto *edge : mesh->Entities(1)) {
    if (bd_flags(*edge)) {
      no_dofs_on_bd += dofhandler.NumInteriorDofs(*edge);
    }
  }
  for (const auto *node : mesh->Entities(2)) {
    if (bd_flags(*node)) {
      no_dofs_on_bd += dofhandler.NumInteriorDofs(*node);
    }
  }
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  std::cout << "indices\n";
  for (const auto *cell : mesh->Entities(0)) {
    // check if we the FE space is really $\Cs_1^0$
    if (dofhandler.NumLocalDofs(*cell) != 3) {
      throw "Not a S_1^0 FE space!";
    }
    // iterate over dofs
    auto int_dofs = dofhandler.GlobalDofIndices(*cell);

    for (auto dof_idx_p = int_dofs.begin(); dof_idx_p < int_dofs.end();
         ++dof_idx_p) {
      // local integral of the basis function associated with this dof:
      // in linear Lagrangian FE, the integral over the basis functions over
      // a triangle K is: 1/3*vol(K)
      const double I_bary = 1.0 / 3.0 * lf::geometry::Volume(*(cell->Geometry()));
      // multiply by the value at the dof to get local contribution
    std::cout << *dof_idx_p << "\n";
      I += I_bary * mu(*dof_idx_p);
    }
  }
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  for (const auto *cell : mesh->Entities(0)) {
    // check if we the FE space is really $\Cs_2^0$
    if (dofhandler.NumLocalDofs(*cell) != 6) {
      throw "Not a S_2^0 FE space!";
    }
    const double weight = 1.0 / 3.0 * lf::geometry::Volume(*(cell->Geometry()));
    // iterate over dofs
    auto int_dofs = dofhandler.GlobalDofIndices(*cell);
    // The integrated basis functions associated with the nodes are 0:
    //  $\int_K b_K^j dx = 0$ for $j = 1,2,3$. Skip!
    for (int l = 3; l < 6; ++l) {
      // The integrated basis functions associated with the edges are:
      // $\int_K b_K^j dx = |K|/3$ for $j = 4,5,6$
      // multiply by the value at the dof to get local contribution
      I += (weight * mu(int_dofs[l]));
    }
  }
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    if (dofh_Linear_FE.NumLocalDofs(*cell) != 3 ||
        dofh_Quadratic_FE.NumLocalDofs(*cell) != 6) {
      throw "dofh_Linear_FE must have 3 dofs per cell and dofh_Quadratic_FE 6!";
    }
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    nonstd::span<const lf::assemble::gdof_idx_t> lin_dofs =
        dofh_Linear_FE.GlobalDofIndices(*cell);
    nonstd::span<const lf::assemble::gdof_idx_t> quad_dofs =
        dofh_Quadratic_FE.GlobalDofIndices(*cell);
    for (std::size_t l = 0; l <= 2; ++l) {
      // Let $p_1,p_2,p_3$ be the nodes of the triangle and $p_4,p_5,p_6$ the
      // edge midpoints. Let $\lambda_1,\lambda_2,\lambda_3$ be the local basis
      // functions of $\Cs_1^0$ and $b_1,..,b_6$ for $\Cs_2^0$. The values of
      // $\lambda_i$ in the interpolation nodes $p_1,..,p_6$ are the
      // coefficients of writing $\lambda_i$ as lin. comb of $b_1,..b_6$.
      // From 2-9.a we know $\lambda_l(p_l) = 1$; $l = 1,2,3$.
      // Hence we simply \textbf{copy} the coefficient of $\lambda_l$ to $b_l$
      // for $l=1,2,3$.
      zeta(quad_dofs[l]) = mu(lin_dofs[l]);
      // And $\lambda_l(p_l+3) = 0.5$, $\lambda_{(l+1) mod 3}(p_l+3) = 0.5$;
      // $l =1,2,3$. Hence we copy 0.5 of the respective coefficients!
      zeta(quad_dofs[l + 3]) =
          0.5 * (mu(lin_dofs[l]) + mu(lin_dofs[(l + 1) % 3]));
    }
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


#include "LatticeFitter.hpp"
#include <algorithm>
#include <NTL/LLL.h>

LatticeFitter::LatticeFitter(QObject* parent)
  : QObject(parent) {}

void LatticeFitter::findBestLattice(const std::vector<cv::Point2f>& points)
{
  emit latticeFittingStarted();
  emit progressUpdated(0);

  std::vector<Lattice> bestLattices;

  double progress = 0.0;
  double progress_delta = 100.0 / points.size();

  for (const auto& p : points) {
    bestLattices.emplace_back(bestLatticeForOrigin(p, points));
    progress += progress_delta;
    emit progressUpdated(static_cast<int>(std::round(progress)));
  }

  best_lattice = *std::min_element(bestLattices.cbegin(), bestLattices.cend(),
  [](const Lattice& l1, const Lattice& l2) {
    return l1.total_error < l2.total_error;
  });

  emit foundBestLattice(best_lattice);
}

Lattice LatticeFitter::bestLatticeForOrigin(
    const cv::Point2f& origin, const std::vector<cv::Point2f>& points) const
{
  std::vector<NTL::vec_ZZ> vecs;

  for (const auto& point : points) {
    if (point == origin) continue;

    vecs.push_back(cvPoint2fToNTLVec(point - origin));
  }

  std::vector<Lattice> lattices;

  for (size_t i = 0; i < vecs.size(); ++i) {
    for (size_t j = i + 1; j < vecs.size(); ++j) {
      lattices.emplace_back();
      auto& current_lattice = lattices.back();

      current_lattice.origin = cvPoint2fToNTLVec(origin);
      current_lattice.bases.SetDims(2, 2);
      current_lattice.bases[0] = vecs[i];
      current_lattice.bases[1] = vecs[j];

      NTL::LLL_FP(current_lattice.bases);
      calculateErrorForLattice(current_lattice, vecs);
    }
  }

  return *std::min_element(lattices.cbegin(), lattices.cend(),
  [](const Lattice& l1, const Lattice& l2) {
    return l1.total_error < l2.total_error;
  });
}

void LatticeFitter::calculateErrorForLattice(Lattice& lattice,
    const std::vector<NTL::vec_ZZ>& points) const
{
  lattice.total_error = 0;

  for (const auto& vec : points) {
    NTL::vec_ZZ v;
    NTL::NearVector(v, lattice.bases, vec);
    auto u = v - vec;

    lattice.total_error += std::hypot(NTL::to_double(u[0]), NTL::to_double(u[1]));
  }
}


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once
#include <random>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>

#include "vmf.hpp"

template<typename T>
class vMFprior {
 public:
  vMFprior(const Eigen::Matrix<T,3,1>& m0, T a, T b)
    : m0_(m0), b_(b), a_(a), unif_(0.,1.)
  {}

  T logMarginal(const Eigen::Matrix<T,3,1>& x) const {
    const T bTilde = (x + b_*m0_).norm();
    const T bOverTan = b_ < 1e-9 ? 2./M_PI : b_/tan(M_PI*0.5*b_);
    const T sinc = bTilde < 1e-9 ? 1. : sin(bTilde*M_PI)/(bTilde*M_PI);
    const T sinus = sin(bTilde*0.5*M_PI);
    return log(bOverTan*0.125) + log(1.-sinc) - 2*log(sinus);
  }

  vMF<T,3> sample(std::mt19937& rnd) {
//    std::cout << "sample from vMF prior " << a_ << " " << b_
//      << " " << m0_.transpose() << std::endl;
    Eigen::Matrix<T,3,1> mu;
    T tau = 1.;
    vMF<T,3> vmf(m0_, tau*b_);
//    std::cout << "sampling from base" << std::endl;
    for (size_t it=0; it<10; ++it) {
      vmf.tau_ = tau*b_;
      mu = vmf.sample(rnd);
//      std::cout << "mu " << mu.transpose() << std::endl;
      const T dot = mu.dot(m0_);
      tau = sampleConcentration(dot, rnd, 3, tau);
//      std::cout <<"@" << it << "tau " << tau << " mu " << mu.transpose() << std::endl;
    }
    return vMF<T,3>(mu, tau);
  }

  vMF<T,3> MAP() {
    Eigen::Matrix<T,3,1> xSum=b_*m0_;
    float tau = MLEstimateTau<float,3>(xSum, m0_, a_);
    return vMF<T,3>(m0_, tau);
  }

  vMFprior<T> posterior(const Eigen::Matrix<T,3,1>& xSum, const T count) const {
    T aN = a_+count;
    Eigen::Matrix<T,3,1> muN = xSum + b_*m0_;
    T bN = muN.norm();
    muN /= bN;
    return vMFprior<T>(muN, aN, bN);
  }

  vMFprior<T> posterior(const Eigen::Matrix<T,4,1>& ss) const {
    T aN = a_+ss(3);
    Eigen::Matrix<T,3,1> muN = ss.topRows(3) + b_*m0_;
    T bN = muN.norm();
    muN /= bN;
    return vMFprior<T>(muN, aN, bN);
  }

  Eigen::Matrix<T,3,1> m0_;
  T b_;
  T a_;
 private:

  T propToConcentrationLogPdf(const T tau, const T dot) const
  {
    if (tau < 1e-16) {
      return 0.;
    } else {
      return a_*(log(tau) + LOG_2 - log(1.-exp(-2.*tau))) + tau*(b_*dot-a_);
    }
  };

  T propToConcentrationLogPdfDeriv(const T tau, const T dot) const
  {
    // this is only for 3D case
    if (tau < 1e-16) {
      return b_*dot;
    } else {
      return a_/tau - (2.*a_*exp(-2.*tau)/(1.-exp(-2.*tau))) + b_*dot -a_;
    }
  };
  T propToConcentrationLogPdfDerivDeriv(const T tau, const T dot) const
  {
    // this is only for 3D case
    if (tau < 1e-16) {
      return -a_/3.;
    } else {
//      return -a_/(tau*tau) + (4.*a_*exp(2.*tau)/(1.-2.*exp(2.*tau)+exp(4.*tau)));
      return -a_/(tau*tau) + (4.*a_*exp(-2.*tau)/(1.-2.*exp(-2.*tau)+exp(-4.*tau)));
    }
  };

  T maximum(const T dot) {
    if (dot*b_ <= 0)
      return 0.;
    T tau = 1.;
    for (size_t it=0; it<100; ++it) {
      T f = propToConcentrationLogPdfDeriv(tau, dot);
      T df = propToConcentrationLogPdfDerivDeriv(tau, dot);
      tau -= f/df;
      if (fabs(f/df) < 1e-6)
        break;
    }
    return tau;
  };

  T intersect(const T c, const T dot, const T tau0) {
    T tau = tau0;
    for (size_t it=0; it<100; ++it) {
      T f = propToConcentrationLogPdf(tau, dot) - c;
      T df = propToConcentrationLogPdfDeriv(tau, dot);
      tau = std::max(0.f, tau-f/df);
//      std::cout << "   __ " <<it << ": " << f << " " << df
//        << "\t f/df " << fabs(f/df)
//        << "\t step to " << tau-f/df << ": "<< tau << std::endl;
      if (fabs(f/df) < 1e-6 || tau == 0.)
        break;
    }
    return tau;
  };

  /// slice sampler for concentration paramter tau
  T sampleConcentration(const T dot, std::mt19937& rnd, size_t maxIt, T tau0 = 0.3)
  {
    T tauMax = maximum(dot);
    T tau = tau0;
//    std::cout << " ----- max " << tauMax << " " << tau0
//      << " dot " << dot << std::endl;
    T tauL = 0.;
    T tauR = tauMax;
    for(size_t t=0; t<maxIt; ++t)
    {
      const T f = propToConcentrationLogPdf(tau,dot);
      const T u = log(unif_(rnd)) + f;

      if (tauMax > 0.) {
        tauL = intersect(u, dot, tauMax*0.001);
        tauR = intersect(u, dot, tauMax*1.5);
      } else {
        tauL = 0.;
        tauR = intersect(u, dot, 0.5);
      }
      tau = unif_(rnd)*(tauR-tauL)+tauL;
//      std::cout << tauL << " - " << tauMax << " - " << tauR
//        << " tau= " << tau
//        << " : u " << u << " f(tau) " << f
//        << " f(tau^star) " << propToConcentrationLogPdf(tauMax,dot)
//        << std::endl;
    }
    return tau;
  };

  /// Old backstepping slice sampler implementation
  T sampleConcentrationStepping(const T dot, std::mt19937& rnd, size_t maxIt, T tau0 = 0.3)
  {
//    std::cout << "start sampling concentration ---" << std::endl;
    // slice sampler for concentration paramter tau
    const T w = 0.1;  // width for expansions of search region
    T tau = tau0;      // arbitrary starting point
    for(size_t t=0; t<maxIt; ++t)
    {
      const T yMax = propToConcentrationLogPdf(tau,dot);
      const T y = log(unif_(rnd)) + yMax;
      T tauMin = tau-w;
      T tauMax = tau+w;
//      std::cout << "before " << tauMin << " " << tauMax
//        << ": " << propToConcentrationLogPdf(tauMin,dot)
//        << " " << propToConcentrationLogPdf(tauMax,dot) << std::endl;
      while (tauMin >=0. && propToConcentrationLogPdf(tauMin,dot) >= y) tauMin -= w;
      tauMin = std::max(static_cast<T>(0.),tauMin);
      while (propToConcentrationLogPdf(tauMax,dot) >= y) tauMax += w;
//      std::cout << "after "  << tauMin << " " << tauMax
//        << ": " << propToConcentrationLogPdf(tauMin,dot)
//        << " " << propToConcentrationLogPdf(tauMax,dot) << std::endl;
      while(42) {
        T tauNew = unif_(rnd)*(tauMax-tauMin)+tauMin;

//        std::cout << "@"<< t << ": " << tauMin << " " << tauMax << " " << tauNew << " " << tau
//          << ": " << propToConcentrationLogPdf(tauNew,dot)
//          << " >=? " << y
//          << ",  " << propToConcentrationLogPdf(tauMin,dot)
//          << " "  << propToConcentrationLogPdf(tauMax,dot) << std::endl;

        if(propToConcentrationLogPdf(tauNew,dot) >= y)
        {
          tau = tauNew; break;
        }else{
          if (tauNew < tau) tauMin = tauNew; else tauMax = tauNew;
        }
      };
    }
    return tau;
  };
  std::uniform_real_distribution<T> unif_;
};