//
// Created by Macx on 2018/10/6.
//

#include <vector>
#include <cmath>
#include <cstdlib>
#include <cstddef>
#include <limits>
#include <algorithm>
#include <time.h>
//#include <Eigen/Dense>
//#include <Eigen/Geometry>
#include <iostream>
#include "Model.h"
#include <ofxProfiler.h>
#include <GL/glcorearb.h>
#include "Math.hpp"
#include "Constants.hpp"
#include "tiny_obj_loader.h"

#if defined __linux__ || defined __APPLE__
// "Compiled for Linux
#else
// Windows doesn't define these values by default, Linux does
#define M_PI 3.141592653589793f
#define INFINITY 1e8
#endif

#if defined __linux__ || defined __APPLE__
#include <SDL.h>
#include <zconf.h>
#else
#include "SDL.h"
#undef min
#undef max
#endif

#define WITH_TEXTURE 1

const S32 width = 800;
const S32 height = 800;
const S32 depth = 255;

//using Eigen::Vec3f;
//using Eigen::Mat4f;
using namespace FW;

SDL_PixelFormat *pixFormat = SDL_AllocFormat(SDL_PIXELFORMAT_RGBA8888);

U32 WHITE = SDL_MapRGBA(pixFormat, 255, 255, 255, 255);
U32 BLACK = SDL_MapRGBA(pixFormat, 0, 0, 0, 255);
U32 RED = SDL_MapRGBA(pixFormat, 255, 0, 0, 255);
U32 GREEN = SDL_MapRGBA(pixFormat, 0, 255, 0, 255);
U32 BLUE = SDL_MapRGBA(pixFormat, 0, 0, 255, 255);

Vec3f light_dir(1, -1, 1);
Vec3f eye(1, 1, 3);
Vec3f center(0, 0, 0);

Model *model = NULL;
SDL_Window *gWindow = NULL;
SDL_Renderer *gRender = NULL;
SDL_Texture *gTexture = NULL;

void init() {
  SDL_Init(SDL_INIT_VIDEO);
  gWindow =
      SDL_CreateWindow("SDL", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,
                       width, height, SDL_WINDOW_SHOWN);

  gRender = SDL_CreateRenderer(
      gWindow, -1, SDL_RENDERER_SOFTWARE | SDL_RENDERER_TARGETTEXTURE);

  gTexture = SDL_CreateTexture(gRender, SDL_PIXELFORMAT_RGBA8888,
                               SDL_TEXTUREACCESS_STREAMING, width, height);
}

void close() {
  SDL_DestroyRenderer(gRender);
  SDL_DestroyTexture(gTexture);
  SDL_DestroyWindow(gWindow);
  SDL_Quit();
}

void Clear(U32 *pix, U32 color) {
  for (S32 i = 0; i < height; ++i) {
    for (S32 j = 0; j < width; ++j) {
      pix[i * width + j] = color;
    }
  }
}

void put_pixel(U32 *pix, U32 color, S32 x, S32 y) {
  pix[(height - y - 1) * width + x] = color;
}

void Line(S32 x0, S32 y0, S32 x1, S32 y1, U32 *pix, U32 color) {
  bool steep = false;
  if (FW::abs(x0 - x1) < FW::abs(y0 - y1)) {
    FW::swap(x0, y0);
    FW::swap(x1, y1);
    steep = true;
  }
  if (x0 > x1) {
    FW::swap(x0, x1);
    FW::swap(y0, y1);
  }

  for (S32 x = x0; x <= x1; x++) {
    F32 t = (x - x0) / (F32) (x1 - x0);
    S32 y = y0 * (1.f - t) + y1 * t;
    if (steep) {
      put_pixel(pix, color, y, x);
    } else {
      put_pixel(pix, color, x, y);
    }
  }
}

void Triangle(Vec2i t0, Vec2i t1, Vec2i t2, U32 *pix, U32 color) {
  if (t0.y == t1.y && t0.y == t2.y) return; // i dont care about degenerate triangles
  if (t0.y > t1.y) FW::swap(t0, t1);
  if (t0.y > t2.y) FW::swap(t0, t2);
  if (t1.y > t2.y) FW::swap(t1, t2);
  S32 total_height = t2.y - t0.y;
  for (S32 i = 0; i < total_height; i++) {
    bool second_half = i > t1.y - t0.y || t1.y == t0.y;
    S32 segment_height = second_half ? t2.y - t1.y : t1.y - t0.y;
    F32 alpha = (F32) i / total_height;
    F32 beta = (F32) (i - (second_half ? t1.y - t0.y : 0))
        / segment_height; // be careful: with above conditions no division by zero here
    Vec2i A = t0 + ((t2 - t0) * alpha);
    Vec2i B = second_half ? t1 + ((t2 - t1) * beta) : t0 + ((t1 - t0) *
        beta);
    if (A.x > B.x) FW::swap(A, B);
    for (S32 j = A.x; j <= B.x; j++) {
      put_pixel(pix, color, j, t0.y + i); // attention, due to S32 casts t0.y+i != A.y
    }
  }
}

