/**
* This file is part of DSO.
* 
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
*/



#pragma once

#include "util/settings.h"
#include "util/NumType.h"
#include "IOWrapper/ImageDisplay.h"
#include "fstream"

namespace dso {



// reads interpolated element from a uchar* array
// SSE2 optimization possible
  EIGEN_ALWAYS_INLINE float
  getInterpolatedElement(const float *const mat, const float x, const float y, const int width) {
    //stats.num_pixelInterpolations++;

    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const float *bp = mat + ix + iy * width;


    float res = dxdy * bp[1 + width]
                + (dy - dxdy) * bp[width]
                + (dx - dxdy) * bp[1]
                + (1 - dx - dy + dxdy) * bp[0];

    return res;
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement43(const Eigen::Vector4f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector4f *bp = mat + ix + iy * width;


    return dxdy * *(const Eigen::Vector3f *) (bp + 1 + width)
           + (dy - dxdy) * *(const Eigen::Vector3f *) (bp + width)
           + (dx - dxdy) * *(const Eigen::Vector3f *) (bp + 1)
           + (1 - dx - dy + dxdy) * *(const Eigen::Vector3f *) (bp);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement33(const Eigen::Vector3f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector3f *bp = mat + ix + iy * width;


    return dxdy * *(const Eigen::Vector3f *) (bp + 1 + width)
           + (dy - dxdy) * *(const Eigen::Vector3f *) (bp + width)
           + (dx - dxdy) * *(const Eigen::Vector3f *) (bp + 1)
           + (1 - dx - dy + dxdy) * *(const Eigen::Vector3f *) (bp);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement33OverAnd(const Eigen::Vector3f *const mat, const bool *overMat, const float x, const float y,
                                  const int width, bool &over_out) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector3f *bp = mat + ix + iy * width;

    const bool *bbp = overMat + ix + iy * width;
    over_out = bbp[1 + width] && bbp[1] && bbp[width] && bbp[0];

    return dxdy * *(const Eigen::Vector3f *) (bp + 1 + width)
           + (dy - dxdy) * *(const Eigen::Vector3f *) (bp + width)
           + (dx - dxdy) * *(const Eigen::Vector3f *) (bp + 1)
           + (1 - dx - dy + dxdy) * *(const Eigen::Vector3f *) (bp);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement33OverOr(const Eigen::Vector3f *const mat, const bool *overMat, const float x, const float y,
                                 const int width, bool &over_out) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector3f *bp = mat + ix + iy * width;

    const bool *bbp = overMat + ix + iy * width;
    over_out = bbp[1 + width] || bbp[1] || bbp[width] || bbp[0];

    return dxdy * *(const Eigen::Vector3f *) (bp + 1 + width)
           + (dy - dxdy) * *(const Eigen::Vector3f *) (bp + width)
           + (dx - dxdy) * *(const Eigen::Vector3f *) (bp + 1)
           + (1 - dx - dy + dxdy) * *(const Eigen::Vector3f *) (bp);
  }

  EIGEN_ALWAYS_INLINE float
  getInterpolatedElement31(const Eigen::Vector3f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector3f *bp = mat + ix + iy * width;


    return dxdy * (*(const Eigen::Vector3f *) (bp + 1 + width))[0]
           + (dy - dxdy) * (*(const Eigen::Vector3f *) (bp + width))[0]
           + (dx - dxdy) * (*(const Eigen::Vector3f *) (bp + 1))[0]
           + (1 - dx - dy + dxdy) * (*(const Eigen::Vector3f *) (bp))[0];
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement13BiLin(const float *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    const float *bp = mat + ix + iy * width;

    float tl = *(bp);
    float tr = *(bp + 1);
    float bl = *(bp + width);
    float br = *(bp + width + 1);

    float dx = x - ix;
    float dy = y - iy;
    float topInt = dx * tr + (1 - dx) * tl;
    float botInt = dx * br + (1 - dx) * bl;
    float leftInt = dy * bl + (1 - dy) * tl;
    float rightInt = dy * br + (1 - dy) * tr;

    return Eigen::Vector3f(
        dx * rightInt + (1 - dx) * leftInt,
        rightInt - leftInt,
        botInt - topInt);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement33BiLin(const Eigen::Vector3f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    const Eigen::Vector3f *bp = mat + ix + iy * width;

    float tl = (*(bp))[0];
    float tr = (*(bp + 1))[0];
    float bl = (*(bp + width))[0];
    float br = (*(bp + width + 1))[0];

    float dx = x - ix;
    float dy = y - iy;
    float topInt = dx * tr + (1 - dx) * tl;
    float botInt = dx * br + (1 - dx) * bl;
    float leftInt = dy * bl + (1 - dy) * tl;
    float rightInt = dy * br + (1 - dy) * tr;

    return Eigen::Vector3f(
        dx * rightInt + (1 - dx) * leftInt,
        rightInt - leftInt,
        botInt - topInt);
  }

  EIGEN_ALWAYS_INLINE float
  getInterpolatedElement11Cub(const float *const p, const float x)  // for x=0, this returns p[1].
  {
    return p[1] + 0.5f * x * (p[2] - p[0] + x * (2.0f * p[0] - 5.0f * p[1] + 4.0f * p[2] - p[3] +
                                                 x * (3.0f * (p[1] - p[2]) + p[3] - p[0])));
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector2f
  getInterpolatedElement12Cub(const float *const p, const float x)  // for x=0, this returns p[1].
  {
    float c1 = 0.5f * (p[2] - p[0]);
    float c2 = p[0] - 2.5f * p[1] + 2 * p[2] - 0.5f * p[3];
    float c3 = 0.5f * (3.0f * (p[1] - p[2]) + p[3] - p[0]);
    float xx = x * x;
    float xxx = xx * x;
    return Eigen::Vector2f(p[1] + x * c1 + xx * c2 + xxx * c3, c1 + x * 2.0f * c2 + xx * 3.0f * c3);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector2f
  getInterpolatedElement32Cub(const Eigen::Vector3f *const p, const float x)  // for x=0, this returns p[1].
  {
    float c1 = 0.5f * (p[2][0] - p[0][0]);
    float c2 = p[0][0] - 2.5f * p[1][0] + 2 * p[2][0] - 0.5f * p[3][0];
    float c3 = 0.5f * (3.0f * (p[1][0] - p[2][0]) + p[3][0] - p[0][0]);
    float xx = x * x;
    float xxx = xx * x;
    return Eigen::Vector2f(p[1][0] + x * c1 + xx * c2 + xxx * c3, c1 + x * 2.0f * c2 + xx * 3.0f * c3);
  }

  EIGEN_ALWAYS_INLINE float
  getInterpolatedElement11BiCub(const float *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    const float *bp = mat + ix + iy * width;

    float val[4];
    val[0] = getInterpolatedElement11Cub(bp - width - 1, dx);
    val[1] = getInterpolatedElement11Cub(bp - 1, dx);
    val[2] = getInterpolatedElement11Cub(bp + width - 1, dx);
    val[3] = getInterpolatedElement11Cub(bp + 2 * width - 1, dx);

    float dy = y - iy;
    return getInterpolatedElement11Cub(val, dy);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement13BiCub(const float *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    const float *bp = mat + ix + iy * width;

    float val[4];
    float grad[4];
    Eigen::Vector2f v = getInterpolatedElement12Cub(bp - width - 1, dx);
    val[0] = v[0];
    grad[0] = v[1];

    v = getInterpolatedElement12Cub(bp - 1, dx);
    val[1] = v[0];
    grad[1] = v[1];

    v = getInterpolatedElement12Cub(bp + width - 1, dx);
    val[2] = v[0];
    grad[2] = v[1];

    v = getInterpolatedElement12Cub(bp + 2 * width - 1, dx);
    val[3] = v[0];
    grad[3] = v[1];

    float dy = y - iy;
    v = getInterpolatedElement12Cub(val, dy);

    return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector3f
  getInterpolatedElement33BiCub(const Eigen::Vector3f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    const Eigen::Vector3f *bp = mat + ix + iy * width;

    float val[4];
    float grad[4];
    Eigen::Vector2f v = getInterpolatedElement32Cub(bp - width - 1, dx);
    val[0] = v[0];
    grad[0] = v[1];

    v = getInterpolatedElement32Cub(bp - 1, dx);
    val[1] = v[0];
    grad[1] = v[1];

    v = getInterpolatedElement32Cub(bp + width - 1, dx);
    val[2] = v[0];
    grad[2] = v[1];

    v = getInterpolatedElement32Cub(bp + 2 * width - 1, dx);
    val[3] = v[0];
    grad[3] = v[1];

    float dy = y - iy;
    v = getInterpolatedElement12Cub(val, dy);

    return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector4f
  getInterpolatedElement44(const Eigen::Vector4f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector4f *bp = mat + ix + iy * width;


    return dxdy * *(bp + 1 + width)
           + (dy - dxdy) * *(bp + width)
           + (dx - dxdy) * *(bp + 1)
           + (1 - dx - dy + dxdy) * *(bp);
  }

  EIGEN_ALWAYS_INLINE Eigen::Vector2f
  getInterpolatedElement42(const Eigen::Vector4f *const mat, const float x, const float y, const int width) {
    int ix = (int) x;
    int iy = (int) y;
    float dx = x - ix;
    float dy = y - iy;
    float dxdy = dx * dy;
    const Eigen::Vector4f *bp = mat + ix + iy * width;


    return dxdy * *(const Eigen::Vector2f *) (bp + 1 + width)
           + (dy - dxdy) * *(const Eigen::Vector2f *) (bp + width)
           + (dx - dxdy) * *(const Eigen::Vector2f *) (bp + 1)
           + (1 - dx - dy + dxdy) * *(const Eigen::Vector2f *) (bp);
  }


  inline Vec3f makeRainbowf3F(float id) {
    id *= freeDebugParam3;
    if (id < 0)
      return Vec3f(1, 1, 1);

    int icP = id;
    float ifP = id - icP;
    icP = icP % 3;

    if (icP == 0) return Vec3f((1 - ifP), ifP, 0);
    if (icP == 1) return Vec3f(0, (1 - ifP), ifP);
    if (icP == 2) return Vec3f(ifP, 0, (1 - ifP));
    assert(false);
    return Vec3f(1, 1, 1);
  }

  inline Vec3b makeRainbow3B(float id) {
    id *= freeDebugParam3;
    if (!(id > 0))
      return Vec3b(255, 255, 255);

    int icP = id;
    float ifP = id - icP;
    icP = icP % 3;

    if (icP == 0) return Vec3b(255 * (1 - ifP), 255 * ifP, 0);
    if (icP == 1) return Vec3b(0, 255 * (1 - ifP), 255 * ifP);
    if (icP == 2) return Vec3b(255 * ifP, 0, 255 * (1 - ifP));
    return Vec3b(255, 255, 255);
  }

  inline Vec3b makeJet3B(float id) {
    if (id <= 0) return Vec3b(128, 0, 0);
    if (id >= 1) return Vec3b(0, 0, 128);

    int icP = (id * 8);
    float ifP = (id * 8) - icP;

    if (icP == 0) return Vec3b(255 * (0.5 + 0.5 * ifP), 0, 0);
    if (icP == 1) return Vec3b(255, 255 * (0.5 * ifP), 0);
    if (icP == 2) return Vec3b(255, 255 * (0.5 + 0.5 * ifP), 0);
    if (icP == 3) return Vec3b(255 * (1 - 0.5 * ifP), 255, 255 * (0.5 * ifP));
    if (icP == 4) return Vec3b(255 * (0.5 - 0.5 * ifP), 255, 255 * (0.5 + 0.5 * ifP));
    if (icP == 5) return Vec3b(0, 255 * (1 - 0.5 * ifP), 255);
    if (icP == 6) return Vec3b(0, 255 * (0.5 - 0.5 * ifP), 255);
    if (icP == 7) return Vec3b(0, 0, 255 * (1 - 0.5 * ifP));
    return Vec3b(255, 255, 255);
  }

  inline Vec3b makeRedGreen3B(float val)  // 0 = red, 1=green, 0.5=yellow.
  {
    if (val < 0) return Vec3b(0, 0, 255);
    else if (val < 0.5) return Vec3b(0, 255 * 2 * val, 255);
    else if (val < 1) return Vec3b(0, 255, 255 - 255 * 2 * (val - 0.5));
    else return Vec3b(0, 255, 0);

  }

  inline double sinc(double x) {
    if (fabs(x) > 1e-6) {
      return sin(x) / x;
    }
    else {
      static const double c_2 = 1.0 / 6.0;
      static const double c_4 = 1.0 / 120.0;
      static const double c_6 = 1.0 / 5040.0;
      const double x_2 = x * x;
      const double x_4 = x_2 * x_2;
      const double x_6 = x_2 * x_2 * x_2;
      return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
    }
  }

  inline Mat66 LeftJacobian(SE3 T) {
    auto Adj = T.Adj();
    double phi = acos((double) T.so3().unit_quaternion().w()) * 2;
    Mat66 ret = Mat66::Identity();
    double phi_inv = 1.0 / phi;

    double phi_temp = phi_inv * phi_inv;
    auto Adj_temp = Adj;
    ret += (4 - phi * sin(phi) - 4 * cos(phi)) * 0.5 * phi_temp * Adj_temp;

    phi_temp *= phi_inv;
    Adj_temp *= Adj;
    ret += (4 * phi - 5 * sin(phi) + phi * cos(phi)) * 0.5 * phi_temp * Adj_temp;

    phi_temp *= phi_inv;
    Adj_temp *= Adj;
    ret += (2 - phi * sin(phi) - 2 * cos(phi)) * 0.5 * phi_temp * Adj_temp;

    phi_temp *= phi_inv;
    Adj_temp *= Adj;
    ret += (2 * phi - 3 * sin(phi) + phi * cos(phi)) * 0.5 * phi_temp * Adj_temp;

    return ret;
  }

  inline Mat66 RightJacobian(SE3 T) {
    return LeftJacobian(T.inverse());
  }

  inline Mat33 Hat(Vec3 a) {
    Mat33 hat = Mat33::Zero();
    hat(0, 1) = -a[2];
    hat(0, 2) = a[1];
    hat(1, 2) = -a[0];
    hat(1, 0) = a[2];
    hat(2, 0) = -a[1];
    hat(2, 1) = a[0];
    return hat;
  }
}