Vec3f barycentric(Vec3i A, Vec3i B, Vec3i C, Vec3i P) {
  Vec3i s[2];
  for (S32 i = 2; i--;) {
    s[i][0] = C[i] - A[i];
    s[i][1] = B[i] - A[i];
    s[i][2] = A[i] - P[i];
  }
  Vec3f u = FW::cross((Vec3f)s[0], (Vec3f)s[1]);
  if (FW::abs(u[2]) != 0) // dont forget that u[2] is integer. If it is zero then triangle ABC is degenerate
  {
    return Vec3f(1.f - (u.x + u.y) / u.z, u.y / u.z, u.x / u.z);
  }
  return Vec3f(-1, 1, 1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
}

F32 Det(Vec3f A, Vec3f B, Vec3f C) {
  return (B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x);
}

void TriangleFast2(Vec3f *pts, S32 *zbuffer, U32 *pix, U32 *colors) {
  Vec2i bboxmin(FW_S32_MAX, FW_S32_MAX);
  Vec2i bboxmax(FW_S32_MIN, FW_S32_MIN);
  Vec2i clamp(width - 1, height - 1);

  for (S32 i = 0; i < 3; i++) {
    for (S32 j = 0; j < 2; j++) {
      bboxmin[j] = FW::max(0, FW::min(bboxmin[j], (S32) (pts[i][j] + 0.5)));
      bboxmax[j] = FW::min(clamp[j], FW::max(bboxmax[j], (S32) (pts[i][j] + 0.5)));
    }
  }

  U8 r[3], g[3], b[3], a[3];
  for (S32 i = 0; i < 3; i++) {
    SDL_GetRGBA(colors[i], pixFormat, &r[i], &g[i], &b[i], &a[i]);
  }

  F32 z_inv_0 = 1.0f / pts[0][2];
  F32 z_inv_1 = 1.0f / pts[1][2];
  F32 z_inv_2 = 1.0f / pts[2][2];

  F32 y_0 = pts[0][1];
  F32 y_1 = pts[1][1];
  F32 y_2 = pts[2][1];

  F32 x_0 = pts[0][0];
  F32 x_1 = pts[1][0];
  F32 x_2 = pts[2][0];

  Vec2f n_pq(pts[0].y - pts[1].y, pts[1].x - pts[0].x);
  Vec2f n_qr(pts[1].y - pts[2].y, pts[2].x - pts[1].x);
  Vec2f n_rp(pts[2].y - pts[0].y, pts[0].x - pts[2].x);
  Vec2f v_p = pts[0].getXY();
  Vec2f v_q = pts[1].getXY();
  Vec2f v_r = pts[2].getXY();

  F32 c_pq = -1.f * (n_pq.dot(v_p));
  F32 c_qr = -1.f * (n_qr.dot(v_q));
  F32 c_rp = -1.f * (n_rp.dot(v_r));

  F32 Den = Det(pts[0], pts[1], pts[2]);

  F32 D_Zpinv_x = ((y_2 - y_0) * (z_inv_1 - z_inv_0) + (y_0 - y_1) * (z_inv_2 - z_inv_0)) / Den;
  F32 D_Zpinv_y = ((x_0 - x_2) * (z_inv_1 - z_inv_0) + (x_1 - x_0) * (z_inv_2 - z_inv_0)) / Den;

  F32 D_Zpinv_x_r =
      ((y_2 - y_0) * (z_inv_1 * r[1] - z_inv_0 * r[0]) + (y_0 - y_1) * (z_inv_2 * r[2] - z_inv_0 * r[0])) / Den;
  F32 D_Zpinv_y_r =
      ((x_0 - x_2) * (z_inv_1 * r[1] - z_inv_0 * r[0]) + (x_1 - x_0) * (z_inv_2 * r[2] - z_inv_0 * r[0])) / Den;

  F32 D_Zpinv_x_g =
      ((y_2 - y_0) * (z_inv_1 * g[1] - z_inv_0 * g[0]) + (y_0 - y_1) * (z_inv_2 * g[2] - z_inv_0 * g[0])) / Den;
  F32 D_Zpinv_y_g =
      ((x_0 - x_2) * (z_inv_1 * g[1] - z_inv_0 * g[0]) + (x_1 - x_0) * (z_inv_2 * g[2] - z_inv_0 * g[0])) / Den;

  F32 D_Zpinv_x_b =
      ((y_2 - y_0) * (z_inv_1 * b[1] - z_inv_0 * b[0]) + (y_0 - y_1) * (z_inv_2 * b[2] - z_inv_0 * b[0])) / Den;
  F32 D_Zpinv_y_b =
      ((x_0 - x_2) * (z_inv_1 * b[1] - z_inv_0 * b[0]) + (x_1 - x_0) * (z_inv_2 * b[2] - z_inv_0 * b[0])) / Den;

  F32 z_p;
  F32 z_inv_p;

  z_inv_p = z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x + (bboxmin.y - y_0) * D_Zpinv_y;
  z_p = 1.0f / z_inv_p;
  F32 r_i_p = (r[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_r + (bboxmin.y - y_0) * D_Zpinv_y_r);
  F32 g_i_p = (g[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_g + (bboxmin.y - y_0) * D_Zpinv_y_g);
  F32 b_i_p = (b[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_b + (bboxmin.y - y_0) * D_Zpinv_y_b);

  Vec3i P;
  for (P.x = bboxmin.x; P.x <= bboxmax.x; P.x++) {
    F32 z_inv_p_base = z_inv_p;
    F32 r_i_p_base = r_i_p;
    F32 g_i_p_base = g_i_p;
    F32 b_i_p_base = b_i_p;
    z_inv_p += D_Zpinv_x;
    r_i_p += D_Zpinv_x_r;
    g_i_p += D_Zpinv_x_g;
    b_i_p += D_Zpinv_x_b;
    for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) {
      P.z = 0;

      z_p = 1.0f / z_inv_p_base;
      P.z = z_p;
      U32 color = SDL_MapRGBA(pixFormat, r_i_p_base * z_p, g_i_p_base * z_p, b_i_p_base * z_p, 255);

      z_inv_p_base += D_Zpinv_y;
      r_i_p_base += D_Zpinv_y_r;
      g_i_p_base += D_Zpinv_y_g;
      b_i_p_base += D_Zpinv_y_b;

      Vec2f s = P.getXY();
      F32 e_pq_s = n_pq.dot(s) + c_pq;
      if (e_pq_s >= 0) {
        F32 e_qr_s = n_qr.dot(s) + c_qr;
        if (e_qr_s >= 0) {
          F32 e_rp_s = n_rp.dot(s) + c_rp;
          if (e_rp_s < 0)
            continue;
        } else continue;
      } else continue;

      S32 idx = P.x + P.y * width;
      if (zbuffer[idx] < P.z) {
        zbuffer[idx] = P.z;
        put_pixel(pix, color, P.x, P.y);
      }
    }
  }
}
#if 0
void Triangle(Vec3i *pts, S32 *zbuffer, U32 *pix, U32 *colors) {
  Vec2i bboxmin(FW::numeric_limits<S32>::max, FW::numeric_limits<S32>::max);
  Vec2i bboxmax(-FW::numeric_limits<S32>::max, -FW::numeric_limits<S32>::max);
  Vec2i clamp(width - 1, height - 1);
  for (S32 i = 0; i < 3; i++) {
    for (S32 j = 0; j < 2; j++) {
      bboxmin[j] = FW::max(0, FW::min(bboxmin[j], pts[i][j]));
      bboxmax[j] = FW::min(clamp[j], FW::max(bboxmax[j], pts[i][j]));
    }
  }
  Vec3i P;
  F32 z_inv;
  for (P.x = bboxmin.x; P.x <= bboxmax.x; P.x++) {
    for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) {
      Vec3f bc_screen = barycentric(pts[0], pts[1], pts[2], P);
      if (bc_screen.x < 0 || bc_screen.y < 0 || bc_screen.z < 0) continue;
      P.z = 0;
      z_inv = 0;
      for (S32 i = 0; i < 3; i++) z_inv += 1.0 / pts[i][2] * bc_screen[i];
      U8 r_add = 0, g_add = 0, b_add = 0, a_add = 0;
      {
        U8 r, g, b, a;
        for (S32 i = 0; i < 3; i++) {
          SDL_GetRGBA(colors[i], pixFormat, &r, &g, &b, &a);
          F32 pfx = bc_screen[i] / pts[i][2] / z_inv;
          r_add += r * pfx;
          g_add += g * pfx;
          b_add += b * pfx;
          a_add += a * pfx;
        }
      }
      P.z = 1.0 / z_inv;
      U32 color = SDL_MapRGBA(pixFormat, r_add, g_add, b_add, a_add);
      S32 idx = P.x + P.y * width;
      if (zbuffer[idx] < P.z) {
        zbuffer[idx] = P.z;
        put_pixel(pix, color, P.x, P.y);
      }
    }
  }
}
#endif // 0

//template<class A, class B>
//inline A lerp(const A &a, const A &b, const B &t) { return (A) (a * ((B) 1 - t) + b * t); }

Vec3i setupPleq(const Vec3f& values, const Vec2i& v0, const Vec2i& d1, const Vec2i& d2, S32 area, int samplesLog2)
{
  F64 t0 = (F64)values.x;
  F64 t1 = (F64)values.y - t0;
  F64 t2 = (F64)values.z - t0;
  F64 xc = (t1 * (F64)d2.y - t2 * (F64)d1.y) / (F64)area;
  F64 yc = (t2 * (F64)d1.x - t1 * (F64)d2.x) / (F64)area;

  Vec2i center = (v0 * 2 + min(d1.x, d2.x, 0) + max(d1.x, d2.x, 0)) >> (CR_SUBPIXEL_LOG2 - samplesLog2 + 1);
  Vec2i vc = v0 - (center << (CR_SUBPIXEL_LOG2 - samplesLog2));

  Vec3i pleq;
  pleq.x = (U32)(S64)FW::floor(xc * exp2(CR_SUBPIXEL_LOG2 - samplesLog2) + 0.5);
  pleq.y = (U32)(S64)FW::floor(yc * exp2(CR_SUBPIXEL_LOG2 - samplesLog2) + 0.5);
  pleq.z = (U32)(S64)FW::floor(t0 - xc * (F64)vc.x - yc * (F64)vc.y + 0.5);
  pleq.z -= pleq.x * center.x + pleq.y * center.y;
  return pleq;
}

bool setupTriangle(Vec3f *pts) {

  Vec3f v0 = pts[0];
  Vec3f v1 = pts[1];
  Vec3f v2 = pts[2];

  Vec2i p0 = Vec2i(v0.x, v0.y);
  Vec2i p1 = Vec2i(v1.x, v1.y);
  Vec2i p2 = Vec2i(v2.x, v2.y);
  Vec2i d1 = p1 - p0;
  Vec2i d2 = p2 - p0;

  S32 area = d1.x * d2.y - d1.y * d2.x;

  if (area <= 0)
    return false;

  Vec3f zvert = FW::lerp(Vec3f(CR_DEPTH_MIN), Vec3f(CR_DEPTH_MIN), Vec3f(v0.z, v1.z, v2.z)* 0.5f + 0.5f);
//  zvert.print();
  return true;
}

void TriangleFast3(Vec3f *pts, S32 *zbuffer, U32 *pix, U32 *colors) {
  Vec2i bboxmin(FW_S32_MAX, FW_S32_MAX);
  Vec2i bboxmax(FW_S32_MIN, FW_S32_MIN);
  Vec2i clamp(width - 1, height - 1);

  for (S32 i = 0; i < 3; i++) {
    for (S32 j = 0; j < 2; j++) {
      bboxmin[j] = FW::max(0, FW::min(bboxmin[j], (S32) (pts[i][j] + 0.5)));
      bboxmax[j] = FW::min(clamp[j], FW::max(bboxmax[j], (S32) (pts[i][j] + 0.5)));
    }
  }

  U8 r[3], g[3], b[3], a[3];
  for (S32 i = 0; i < 3; i++) {
    SDL_GetRGBA(colors[i], pixFormat, &r[i], &g[i], &b[i], &a[i]);
  }

  F32 z_inv_0 = 1.0f / pts[0][2];
  F32 z_inv_1 = 1.0f / pts[1][2];
  F32 z_inv_2 = 1.0f / pts[2][2];

  F32 y_0 = pts[0][1];
  F32 y_1 = pts[1][1];
  F32 y_2 = pts[2][1];

  F32 x_0 = pts[0][0];
  F32 x_1 = pts[1][0];
  F32 x_2 = pts[2][0];

  Vec2f n_pq(pts[0].y - pts[1].y, pts[1].x - pts[0].x);
  Vec2f n_qr(pts[1].y - pts[2].y, pts[2].x - pts[1].x);
  Vec2f n_rp(pts[2].y - pts[0].y, pts[0].x - pts[2].x);
  Vec2f v_p = pts[0].getXY();
  Vec2f v_q = pts[1].getXY();
  Vec2f v_r = pts[2].getXY();

  F32 c_pq = -1.f * (n_pq.dot(v_p));
  F32 c_qr = -1.f * (n_qr.dot(v_q));
  F32 c_rp = -1.f * (n_rp.dot(v_r));
  setupTriangle(pts);
  F32 Den = Det(pts[0], pts[1], pts[2]);
  F32 Den_1 = 1.0f / Den;

  F32 D_Zpinv_x = ((y_2 - y_0) * (z_inv_1 - z_inv_0) + (y_0 - y_1) * (z_inv_2 - z_inv_0)) * Den_1;
  F32 D_Zpinv_y = ((x_0 - x_2) * (z_inv_1 - z_inv_0) + (x_1 - x_0) * (z_inv_2 - z_inv_0)) * Den_1;

  F32 D_Zpinv_x_r =
      ((y_2 - y_0) * (z_inv_1 * r[1] - z_inv_0 * r[0]) + (y_0 - y_1) * (z_inv_2 * r[2] - z_inv_0 * r[0])) * Den_1;
  F32 D_Zpinv_y_r =
      ((x_0 - x_2) * (z_inv_1 * r[1] - z_inv_0 * r[0]) + (x_1 - x_0) * (z_inv_2 * r[2] - z_inv_0 * r[0])) * Den_1;

  F32 D_Zpinv_x_g =
      ((y_2 - y_0) * (z_inv_1 * g[1] - z_inv_0 * g[0]) + (y_0 - y_1) * (z_inv_2 * g[2] - z_inv_0 * g[0])) * Den_1;
  F32 D_Zpinv_y_g =
      ((x_0 - x_2) * (z_inv_1 * g[1] - z_inv_0 * g[0]) + (x_1 - x_0) * (z_inv_2 * g[2] - z_inv_0 * g[0])) * Den_1;

  F32 D_Zpinv_x_b =
      ((y_2 - y_0) * (z_inv_1 * b[1] - z_inv_0 * b[0]) + (y_0 - y_1) * (z_inv_2 * b[2] - z_inv_0 * b[0])) * Den_1;
  F32 D_Zpinv_y_b =
      ((x_0 - x_2) * (z_inv_1 * b[1] - z_inv_0 * b[0]) + (x_1 - x_0) * (z_inv_2 * b[2] - z_inv_0 * b[0])) * Den_1;

  F32 z_p;
  F32 z_inv_p;

  z_inv_p = z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x + (bboxmin.y - y_0) * D_Zpinv_y;
  z_p = 1.0 / z_inv_p;
  F32 r_i_p = (r[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_r + (bboxmin.y - y_0) * D_Zpinv_y_r);
  F32 g_i_p = (g[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_g + (bboxmin.y - y_0) * D_Zpinv_y_g);
  F32 b_i_p = (b[0] * z_inv_0 + (bboxmin.x - x_0) * D_Zpinv_x_b + (bboxmin.y - y_0) * D_Zpinv_y_b);

  Vec3i P;
  for (P.x = bboxmin.x; P.x <= bboxmax.x; P.x++) {
    F32 z_inv_p_base = z_inv_p;
    F32 r_i_p_base = r_i_p;
    F32 g_i_p_base = g_i_p;
    F32 b_i_p_base = b_i_p;

    z_inv_p += D_Zpinv_x;
    r_i_p += D_Zpinv_x_r;
    g_i_p += D_Zpinv_x_g;
    b_i_p += D_Zpinv_x_b;

    for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) {
      P.z = 0;

      z_p = 1.0 / z_inv_p_base;
      P.z = z_p;
      U32 color = SDL_MapRGBA(pixFormat, r_i_p_base * z_p, g_i_p_base * z_p, b_i_p_base * z_p, 255);

      z_inv_p_base += D_Zpinv_y;
      r_i_p_base += D_Zpinv_y_r;
      g_i_p_base += D_Zpinv_y_g;
      b_i_p_base += D_Zpinv_y_b;

      Vec2f s = P.getXY();
      F32 e_pq_s = n_pq.dot(s) + c_pq;
      if (e_pq_s >= 0) {
        F32 e_qr_s = n_qr.dot(s) + c_qr;
        if (e_qr_s >= 0) {
          F32 e_rp_s = n_rp.dot(s) + c_rp;
          if (e_rp_s < 0)
            continue;
        } else continue;
      } else continue;

      S32 idx = P.x + P.y * width;
      if (zbuffer[idx] < P.z) {
        zbuffer[idx] = P.z;
        put_pixel(pix, color, P.x, P.y);
      }
    }
  }
}

template<typename T>
class Device {
 public:
  void sr_glViewport(GLint x,
                     GLint y,
                     GLsizei width,
                     GLsizei height) {
    viewPort.m_x = x;
    viewPort.m_y = y;
    viewPort.m_width = width;
    viewPort.m_height = height;
  }

  void sr_glDepthRange(T nearVal,
                       T farVal) {
    viewPort.m_nearVal = nearVal;
    viewPort.m_farVal = farVal;
  }
 private:
  struct ViewPort {
    GLint m_x = 0;
    GLint m_y = 0;
    GLsizei m_width = 1;
    GLsizei m_height = 1;
    T m_nearVal = 0.0;
    T m_farVal = 1.0;
  };
 protected:
  ViewPort viewPort;
};

Mat4f viewport(S32 x, S32 y, S32 w, S32 h) {
  Mat4f m;
  m.setIdentity();
  m(0, 3) = x + w / 2.f;
  m(1, 3) = y + h / 2.f;
  m(2, 3) = depth / 2.f;

  m(0, 0) = w / 2.f;
  m(1, 1) = h / 2.f;
  m(2, 2) = depth / 2.f;
  return m;
}

Vec3f world2screen(Vec3f v) {
  return Vec3f(S32((v.x + 1.f) * width / 2.f + .5f), S32((v.y + 1.f) * height / 2.f + .5f), v.z);
}

Mat4f lookat(Vec3f eye, Vec3f center, Vec3f up) {
  Vec3f z = (eye - center).normalized();
  z.normalize();
  Vec3f x = cross(up,z);
  x.normalize();
  Vec3f y = z.cross(x);
  y.normalize();
  Mat4f res;
  res.setIdentity();
  for (S32 i = 0; i < 3; i++) {
    res(0, i) = x[i];
    res(1, i) = y[i];
    res(2, i) = z[i];
    res(i, 3) = -center[i];
  }
  return res;
}

S32 main(S32 argc, char **argv) {
  if (2 == argc) {
    model = new Model(argv[1]);
  } else {
#if defined __linux__ || defined __APPLE__
    model = new Model("../../obj/african_head.obj");
#else
    model = new Model("../obj/african_head.obj");
#endif
  }

//add device
  Device<GLfloat> m_device;
  m_device.sr_glViewport(0, 0, 800, 800);
  m_device.sr_glDepthRange(0, 1);

  S32 *zbuffer = new S32[width * height];
  light_dir.normalize();
  void *pix;
  S32 pitch;

  init();

  S32 x_current(0);
  S32 y_current(0);

  // Main loop flag
  bool quit = false;

  // Event handler
  SDL_Event e;

  F32 deltaX(1), deltaY(1), deltaZ(3);

  eye = Vec3f( deltaX, deltaY, deltaZ);

  // timer
  clock_t start(0), finish(0);
  double duration;
  S32 frame_count(0);
  // While application is running
  while (!quit) {
    PROFILE_START_FRAME;
    PROFILE_BEGIN("FRAME");
    PROFILE_BEGIN("Prepare");
    frame_count++;
    if (frame_count == 60) {
      finish = clock();
      duration = (finish - start) / CLOCKS_PER_SEC;
      FW::printf("%f  FPS\n", 60 / duration);
      start = clock();
      frame_count = 0;
    }
    // Handle events on queue
    while (SDL_PollEvent(&e) != 0) {
      // User requests quit
      if (e.type == SDL_QUIT || e.key.keysym.sym == SDLK_ESCAPE) {
        quit = true;
      }
      if (e.key.keysym.sym == SDLK_UP) {
        deltaY += 0.2f;
        break;
      }
      if (e.key.keysym.sym == SDLK_DOWN) {
        deltaY -= 0.2f;
        break;
      }
      if (e.key.keysym.sym == SDLK_RIGHT) {
        deltaX += 0.2f;
        break;
      }
      if (e.key.keysym.sym == SDLK_LEFT) {
        deltaX -= 0.2f;
        break;
      }
      if (e.key.keysym.sym == SDLK_w) {
        deltaZ -= 0.2f;
        break;
      }
      if (e.key.keysym.sym == SDLK_s) {
        deltaZ += 0.2f;
        break;
      }
      if (e.type == SDL_MOUSEMOTION || e.button.button == SDL_BUTTON_LEFT) {
        x_current = e.motion.x;
        y_current = e.motion.y;
      }
    }

    SDL_LockTexture(gTexture, NULL, &pix, &pitch);
    // Render begin

    // Clear color
    Clear((U32 *) pix, BLACK);

    // Eye and center position
    eye = Vec3f(deltaX, deltaY, deltaZ);

    Mat4f ModelView = lookat(eye, center, Vec3f(0, 1, 0));
    Mat4f Projection;
    Projection.setIdentity();
    Mat4f ViewPort = viewport(width / 8, height / 8, width * 3 / 4, height * 3 / 4);
    Projection(3, 2) = -1.f / (eye - center).length();
    Mat4f z = ViewPort * Projection * ModelView;

    for (S32 i = 0; i < width * height; i++) {
      zbuffer[i] = std::numeric_limits<S32>::min();
    }
    PROFILE_END();
    PROFILE_BEGIN("Render face");
    // Loop over shapes

    for (S32 i = 0; i < model->nfaces(); i++) {
      std::vector<tinyobj::index_t> face = model->m_faces[i];
      Vec3f screen_coords[3];
      Vec3f world_coords[3];
      U32 colors[3];
      Vec3f n_v[3];
      F32 intensity[3];

      // load texture data
      for (S32 j = 0; j < 3; ++j) {
        tinyobj::index_t _idx = face[j];
        Vec3f v = model->vert(_idx);
        n_v[j] = model->normal(_idx);

        Vec4f v_(v,1);
//        v_ << v, 1;
        Vec4f v_temp;
        v_temp = ViewPort * Projection * ModelView * v_;
        v_temp = v_temp / v_temp.w;
        screen_coords[j] = (v_temp).getXYZ();

        world_coords[j] = v;
        intensity[j] = fabs(n_v[j].dot(light_dir));

    // Render face
//    for (S32 i = 0; i < model->nfaces(); i++) {
//      std::vector<S32> face = model->face(i);
//      Vec3f screen_coords[3];
//      Vec3f world_coords[3];
//      U32 colors[3];
//      Vec3f n_v[3];
//      F32 intensity[3];
//
//      // load texture data
//      for (S32 j = 0; j < 3; ++j) {
//        Vec3f v = model->vert(face[j]);
//        n_v[j] = model->normal(i, j);
//
//        Vec4f v_(v,1);
////        v_ << v, 1;
//        Vec4f v_temp;
//        v_temp = ViewPort * Projection * ModelView * v_;
//        v_temp = v_temp / v_temp.w;
//        screen_coords[j] = (v_temp).getXYZ();
//
//        world_coords[j] = v;
//        intensity[j] = fabs(n_v[j].dot(light_dir));

#if  defined(WITH_TEXTURE)
        if (intensity[j] > 0) {
          colors[j] = SDL_MapRGBA(pixFormat,
                                  static_cast<U8>(model->diffuse(model->uv(_idx))[2] * intensity[j]),
                                  static_cast<U8>(model->diffuse(model->uv(_idx))[1] * intensity[j]),
                                  static_cast<U8>(model->diffuse(model->uv(_idx))[0] * intensity[j]),
                                  model->diffuse(model->uv(_idx))[3]);
        } else {
          colors[j] = SDL_MapRGBA(pixFormat,
                                  model->diffuse(model->uv(_idx))[2],
                                  model->diffuse(model->uv(_idx))[1],
                                  model->diffuse(model->uv(_idx))[0],
                                  model->diffuse(model->uv(_idx))[3]);
        }
#else
		if (intensity[j] > 0) {
			colors[j] = SDL_MapRGBA(pixFormat,
				static_cast<U8>(255.0 * intensity[j]),
				static_cast<U8>(255.0 * intensity[j]),
				static_cast<U8>(255.0 * intensity[j]),
				255);
		}
#endif

      }

      // back face culling
      Vec3f AB = screen_coords[0] - screen_coords[1];
      Vec3f AC = screen_coords[0] - screen_coords[2];
      Vec3f N = AC.cross(AB);
      if (N.z > 0) {
        continue;
      }

      // tessellation
//      Vec3i screen_coords_i[3];
//      screen_coords_i[0] = screen_coords[0];
//      screen_coords_i[1] = screen_coords[1];
//      screen_coords_i[2] = screen_coords[2];
      TriangleFast3(screen_coords, zbuffer, (U32 *) pix, colors);
    }
    PROFILE_END();
//    Vec3f world_coords[3];
//    world_coords[0] << 0, 0, 0.99;
//    world_coords[1] << 1, 0.5, 0.99;
//    world_coords[2] << 0.5, 1, -0.99;
//
//    Vec3f screen_coords[3];
//
//    U32 colors[3];
//    colors[0] = RED;
//    colors[1] = GREEN;
//    colors[2] = BLUE;
//    for (S32 j = 0; j < 3; ++j) {
//      Vec4f v_;
//      v_ << world_coords[j], 1;
//      Vec4f v_temp;
//      v_temp = ViewPort * Projection * ModelView * v_;
//      v_temp = v_temp / v_temp.w;
//      screen_coords[j] = v_temp.block(0, 0, 3, 1);
//    }

    // tessellation
//    TriangleFast2(screen_coords, zbuffer, (U32 *) pix, colors);
    PROFILE_BEGIN("Render present");
    // Render end
    SDL_UnlockTexture(gTexture);
    SDL_RenderCopy(gRender, gTexture, NULL, NULL);
    SDL_RenderPresent(gRender);
    PROFILE_END();
    PROFILE_END();
    cout << ofxProfiler::getResults();

#if defined __linux__ || defined __APPLE__
    usleep(10);
#else
    Sleep(1);
#endif //

  }

  { // dump z-buffer (debugging purposes only)
    TGAImage zbimage(width, height, TGAImage::GRAYSCALE);
    for (S32 i = 0; i < width; i++) {
      for (S32 j = 0; j < height; j++) {
        unsigned char zzzz = zbuffer[i + j * width];
        zbimage.set(i, j, TGAColor(&zzzz, 1));
      }
    }
    zbimage.flip_vertically(); // i want to have the origin at the left bottom corner of the image
    zbimage.write_tga_file("zbuffer.tga");
  }

  close();
  return 0;
}


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

#include <x/vio/range_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

RangeUpdate::RangeUpdate(const x::RangeMeasurement& range_meas,
                         const std::vector<int>& tr_feat_ids,
                         const x::AttitudeList& quats,
                         const x::TranslationList& poss,
                         const MatrixXd& feature_states,
                         const std::vector<int>& anchor_idxs,
                         const MatrixXd& cov_s,
                         const int n_poses_max,
                         const double sigma_range)
{
  // Initialize Kalman update matrices
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(1, cols);
  cov_m_diag_ = VectorXd::Ones(1);
  res_ = MatrixXd::Zero(1, 1);

  // Compute residual, Jacobian and covariance block
  processRangedFacet(range_meas,
                     tr_feat_ids,
                     quats,
                     poss,
                     feature_states,
                     anchor_idxs,
                     cov_s,
                     n_poses_max,
                     sigma_range);
  /*processRangedFeature(range_meas,
                         quats,
                         poss,
                         feature_states,
                         P,
                         n_poses_max,
                         sigma_range,
                         0);*/
}

void RangeUpdate::processRangedFacet(const x::RangeMeasurement& range_meas,
                                     const std::vector<int>& tr_feat_ids,
                                     const x::AttitudeList& C_q_G,
                                     const x::TranslationList& G_p_C,
                                     const MatrixXd& feature_states,
                                     const std::vector<int>& anchor_idxs,
                                     const MatrixXd& P,
                                     const int n_poses_max,
                                     const double sigma_range)
{
  /********************************************************
   * Cartesian world coordinates of the triangle features *
   ********************************************************/

  // Array storing the  of the three features
  std::array<Vector3d, 3> G_p_fj; // world position
  std::array<Vector3d, 3> G_p_Ca; // anchor position
  std::array<x::Quaternion, 3> Ca_q_G; // anchor quaternion
  std::array<int, 3> anchor_idx; // anchor quaternion
  std::array<double, 3> alpha; // Normalized undistored image coordinates X
  std::array<double, 3> beta; // Normalized undistored image coordinates Y
  std::array<double, 3> rho; // Inverse depth

  for(unsigned int j=0; j<3; j++)
  {
    // Inverse-depth parameters in last observation frame
    alpha[j] = feature_states(tr_feat_ids[j] * 3, 0);
    beta[j] = feature_states(tr_feat_ids[j] * 3 + 1, 0);
    rho[j] = feature_states(tr_feat_ids[j] * 3 + 2, 0);

    // Anchor pose
    anchor_idx[j] = anchor_idxs[tr_feat_ids[j]];
    Ca_q_G[j].x() = C_q_G[anchor_idx[j]].ax;
    Ca_q_G[j].y() = C_q_G[anchor_idx[j]].ay;
    Ca_q_G[j].z() = C_q_G[anchor_idx[j]].az;
    Ca_q_G[j].w() = C_q_G[anchor_idx[j]].aw;

    const Vector3d anchor_pos(G_p_C[anchor_idx[j]].tx, G_p_C[anchor_idx[j]].ty, G_p_C[anchor_idx[j]].tz);
    G_p_Ca[j] = anchor_pos;

    // Coordinate of feature in global frame
    G_p_fj[j] = 1 / (rho[j])*Ca_q_G[j].normalized().toRotationMatrix() * Vector3d(alpha[j], beta[j], 1) + G_p_Ca[j];
  }

  /***********************
   * Current camera pose *
   ***********************/

  // Orientation
  x::Attitude Cn_q_G(C_q_G.back());
  x::Quatern attitude_to_quaternion;
  x::Quaternion Ci_q_G  = attitude_to_quaternion(Cn_q_G);

  // Position
  x::Translation G_p_Cn(G_p_C.back());
  Vector3d G_p_Ci(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz);

  /************
   * Residual *
   ************/

  // Compute normal vector to the triangle plane in world coordinates
  const Vector3d G_n = (G_p_fj[0] - G_p_fj[1]).cross(G_p_fj[2] - G_p_fj[1]);

  // Undistorted normalized homogeneous 2D coordinates of the LRF impact point on the ground
  const Vector3d lrf_img_pt_nh(range_meas.img_pt_n.getX(), range_meas.img_pt_n.getY(), 1.0);

  // Measurement prediction
  const double a = (G_p_fj[1] - G_p_Ci).dot(G_n);
  const double b = lrf_img_pt_nh.dot(Ci_q_G.normalized().toRotationMatrix().transpose() * G_n);
  double range_hat = a/b;

  // Measurement
  const double range(range_meas.range);

  // Actual residual
  MatrixXd res_j(MatrixXd::Zero(1, 1));
  res_j(0, 0) = range - range_hat;

  /*******************************
   * Measurement Jacobian matrix *
   *******************************/

  const size_t cols = P.cols();
  MatrixXd h_j(MatrixXd::Zero(1, cols));

  // Camera Position
  const RangeJacobian J_pc = - 1.0 / b * G_n.transpose();

  // Camera attitude
  const RangeJacobian J_qc = a / std::pow(b, 2.0)
    * G_n.transpose() * Ci_q_G.normalized().toRotationMatrix()
    * x::Skew(lrf_img_pt_nh(0), lrf_img_pt_nh(1), lrf_img_pt_nh(2)).matrix;

  // Position of the LRF target point in world frame
  const Vector3d G_p_r = a / b * Ci_q_G.normalized().toRotationMatrix() * lrf_img_pt_nh + G_p_Ci;

  // Barycenter
  const Vector3d G_p_bary = 1.0/3.0 * (G_p_fj[0]+G_p_fj[1]+G_p_fj[2]);
  /* Triangle feature 1 */
  // Jacobian wrt cartesian feature coordinates
  const RangeJacobian J_f0 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[2] - G_p_fj[1]).cross(G_p_bary - G_p_r)).transpose();
  // Anchor position
  const RangeJacobian J_pc0 = J_f0;
  // Anchor attitude
  const RangeJacobian J_qc0 = - 1.0 / rho[0] * J_f0 * Ca_q_G[0].normalized().toRotationMatrix()
    * x::Skew(alpha[0], beta[0], 1.0).matrix;
  // Inverse-depth feature coordinates
  MatrixXd mat(MatrixXd::Identity(3, 3));
  mat(0, 2) = - alpha[0] / rho[0];
  mat(1, 2) = - beta[0] / rho[0];
  mat(2, 2) =  - 1 / rho[0];
  RangeJacobian J_fi0 = 1 / rho[0] * J_f0 * Ca_q_G[0].normalized().toRotationMatrix() * mat;

  /* Triangle feature 2 */
  // Jacobian wrt cartesian feature coordinates
  const RangeJacobian J_f1 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[0] - G_p_fj[2]).cross(G_p_bary - G_p_r)).transpose();
  // Anchor position
  const RangeJacobian J_pc1 = J_f1;
  // Anchor attitude
  const RangeJacobian J_qc1 = - 1.0 / rho[1] * J_f1 * Ca_q_G[1].normalized().toRotationMatrix()
    * x::Skew(alpha[1], beta[1], 1.0).matrix;
  // Inverse-depth feature coordinates
  mat = MatrixXd::Identity(3, 3);
  mat(0, 2) = - alpha[1] / rho[1];
  mat(1, 2) = - beta[1] / rho[1];
  mat(2, 2) = - 1 / rho[1];
  RangeJacobian J_fi1 = 1 / rho[1] * J_f1 * Ca_q_G[1].normalized().toRotationMatrix() * mat;

  // Triangle feature 3
  // Jacobian wrt cartesian feature coordinates
  const RangeJacobian J_f2 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[1] - G_p_fj[0]).cross(G_p_bary - G_p_r)).transpose();
  // Anchor position
  const RangeJacobian J_pc2 = J_f2;
  // Anchor attitude
  const RangeJacobian J_qc2 = - 1.0 / rho[2] * J_f2 * Ca_q_G[2].normalized().toRotationMatrix()
    * x::Skew(alpha[2], beta[2], 1.0).matrix;
  // Inverse-depth feature coordinates
  mat = MatrixXd::Identity(3, 3);
  mat(0, 2) = - alpha[2] / rho[2];
  mat(1, 2) = - beta[2] / rho[2];
  mat(2, 2) = - 1 / rho[2];
  RangeJacobian J_fi2 = 1 / rho[2] * J_f2 * Ca_q_G[2].normalized().toRotationMatrix() * mat;

  // Update stacked Jacobian matrices associated to the current feature
  const unsigned int row = 0;
  const unsigned int pos = C_q_G.size()- 1;
  unsigned int col = pos * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_pc;

  col += n_poses_max * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_qc;

  col = anchor_idx[0] * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc0;
  col += n_poses_max * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc0;
  col = (n_poses_max * 2 + tr_feat_ids[0]) * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi0;

  col = anchor_idx[1] * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc1;
  col += n_poses_max * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc1;
  col = (n_poses_max * 2 + tr_feat_ids[1]) * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi1;

  col = anchor_idx[2] * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc2;
  col += n_poses_max * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) =
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc2;
  col = (n_poses_max * 2 + tr_feat_ids[2]) * kJacCols;
  h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi2;

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  VectorXd r_j(1);
  const double var_range = sigma_range * sigma_range;
  r_j << var_range;
  MatrixXd S_inv = (h_j * P * h_j.transpose() + r_j).inverse();
  MatrixXd gamma = res_j.transpose() * S_inv * res_j;
  boost::math::chi_squared_distribution<> my_chisqr(1);
  double chi = quantile(my_chisqr, 0.9);  // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
    jac_.block(0,               // startRow
             0,               // startCol
             1,               // numRows
             cols) = h_j;  // numCols

    // Residual vector
    res_.block(0, 0, 1, 1) = res_j;

    // Measurement covariance matrix
    cov_m_diag_ = r_j;
  }
}

void RangeUpdate::processRangedFeature(const x::RangeMeasurement& range_meas,
                                       const x::AttitudeList& C_q_G,
                                       const x::TranslationList& G_p_C,
                                       const MatrixXd& feature_states,
                                       const std::vector<int>& anchor_idxs,
                                       const MatrixXd& P,
                                       const int n_poses_max,
                                       const double sigma_range,
                                       const size_t& j)
{
  const size_t cols = P.cols();
  MatrixXd h_j(MatrixXd::Zero(1, cols));
  MatrixXd res_j(MatrixXd::Zero(1, 1));

  //==========================================================================
  // Feature information
  //==========================================================================
  // A-priori inverse-depth parameters in last observation frame
  double alpha = feature_states(j * 3, 0);
  double beta = feature_states(j * 3 + 1, 0);
  double rho = feature_states(j * 3 + 2, 0);

  // Anchor pose
  unsigned int anchor_idx = anchor_idxs[j];
  x::Quaternion Ca_q_G;
  Ca_q_G.x() = C_q_G[anchor_idx].ax;
  Ca_q_G.y() = C_q_G[anchor_idx].ay;
  Ca_q_G.z() = C_q_G[anchor_idx].az;
  Ca_q_G.w() = C_q_G[anchor_idx].aw;

  Vector3d G_p_Ca(G_p_C[anchor_idx].tx, G_p_C[anchor_idx].ty, G_p_C[anchor_idx].tz);

  // Coordinate of feature in global frame
  Vector3d G_p_fj = 1 / (rho)*Ca_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Ca;

  // FOR LAST FEATURE OBSERVATION
  x::Translation G_p_Cn(G_p_C.back());
  x::Attitude Cn_q_G(C_q_G.back());

  x::Quatern attitude_to_quaternion;
  Quaterniond Ci_q_G_ = attitude_to_quaternion(Cn_q_G);
  Vector3d G_p_Ci_(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz);

  // Feature position expressed in camera frame.
  Vector3d Ci_p_fj;
  Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

  //==========
  // Residual
  //==========
  double range(range_meas.range);
  double range_hat(Ci_p_fj(2));
  res_j(0, 0) = range - range_hat;

  //============================
  // Measurement Jacobian matrix
  //============================
  const unsigned int pos = C_q_G.size() - 1;
  if (anchor_idx == pos)  // Handle special case
  {
    // Inverse-depth feature coordinates jacobian
    RangeJacobian mat(MatrixXd::Zero(1, 3));
    mat(0, 2) = -1 / std::pow(rho, 2);

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    unsigned int col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = mat;
  }
  else
  {
    // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
    RangeJacobian J_i(MatrixXd::Zero(1, 3));
    J_i(0, 2) = 1.0;

    // Attitude
    Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);
    RangeJacobian J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix;

    // Position
    RangeJacobian J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

    // Anchor attitude
    RangeJacobian J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                                         Ca_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix;

    // Anchor position
    RangeJacobian J_anchor_pos = -J_position;

    // Inverse-depth feature coordinates
    MatrixXd mat(MatrixXd::Identity(3, 3));
    mat(0, 2) = -alpha / rho;
    mat(1, 2) = -beta / rho;
    mat(2, 2) = -1 / rho;
    RangeJacobian Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                                  Ca_q_G.normalized().toRotationMatrix() * mat;

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    const unsigned int pos = C_q_G.size() - 1;
    unsigned int col = pos * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_position;

    col += n_poses_max * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_attitude;

    col = anchor_idx * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_anchor_pos;

    col += n_poses_max * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_anchor_att;

    col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = Hf_j1;
  }

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  VectorXd r_j(1);
  const double var_range = sigma_range * sigma_range;
  r_j << var_range;
  // MatrixXd S_inv = (h_j * P * h_j.transpose() + R_j).inverse();
  // MatrixXd gamma = res_j.transpose() * S_inv * res_j;
  // chi_squared_distribution<> my_chisqr(2 * features[j].size());
  // double chi = quantile(my_chisqr, 0.9);  // 95-th percentile

  if (true)  // gamma(0, 0) < chi)  // Inlier
  {
    jac_.block(0,               // startRow
             0,               // startCol
             1,               // numRows
             cols) = h_j;  // numCols

    // Residual vector
    res_.block(0, 0, 1, 1) = res_j;

    // Measurement covariance matrix
    cov_m_diag_ = r_j;
  }
}


////////////////////////////////////////////////////////////////////////////////
// Copyright 2019 FZI Research Center for Information Technology
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------------
/*!\file    JacobianTransposeSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/26
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/JacobianTransposeSolver.h>

// Pluginlib
#include <pluginlib/class_list_macros.h>

// other
#include <boost/algorithm/clamp.hpp>

/**
 * \class cartesian_controller_base::JacobianTransposeSolver
 *
 * Users may explicitly specify this solver with \a "jacobian_transpose" as \a
 * ik_solver in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "jacobian_transpose"
 *     ...
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::JacobianTransposeSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  JacobianTransposeSolver::JacobianTransposeSolver()
  {
  }

  JacobianTransposeSolver::~JacobianTransposeSolver(){}

  trajectory_msgs::JointTrajectoryPoint JacobianTransposeSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {
    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
    m_current_accelerations.data = m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();

    // Integrate twice, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool JacobianTransposeSolver::init(ros::NodeHandle& nh,
                                     const KDL::Chain& chain,
                                     const KDL::JntArray& upper_pos_limits,
                                     const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_jacobian.resize(m_number_joints);

    return true;
  }
} // namespace


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "bempp/common/config_ahmed.hpp"
#include "bempp/common/config_trilinos.hpp"

#include "aca_global_assembler.hpp"

#include "assembly_options.hpp"
#include "cluster_construction_helper.hpp"
#include "index_permutation.hpp"
#include "discrete_boundary_operator_composition.hpp"
#include "discrete_sparse_boundary_operator.hpp"

#include "../common/armadillo_fwd.hpp"
#include "../common/auto_timer.hpp"
#include "../common/boost_shared_array_fwd.hpp"
#include "../common/chunk_statistics.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/local_assembler_for_operators.hpp"
#include "../fiber/serial_blas_region.hpp"
#include "../fiber/scalar_traits.hpp"
#include "../space/space.hpp"

#include <stdexcept>
#include <iostream>

#include <boost/type_traits/is_complex.hpp>

#include <tbb/atomic.h>
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/concurrent_queue.h>

#ifdef WITH_AHMED
#include "ahmed_aux.hpp"
#include "discrete_aca_boundary_operator.hpp"
#include "scattered_range.hpp"
#include "weak_form_aca_assembly_helper.hpp"
#endif

namespace Bempp
{

// Body of parallel loop
namespace
{

#ifdef WITH_AHMED
template <typename BasisFunctionType, typename ResultType>
class AcaWeakFormAssemblerLoopBody
{
    typedef typename Fiber::ScalarTraits<ResultType>::RealType CoordinateType;
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef bemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
    typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;
public:
    typedef tbb::concurrent_queue<size_t> LeafClusterIndexQueue;

    AcaWeakFormAssemblerLoopBody(
            WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>& helper,
            AhmedLeafClusterArray& leafClusters,
            boost::shared_array<AhmedMblock*> blocks,
            const AcaOptions& options,
            tbb::atomic<size_t>& done,
            bool verbose,
            LeafClusterIndexQueue& leafClusterIndexQueue,
            bool symmetric,
            std::vector<ChunkStatistics>& stats) :
        m_helper(helper),
        m_leafClusters(leafClusters), m_blocks(blocks),
        m_options(options), m_done(done), m_verbose(verbose),
        m_leafClusterIndexQueue(leafClusterIndexQueue),
        m_symmetric(symmetric),
        m_stats(stats)
    {
    }

    template <typename Range>
    void operator() (const Range& r) const {
        const char* TEXT = "Approximating ... ";
        for (typename Range::const_iterator i = r.begin(); i != r.end(); ++i) {
            size_t leafClusterIndex = 0;
            if (!m_leafClusterIndexQueue.try_pop(leafClusterIndex)) {
                std::cerr << "AcaWeakFormAssemblerLoopBody::operator(): "
                             "Warning: try_pop failed; this shouldn't happen!"
                          << std::endl;
                continue;
            }
            m_stats[leafClusterIndex].valid = true;
            m_stats[leafClusterIndex].chunkStart = r.begin();
            m_stats[leafClusterIndex].chunkSize = r.size();
            m_stats[leafClusterIndex].startTime = tbb::tick_count::now();

            AhmedBemBlcluster* cluster =
                    dynamic_cast<AhmedBemBlcluster*>(m_leafClusters[leafClusterIndex]);
            if (m_symmetric)
                apprx_sym(m_helper, m_blocks[cluster->getidx()],
                          cluster, m_options.eps, m_options.maximumRank,
                          true /* complex_sym */);
            else
                apprx_unsym(m_helper, m_blocks[cluster->getidx()],
                            cluster, m_options.eps, m_options.maximumRank);
            m_stats[leafClusterIndex].endTime = tbb::tick_count::now();
            // TODO: recompress
            const int HASH_COUNT = 20;
            if (m_verbose)
                progressbar(std::cout, TEXT, (++m_done) - 1,
                            m_leafClusters.size(), HASH_COUNT, true);
        }

    }

private:
    WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>& m_helper;
    AhmedLeafClusterArray& m_leafClusters;
    boost::shared_array<AhmedMblock*> m_blocks;
    const AcaOptions& m_options;
    tbb::atomic<size_t>& m_done;
    bool m_verbose;
    LeafClusterIndexQueue& m_leafClusterIndexQueue;
    bool m_symmetric;
    std::vector<ChunkStatistics>& m_stats;
};

void reallyGetClusterIds(const cluster& clusterTree,
                         const std::vector<unsigned int>& p2oDofs,
                         std::vector<unsigned int>& clusterIds,
                         unsigned int& id)
{
    if (clusterTree.isleaf())
        for (unsigned int nDof = clusterTree.getnbeg(); nDof < clusterTree.getnend(); ++nDof)
            clusterIds[p2oDofs[nDof]] = id;
    else
        for (unsigned int nSon = 0; nSon < clusterTree.getns(); ++nSon)
            reallyGetClusterIds(*clusterTree.getson(nSon), p2oDofs, clusterIds, ++id);
}

void getClusterIds(const cluster& clusterTree,
                   const std::vector<unsigned int>& p2oDofs,
                   std::vector<unsigned int>& clusterIds)
{
    clusterIds.resize(p2oDofs.size());
    unsigned int id = 0;
    reallyGetClusterIds(clusterTree, p2oDofs, clusterIds, id);
}
#endif

} // namespace

template <typename BasisFunctionType, typename ResultType>
std::auto_ptr<DiscreteBoundaryOperator<ResultType> >
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
        const Space<BasisFunctionType>& testSpace,
        const Space<BasisFunctionType>& trialSpace,
        const std::vector<LocalAssembler*>& localAssemblers,
        const std::vector<const DiscreteBndOp*>& sparseTermsToAdd,
        const std::vector<ResultType>& denseTermsMultipliers,
        const std::vector<ResultType>& sparseTermsMultipliers,
        const AssemblyOptions& options,
        int symmetry)
{
#ifdef WITH_AHMED
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;
    typedef bemblcluster<AhmedDofType, AhmedDofType> AhmedBemBlcluster;
    typedef DiscreteAcaBoundaryOperator<ResultType> DiscreteAcaLinOp;

    const AcaOptions& acaOptions = options.acaOptions();
    const bool indexWithGlobalDofs = acaOptions.globalAssemblyBeforeCompression;
    const bool verbosityAtLeastDefault =
            (options.verbosityLevel() >= VerbosityLevel::DEFAULT);
    const bool verbosityAtLeastHigh =
            (options.verbosityLevel() >= VerbosityLevel::HIGH);

    // Currently we don't support Hermitian ACA operators. This is because we
    // don't have the means to really test them -- we would need complex-valued
    // basis functions for that. (Assembly of such a matrix would be very easy
    // -- just change complex_sym from true to false in the call to apprx_sym()
    // in AcaWeakFormAssemblerLoopBody::operator() -- but operations on
    // symmetric/Hermitian matrices are not always trivial and we do need to be
    // able to test them properly.)
    bool symmetric = symmetry & SYMMETRIC;
    if (symmetry & HERMITIAN && !(symmetry & SYMMETRIC) &&
            verbosityAtLeastDefault)
        std::cout << "Warning: assembly of non-symmetric Hermitian H-matrices "
                     "is not supported yet. A general H-matrix will be assembled"
                  << std::endl;

#ifndef WITH_TRILINOS
    if (!indexWithGlobalDofs)
        throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                                 "ACA assembly with globalAssemblyBeforeCompression "
                                 "set to false requires BEM++ to be linked with "
                                 "Trilinos");
#endif // WITH_TRILINOS

    const size_t testDofCount = indexWithGlobalDofs ?
                testSpace.globalDofCount() : testSpace.flatLocalDofCount();
    const size_t trialDofCount = indexWithGlobalDofs ?
                trialSpace.globalDofCount() : trialSpace.flatLocalDofCount();

    if (symmetric && testDofCount != trialDofCount)
        throw std::invalid_argument("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                                    "you cannot generate a symmetric weak form "
                                    "using test and trial spaces with different "
                                    "numbers of DOFs");

    // o2p: map of original indices to permuted indices
    // p2o: map of permuted indices to original indices
    typedef ClusterConstructionHelper<BasisFunctionType> CCH;
    shared_ptr<AhmedBemCluster> testClusterTree;
    shared_ptr<IndexPermutation> test_o2pPermutation, test_p2oPermutation;
    CCH::constructBemCluster(testSpace, indexWithGlobalDofs, acaOptions,
                             testClusterTree,
                             test_o2pPermutation, test_p2oPermutation);
    shared_ptr<AhmedBemCluster> trialClusterTree;
    shared_ptr<IndexPermutation> trial_o2pPermutation, trial_p2oPermutation;
    if (symmetric || &testSpace == &trialSpace) {
        trialClusterTree = testClusterTree;
        trial_o2pPermutation = test_o2pPermutation;
        trial_p2oPermutation = test_p2oPermutation;
    } else
        CCH::constructBemCluster(trialSpace, indexWithGlobalDofs, acaOptions,
                                 trialClusterTree,
                                 trial_o2pPermutation, trial_p2oPermutation);

//    // Export VTK plots showing the disctribution of leaf cluster ids
//    std::vector<unsigned int> testClusterIds;
//    getClusterIds(*testClusterTree, test_p2oPermutation->permutedIndices(), testClusterIds);
//    testSpace.dumpClusterIds("testClusterIds", testClusterIds,
//                             indexWithGlobalDofs ? GLOBAL_DOFS : FLAT_LOCAL_DOFS);
//    std::vector<unsigned int> trialClusterIds;
//    getClusterIds(*trialClusterTree, trial_p2oPermutation->permutedIndices(), trialClusterIds);
//    trialSpace.dumpClusterIds("trialClusterIds", trialClusterIds,
//                              indexWithGlobalDofs ? GLOBAL_DOFS : FLAT_LOCAL_DOFS);

    if (verbosityAtLeastHigh)
        std::cout << "Test cluster count: " << testClusterTree->getncl()
                  << "\nTrial cluster count: " << trialClusterTree->getncl()
                  << std::endl;

    unsigned int blockCount = 0;
    shared_ptr<AhmedBemBlcluster> bemBlclusterTree(
                CCH::constructBemBlockCluster(acaOptions, symmetric,
                                              *testClusterTree, *trialClusterTree,
                                              blockCount).release());

    if (verbosityAtLeastHigh)
        std::cout << "Mblock count: " << blockCount << std::endl;

    std::vector<unsigned int> p2oTestDofs =
        test_p2oPermutation->permutedIndices();
    std::vector<unsigned int> p2oTrialDofs =
        trial_p2oPermutation->permutedIndices();
    WeakFormAcaAssemblyHelper<BasisFunctionType, ResultType>
        helper(testSpace, trialSpace, p2oTestDofs, p2oTrialDofs,
               localAssemblers, sparseTermsToAdd,
               denseTermsMultipliers, sparseTermsMultipliers, options);

    typedef mblock<typename AhmedTypeTraits<ResultType>::Type> AhmedMblock;
    boost::shared_array<AhmedMblock*> blocks =
            allocateAhmedMblockArray<ResultType>(bemBlclusterTree.get());

    // matgen_sqntl(helper, AhmedBemBlclusterTree.get(), AhmedBemBlclusterTree.get(),
    //              acaOptions.recompress, acaOptions.eps,
    //              acaOptions.maximumRank, blocks.get());

    // matgen_omp(helper, blockCount, AhmedBemBlclusterTree.get(),
    //            acaOptions.eps, acaOptions.maximumRank, blocks.get());

    // // Dump mblocks
    // const int mblockCount = AhmedBemBlclusterTree->nleaves();
    // for (int i = 0; i < mblockCount; ++i)
    //     if (blocks[i]->isdns())
    //     {
    //         char  buffer[1024];
    //         sprintf(buffer, "mblock-dns-%d-%d.txt",
    //                 blocks[i]->getn1(), blocks[i]->getn2());
    //         arma::Col<ResultType> block((ResultType*)blocks[i]->getdata(),
    //                                     blocks[i]->nvals());
    //         arma::diskio::save_raw_ascii(block, buffer);
    //     }
    //     else
    //     {
    //         char buffer[1024];
    //         sprintf(buffer, "mblock-lwr-%d-%d.txt",
    //                 blocks[i]->getn1(), blocks[i]->getn2());
    //         arma::Col<ResultType> block((ResultType*)blocks[i]->getdata(),
    //                                     blocks[i]->nvals());
    //         arma::diskio::save_raw_ascii(block, buffer);
    //     }

    AhmedLeafClusterArray leafClusters(bemBlclusterTree.get());
    leafClusters.sortAccordingToClusterSize();
    const size_t leafClusterCount = leafClusters.size();

    const ParallelizationOptions& parallelOptions =
            options.parallelizationOptions();
    int maxThreadCount = 1;
    if (!parallelOptions.isOpenClEnabled())
    {
        if (parallelOptions.maxThreadCount() == ParallelizationOptions::AUTO)
            maxThreadCount = tbb::task_scheduler_init::automatic;
        else
            maxThreadCount = parallelOptions.maxThreadCount();
    }
    tbb::task_scheduler_init scheduler(maxThreadCount);
    tbb::atomic<size_t> done;
    done = 0;

    std::vector<ChunkStatistics> chunkStats(leafClusterCount);

    //    typedef AcaWeakFormAssemblerLoopBody<BasisFunctionType, ResultType> Body;
    //    // std::cout << "Loop start" << std::endl;
    //    tbb::tick_count loopStart = tbb::tick_count::now();
    // //    tbb::parallel_for(tbb::blocked_range<size_t>(0, leafClusterCount),
    // //                      Body(helper, leafClusters, blocks, acaOptions, done
    // //                           , chunkStats));
    //    tbb::parallel_for(ScatteredRange(0, leafClusterCount),
    //                      Body(helper, leafClusters, blocks, acaOptions, done
    //                           , chunkStats));
    //    tbb::tick_count loopEnd = tbb::tick_count::now();
    //    // std::cout << "Loop end" << std::endl;

    typedef AcaWeakFormAssemblerLoopBody<BasisFunctionType, ResultType> Body;
    typename Body::LeafClusterIndexQueue leafClusterIndexQueue;
    for (size_t i = 0; i < leafClusterCount; ++i)
        leafClusterIndexQueue.push(i);

    if (verbosityAtLeastDefault)
        std::cout << "About to start the ACA assembly loop" << std::endl;
    tbb::tick_count loopStart = tbb::tick_count::now();
    {
        Fiber::SerialBlasRegion region; // if possible, ensure that BLAS is single-threaded
        tbb::parallel_for(tbb::blocked_range<size_t>(0, leafClusterCount),
                          Body(helper, leafClusters, blocks, acaOptions, done,
                               verbosityAtLeastDefault,
                               leafClusterIndexQueue, symmetric, chunkStats));
    }
    tbb::tick_count loopEnd = tbb::tick_count::now();
    if (verbosityAtLeastDefault) {
        std::cout << "\n"; // the progress bar doesn't print the final \n
        std::cout << "ACA loop took " << (loopEnd - loopStart).seconds() << " s"
                  << std::endl;
    }

    // TODO: parallelise!
    if (acaOptions.recompress) {
        if (verbosityAtLeastDefault)
            std::cout << "About to start ACA agglomeration" << std::endl;
        agglH(bemBlclusterTree.get(), blocks.get(),
              acaOptions.eps, acaOptions.maximumRank);
        if (verbosityAtLeastDefault)
            std::cout << "Agglomeration finished" << std::endl;
    }

    // // Dump timing data of individual chunks
    //    std::cout << "\nChunks:\n";
    //    for (int i = 0; i < leafClusterCount; ++i)
    //        if (chunkStats[i].valid) {
    //            int blockIndex = leafClusters[i]->getidx();
    //            std::cout << chunkStats[i].chunkStart << "\t"
    //                      << chunkStats[i].chunkSize << "\t"
    //                      << (chunkStats[i].startTime - loopStart).seconds() << "\t"
    //                      << (chunkStats[i].endTime - loopStart).seconds() << "\t"
    //                      << (chunkStats[i].endTime - chunkStats[i].startTime).seconds() << "\t"
    //                      << blocks[blockIndex]->getn1() << "\t"
    //                      << blocks[blockIndex]->getn2() << "\t"
    //                      << blocks[blockIndex]->islwr() << "\t"
    //                      << (blocks[blockIndex]->islwr() ? blocks[blockIndex]->rank() : 0) << "\n";
    //        }

    {
        size_t origMemory = sizeof(ResultType) * testDofCount * trialDofCount;
        size_t ahmedMemory = sizeH(bemBlclusterTree.get(), blocks.get());
        int maximumRank = Hmax_rank(bemBlclusterTree.get(), blocks.get());
        if (verbosityAtLeastDefault)
            std::cout << "\nNeeded storage: "
                      << ahmedMemory / 1024. / 1024. << " MB.\n"
                      << "Without approximation: "
                      << origMemory / 1024. / 1024. << " MB.\n"
                      << "Compressed to "
                      << (100. * ahmedMemory) / origMemory << "%.\n"
                      << "Maximum rank: " << maximumRank << ".\n"
                      << std::endl;

        if (acaOptions.outputPostscript) {
            if (verbosityAtLeastDefault)
                std::cout << "Writing matrix partition ..." << std::flush;
            std::ofstream os(acaOptions.outputFname.c_str());
            if (symmetric) // seems valid also for Hermitian matrices
                psoutputHeH(os, bemBlclusterTree.get(), testDofCount, blocks.get());
            else
                psoutputGeH(os, bemBlclusterTree.get(), testDofCount, blocks.get());
            os.close();
            if (verbosityAtLeastDefault)
                std::cout << " done." << std::endl;
        }
    }

    int outSymmetry = NO_SYMMETRY;
    if (symmetric) {
        outSymmetry = SYMMETRIC;
        if (!boost::is_complex<ResultType>())
            outSymmetry |= HERMITIAN;
    }
    std::auto_ptr<DiscreteAcaLinOp> acaOp(
                new DiscreteAcaLinOp(testDofCount, trialDofCount,
                                     acaOptions.eps,
                                     acaOptions.maximumRank,
                                     outSymmetry,
                                     bemBlclusterTree, blocks,
                                     *trial_o2pPermutation,
                                     *test_o2pPermutation,
                                     parallelOptions));

    std::auto_ptr<DiscreteBndOp> result;
    if (indexWithGlobalDofs)
        result = acaOp;
    else {
#ifdef WITH_TRILINOS
        // without Trilinos, this code will never be reached -- an exception
        // will be thrown earlier in this function
        typedef DiscreteBoundaryOperatorComposition<ResultType> DiscreteBndOpComp;
        shared_ptr<DiscreteBndOp> acaOpShared(acaOp.release());
        shared_ptr<DiscreteBndOp> trialGlobalToLocal =
                constructOperatorMappingGlobalToFlatLocalDofs<
                BasisFunctionType, ResultType>(trialSpace);
        shared_ptr<DiscreteBndOp> testLocalToGlobal =
                constructOperatorMappingFlatLocalToGlobalDofs<
                BasisFunctionType, ResultType>(testSpace);
        shared_ptr<DiscreteBndOp> tmp(
                    new DiscreteBndOpComp(acaOpShared, trialGlobalToLocal));
        result.reset(new DiscreteBndOpComp(testLocalToGlobal, tmp));
#endif // WITH_TRILINOS
    }
    return result;

#else // without Ahmed
    throw std::runtime_error("AcaGlobalAssembler::assembleDetachedWeakForm(): "
                             "To enable assembly in ACA mode, recompile BEM++ "
                             "with the symbol WITH_AHMED defined.");
#endif // WITH_AHMED
}

template <typename BasisFunctionType, typename ResultType>
std::auto_ptr<DiscreteBoundaryOperator<ResultType> >
AcaGlobalAssembler<BasisFunctionType, ResultType>::assembleDetachedWeakForm(
        const Space<BasisFunctionType>& testSpace,
        const Space<BasisFunctionType>& trialSpace,
        LocalAssembler& localAssembler,
        const AssemblyOptions& options,
        int symmetry)
{
    std::vector<LocalAssembler*> localAssemblers(1, &localAssembler);
    std::vector<const DiscreteBndOp*> sparseTermsToAdd;
    std::vector<ResultType> denseTermsMultipliers(1, 1.0);
    std::vector<ResultType> sparseTermsMultipliers;

    return assembleDetachedWeakForm(testSpace, trialSpace, localAssemblers,
                            sparseTermsToAdd,
                            denseTermsMultipliers,
                            sparseTermsMultipliers,
                            options, symmetry);
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(AcaGlobalAssembler);

} // namespace Bempp