#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <chrono>

#include <unistd.h>
#include <sys/wait.h>

#include <boost/program_options.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "../../../core/image.h"
#include "../../../core/helper.h"
#include "../../../core/math_helper.h"
#include "../../../stereo_egomotion/base/egomotion_base.h"
#include "../../../tracker/stereo/experiment_factory.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
//#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../../tracker/base/tracker_helper.h"
#include "../../../optimization/bundle_adjustment/bundle_adjuster_base.h"
//#include "../../../optimization/bundle_adjustment/bundle_adjuster_multiframe.h"
#include "../../math_helper.h"
#include "../../../tracker/base/eval_helper.h"
#include "../../../core/format_helper.h"
//#include "../../helper_libviso.h"
//#include "../../cv_plotter.h"

using namespace core;

void RunEgomotionExperiment(const std::string& source_folder,
                            const std::string& imagelistfn,
                            const std::string& config_filepath,
                            const std::string& experiment_config,
                            const std::string& cparams_file,
                            const std::string& gt_filepath,
                            const std::string& deformation_field_path,
                            std::string& output_folder)
{
  std::string track_name;
  bool have_gt = false;
  std::ifstream gt_file;
  if (!gt_filepath.empty()) {
    have_gt = true;
    gt_file.open(gt_filepath);
  }
  // no need - Tsukuba baseline is in cm
  bool using_kitti = false;
  if (experiment_config.find("tsukuba") != std::string::npos) {
    printf("---------- Using Tsukuba dataset! ------------\n");
    track_name = "00";
  }
  else if (experiment_config.find("bb2") != std::string::npos) {
    printf("---------- Using Bumblebee dataset! ------------\n");
    track_name = "bb";
  }
  // else it is KITTI
  else {
    printf("---------- Using KITTI dataset! ------------\n");
    track_name = imagelistfn.substr(imagelistfn.size()-10,2);
    using_kitti = true;
  }

  bool serialize_tracker = false;
  bool use_serialized_data = false;
  const std::string serialize_folder = "/mnt/ssd/kivan/datasets/serialized_tracker/"
      + track_name + "/";
  if (serialize_tracker) {
    struct stat st = {0};
    if(stat(serialize_folder.c_str(), &st) == -1)
      mkdir(serialize_folder.c_str(), 0700);
  }

  double cam_params[5];
  FormatHelper::readCameraParams(cparams_file, cam_params);
  Eigen::VectorXd camera_params(5);
  for (int i = 0; i < 5; i++)
    camera_params[i] = cam_params[i];

  std::vector<std::string> imagelist;
  bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
  if (!ok || imagelist.empty())
    throw "can not open " + imagelistfn + " or the string list is empty\n";

  size_t start_frame = 0 * 2;
  //size_t start_frame = 770 * 2;
  size_t end_frame = imagelist.size();
  //int nframes = (imagelist.size() - start_frame) / 2;
  bool debug = false;
  //debug = true;

  std::cout << source_folder + imagelist[start_frame] << "\n";
  cv::Mat img_left_prev, img_right_prev;
  cv::Mat img_left = cv::imread(source_folder+imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat img_right = cv::imread(source_folder+imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);
  if (img_left.empty()) {
    std::cout << "Error: no images!\n";
    throw 1;
  }
  int img_rows = img_left.rows;
  int img_cols = img_left.cols;
  bool use_ba; // use bundle adjustment
  track::FeatureDetectorBase* detector = nullptr;
  track::TrackerBase* mono_tracker = nullptr;
  track::StereoTrackerBase* stereo_tracker = nullptr;
  egomotion::EgomotionBase* egomotion = nullptr;
  optim::BundleAdjusterBase* bundle_adjuster = nullptr;
  bool status = track::ExperimentFactory::create_experiment(
      experiment_config, deformation_field_path, cam_params, img_rows, img_cols, output_folder,
      &detector, &mono_tracker, &stereo_tracker, &egomotion, use_ba, &bundle_adjuster);
  if (!status) {
    std::cout << "Invalid experiment config!\n";
    return;
  }

  //int mf_ba_frames = 3;
  //optim::BundleAdjusterBase* bundle_adjuster_multiframe = new optim::BundleAdjusterMultiframe(
  //    mf_ba_frames-1, stereo_tracker->countFeatures(), optim::SBAbase::kMotion, true);
  if (use_ba) {
    bundle_adjuster->SetCameraParams(camera_params);
    //bundle_adjuster_multiframe->set_camera_params(cam_params);
  }

  //egomotion::Matrix pose = egomotion::Matrix::eye(4);
  //cv::Mat pose = cv::Mat::eye(4, 4, CV_64F);
  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  Eigen::Matrix4d Rt = Eigen::Matrix4d::Identity();
  //Eigen::Matrix4d Rt_inv = Eigen::Matrix4d::Identity();
  //Matrix point_rt = Matrix::eye(4);
  //cv::Mat pose_mat = cv::Mat::eye(4, 4, CV_64F);
  //cv::Mat Rt_inv;
  //cv::Mat mat_I = cv::Mat::eye(4, 4, CV_64F);
  //mat_I.copyTo(Rt_inv);
  //cv::Mat Rt(4, 4, CV_64F);
  //cv::Mat Rt_prev(4, 4, CV_64F);
  //cv::Mat Rt_gt(4, 4, CV_64F);
  //cv::Mat Rt_gt_prev = cv::Mat::eye(4, 4, CV_64F);
  //cv::Mat Rt_gt_curr = cv::Mat::eye(4, 4, CV_64F);
  //core::MathHelper::invTrans(Rt_inv, Rt);

  // init the tracker
  if (!use_serialized_data)
    stereo_tracker->init(img_left, img_right);

  // matrix transforms points in current camera coordinate system to world coord system
  std::vector<Eigen::Matrix4d> egomotion_Rt_all;
  //Eigen::Matrix4d pose_egomotion = Eigen::Matrix4d::Identity();
  egomotion_Rt_all.push_back(pose);

  std::vector<cv::Mat> gt_world_motion, gt_camera_motion;
  if (have_gt) {
    int num_of_motions = ((imagelist.size()/2) - 1);
    core::FormatHelper::Read2FrameMotionFromAccCameraMotion(gt_filepath, num_of_motions,
                                                            gt_world_motion, gt_camera_motion);
  }

  //std::string bad_folder = "/home/kivan/Projects/datasets/KITTI/tracks/bad_patches/";
  //std::string good_folder = "/home/kivan/Projects/datasets/KITTI/tracks/good_patches/";
  //core::Helper::CleanFolder(bad_folder);
  //core::Helper::CleanFolder(good_folder);

  std::vector<size_t> track_index, track_cnt;
  track_index.assign(stereo_tracker->countFeatures(), 0);
  track_cnt.assign(stereo_tracker->countFeatures(), 0);

  int frame_num = 0;
  auto start = std::chrono::system_clock::now();
  //for (unsigned i = start_frame + 2; i < start_frame+60; i+=2) {
  //double max_trans_error = 0.0;
  for (size_t i = start_frame + 2; i < end_frame; i+=2) {
    size_t motion_idx = i / 2 - 1;
    frame_num++;
    std::cout << "Frame num: " << frame_num << " / " << end_frame/2 << "\n";
    if (have_gt)
      std::cout << "motion_idx: " << motion_idx << " / " << (gt_world_motion.size() - 1) << "\n";
    std::cout << source_folder + imagelist[i] << "\n";

    if (!use_serialized_data) {
      cv::swap(img_left_prev, img_left);
      cv::swap(img_right_prev, img_right);
      img_left = cv::imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
      img_right = cv::imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
      stereo_tracker->track(img_left, img_right);
    }
    // serialization code
    else if (!serialize_tracker) {
      std::string serialize_path = serialize_folder + "/tracks_" + std::to_string(motion_idx);
      std::cout << "Loading serialized data at: " << serialize_path << "\n";
      std::ifstream input_file(serialize_path);
      boost::archive::binary_iarchive iarchive(input_file);
      iarchive >> *(dynamic_cast<track::StereoTracker*>(stereo_tracker));
      //track::FeatureInfo data = stereo_tracker->featureLeft(54);
      //std::cout << "Age = " << data.age_ << "\n";
    }
    else return;
    if (serialize_tracker) {
      std::cout << "Serializing tracker data to disk...\n";
      // save data to archive
      std::ofstream ofs(serialize_folder + "/tracks_" + std::to_string(motion_idx));
      //boost::archive::text_oarchive oarchive(ofs);
      boost::archive::binary_oarchive oarchive(ofs);
      // write class instance to archive
      oarchive << *(dynamic_cast<track::StereoTracker*>(stereo_tracker));
      // archive and stream closed when destructors are called
    }

    // default prior
    //double max_z_dist = 300.0; // for tsukuba in cm
    double min_disp = 0.1;
    double max_disp_diff = 50.0;
    // for KITTI
    if (using_kitti) {
      max_disp_diff = 30.0;
      //max_z_dist = 3.0;         // 3m
    }
    // filter tracks with prior knowledge
    // TODO
    track::TrackerHelper::FilterTracksWithPriorOld(*stereo_tracker, max_disp_diff, min_disp);

    //// filter outliers with GT
    //bool gt_filtering_passed = false;
    //if(have_gt) {
    //  std::vector<std::vector<double>> left_tmp, right_tmp;
    //  gt_filtering_passed = track::EvalHelper::FilterOutliersWithGroundtruth(*stereo_tracker,
    //                         cam_params, gt_motion[i/2], 4.0); // the best thresh - 4.0
    //}

    // filter outliers with GT
    //if (have_gt) {
    //  std::vector<std::vector<double>> left_tmp, right_tmp;
    //  track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, gt_motion[i/2],
    //                                         img_left_prev,
    //      img_right_prev, "left_prev_best", false, left_tmp, right_tmp,
    //      h_bins, v_bins, false, 4.0, true); // the best - 4.0
    //}

    // convert from my tracker to egomotion data structure
    //std::vector<int> active_tracks;
    //std::vector<egomotion::Matcher::p_match> egomotion_tracks;
    //FeatureHelper::TrackerBaseToegomotion(stereo_tracker, egomotion_tracks, active_tracks);

    // draw tracks before ransac
    if (debug)
      track::EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
                                   "all_left", "all_right");

    // run egomotion optimization
    // Rt is the motion of world points with respect to the camera
    bool success = egomotion->GetMotion(*stereo_tracker, Rt);
    //bool success = true;

    // diff check to test for estimation chrashes on KITTI test
    //double trans_diff;
    //core::MathHelper::GetMotionError(Rt_prev, Rt, trans_diff);
    //  std::cout << "Trans diff = " << trans_diff << "\n";
    //if (frame_num > 1 && trans_diff > 0.1) {
    //  std::cout << "[Error]: Big trans diff = " << trans_diff << "\n";
    //  throw 1;
    //}

    // CheckMotionDiff(Rt, Rt_prev);
    //if (egomotion->process(egomotion_tracks)) {
    if (success) {
      // is it better to kill or not to kill outliers in tracker?

      // filter outlier tracks
      std::cout << "Tracks before RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      std::vector<int> outliers = egomotion->GetTrackerOutliers();
      for (size_t j = 0; j < outliers.size(); j++)
        stereo_tracker->removeTrack(outliers[j]);
      std::cout << "Tracks after RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      ////track::FeatureInfo data = stereo_tracker->featureLeft(54);
      ////std::cout << "After filtering Age = " << data.age_ << "\n";

      //// draw tracks
      if (debug)
        track::EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
                                            "ransac_left", "ransac_right");

      //////FeatureHelper::drawStereoRefinerTracks(tracker_basic, tracker, disp_camera_left,
      //                                           disp_camera_right);
      //waitKey(0);

      //// run GT filter after RANSAC
      //bool filter_bad = false;
      ////if (using_kitti)
      ////  filter_bad = true;
      ////double max_reproj_error = 0.6; // for BA experiment
      //double max_reproj_error = 1.0;
      //double max_remove_ratio = 0.5;  // remove max 50%
      //int bad_tracks = track::EvalHelper::
      //    CountFilterStoreBadTracks(*stereo_tracker, cam_params, Rt_gt,
      //    good_folder, bad_folder, track_index, track_cnt, all_tracks_cnt,
      //    filter_bad, max_reproj_error, max_remove_ratio);
      //std::cout << "Num of bad tracks = " << bad_tracks << '\n';

      //track::DebugHelper::DebugStereoRefiner(img_left_prev, img_right_prev, img_left, img_right,
      //    *dynamic_cast<StereoTrackerRefiner*>(stereo_tracker), Rt_gt, cam_params);

      //track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, gt_motion[i/2],
      //                                       img_left_prev, img_right_prev, "left_prev_ransac",
      //                                       true, left_reproj_errors, right_reproj_errors,
      //                                       h_bins, v_bins, false);
      //if (frame_num > 10)
      //  track::EvalHelper::DrawErrorDistribution(h_bins, v_bins, left_reproj_errors, error_img,
      //                                           true, false);
      //if (i == (end_frame-2)) {
      ////if (i == 2*20) {
      //  cv::Mat eq_img;
      //  cv::cvtColor(error_img, eq_img, cv::COLOR_RGB2GRAY);
      //  cv::equalizeHist(eq_img, eq_img);
      //  cv::imwrite("heat_map_with_stats_orig.png", error_img);
      //  cv::imwrite("heat_map_with_stats.png", eq_img);
      //  track::EvalHelper::DrawErrorDistribution(h_bins, v_bins, left_reproj_errors,
      //                                           error_img, false, true);
      //  cv::cvtColor(error_img, eq_img, cv::COLOR_RGB2GRAY);
      //  cv::equalizeHist(eq_img, eq_img);
      //  cv::imwrite("heat_map_orig.png", error_img);
      //  cv::imwrite("heat_map.png", eq_img);
      //}

      //if (have_gt) {
      //  std::cout << "groundtruth:\n" << gt_world_motion[motion_idx] << std::endl;
      //  //double trans_error;
      //  //core::MathHelper::GetMotionError(gt_motion[i/2], Rt, trans_error);
      //  //std::cout << "Translation error = " << trans_error << " m\n";
      //  ////if (trans_error > 0.8) throw 1;
      //  //if (trans_error > max_trans_error)
      //  //  max_trans_error = trans_error;
      //}
      if (debug)
        while(cv::waitKey(0) != 27);

      //track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, Rt, img_left_prev,
      //                                       img_right_prev);
      // TODO:
      //Eigen::Vector3d t, r;
      //core::MathHelper::GetEulerAngles(Rt, r);
      //core::MathHelper::GetTranslation(Rt, t);

      // if the motion is to small skip it
      // tracker.undo_state();
      //MathHelper::InverseTransform(Rt, Rt_inv);    // better
      // on success, update current pose
      //pose = pose * Rt_inv;
      pose = pose * Rt.inverse();
      egomotion_Rt_all.push_back(pose);

      if (use_ba) {
        //bundle_adjuster->UpdateTracks(*stereo_tracker, Rt);
        bundle_adjuster->UpdateTracks(*stereo_tracker, Eigen::Matrix4d::Identity());
        //bundle_adjuster_multiframe->update_tracks(*stereo_tracker, Rt);
        // for old BA version
        //bundle_adjuster_multiframe->update_tracks(*stereo_tracker, Rt_inv);
        //if (frame_num >= 2) {
        if (frame_num >= (bundle_adjuster->num_frames() - 1)) {
          //double egomotion_error = track::EvalHelper::
          //    GetStereoReprojError(*stereo_tracker, cam_params, Rt);
          //double gt_error;
          //if (have_gt)
          //  gt_error = track::EvalHelper::
          //      GetStereoReprojError(*stereo_tracker, cam_params, gt_world_motion[motion_idx]);
          if(!bundle_adjuster->Optimize())
            return;
          //cv::Mat Rt_sba = bundle_adjuster->
          //    camera_motion(bundle_adjuster->camera_motion_num()-1);
          //cv::Mat Rt_sba_inv;
          //core::MathHelper::invTrans(Rt_sba, Rt_sba_inv);
          if (use_ba) {
            std::cout << "Two-frame odometry:\n" << Rt << std::endl;
            std::cout << "BA odometry:\n" << bundle_adjuster->camera_motions().back() << "\n";
          }

          //double ba_error = track::EvalHelper::
          //    GetStereoReprojError(*stereo_tracker, cam_params, Rt_sba_inv);
          //if (have_gt) printf("GT reproj error = %e\n", gt_error);
          //printf("egomotion reproj error = %e\n", egomotion_error);
          //printf("BA reproj error = %e\n", ba_error);
        }
        //if (frame_num >= (mf_ba_frames-1)) {
        //  bundle_adjuster_multiframe->optimize();
        //  cv::Mat Rt_sba = bundle_adjuster_multiframe->camera_motion(
        //                bundle_adjuster_multiframe->camera_motion_num() - 1);
        //  cv::Mat Rt_sba_inv;
        //  MathHelper::invTrans(Rt_sba, Rt_sba_inv);
        //  cout << "multi-frame BA odometry:\n" << Rt_sba_inv << endl;
        //}
      }

      //double num_matches = egomotion->getNumberOfMatches();
      //double num_inliers = egomotion->getNumberOfInliers();
      //cout << "[egomotion] Matches: " << num_matches << ", Inliers: " << num_inliers << " -> "
      //     << 100.0*num_inliers/num_matches << " %" << endl;
      std::cout << "-----------------------------------------------------------------------\n\n";
      //cv::Mat location_egomotion(pose_egomotion, Range(0,4), Range(3,4)); // extract 4-th column
    } else {
      std::cout << "Egomotion failed!\n";
      return;
      //throw "Error\n";
    }
  }
  auto end = std::chrono::system_clock::now();
  //auto elapsed = end - start;
  std::chrono::duration<double> elapsed = end - start;
  std::cout << "Elapsed time = " << elapsed.count() << " sec\n";
  std::ofstream outfile("times.txt", std::ios_base::app);
  outfile << track_name << ": " << elapsed.count() << " sec\n";

  //if (have_gt)
  //  std::cout << "Max trans error = " << max_trans_error << " m\n";
  std::cout << "Writing track " << track_name << " to files\n";
  std::ofstream egomotion_ofile(output_folder + '/' + track_name + ".txt");

  std::ofstream ba_ofile;
  std::ofstream ba_ofile2;
  //cv::Mat sba_pose;
  if (use_ba) {
    std::string ba_folder = output_folder.substr(0, output_folder.size()-1) + "_ba/";
    //std::string ba_folder2 = output_folder.substr(0, output_folder.size()-1) + "_ba_multiframe/";
    ba_ofile.open(ba_folder + track_name + ".txt");
    //ba_ofile2.open(ba_folder2 + track_name + ".txt");
    //sba_pose = cv::Mat::eye(4, 4, CV_64F);
  }
  pose = Eigen::Matrix4d::Identity();
  std::vector<Eigen::Matrix4d> bundle_adjusted_motions;
  if (use_ba) {
    bundle_adjusted_motions = bundle_adjuster->camera_motions();
    if (egomotion_Rt_all.size() != bundle_adjusted_motions.size()+1) {
      std::cout << "[egomotion.cc] " << egomotion_Rt_all.size() << "!="
                << bundle_adjusted_motions.size()+1 << "\n";
      return;
    }
  }
  for (size_t i = 0; i < egomotion_Rt_all.size(); i++) {
    FormatHelper::WriteMotionToFile(egomotion_Rt_all[i], egomotion_ofile);
    if (use_ba) {
      FormatHelper::WriteMotionToFile(pose, ba_ofile);
      if (i < bundle_adjusted_motions.size())
        pose = pose * bundle_adjusted_motions[i].inverse();
      //FormatHelper::WriteMatRt(bundle_adjuster_multiframe->camera_motion_acc(i), ba_ofile2);
    }
  }

  if (use_ba)
    delete bundle_adjuster;
  delete detector;
  delete mono_tracker;
  delete stereo_tracker;
  delete egomotion;

  //// run evaluation
  //std::string cmd = "/home/kivan/Projects/cv-stereo/stereo_odometry/evaluation/" +
  //                  "evaluate_odometry_dense 7 7 " +
  //    std::to_string(start_frame/2) + " " + std::to_string(nframes) + " ./results/ eval_stats";
  //std::cout << cmd << "\n";
  //system(cmd.c_str());
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string experiment_config;
  std::string imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string gt_filepath;
  std::string deformation_field_path;
  std::string output_folder;

  if (argc != 5 && argc != 7 && argc != 9){
    std::cout << "Usage:\n" << argv[0]
        << " -c dataset_config -e experiment_config [-o output_folder] [-d deformation_field]\n";
    return 0;
  }

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help, h", "produce help message")
      ("config,c", po::value<std::string>(&config_file), "config filename")
      ("experiment_config,e", po::value<std::string>(&experiment_config), "experiment filename")
      ("output_folder,o", po::value<std::string>(&output_folder), "output folder path")
      ("deformation_field,d", po::value<std::string>(&deformation_field_path),
        "file with deformation field");
    po::options_description cmdline_options;
    cmdline_options.add(generic);

    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if (vm.count("help")) {
      std::cout << generic;
      return 0;
    }

    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<std::string>(&cam_params_file)->
        default_value("camera_params.txt"), "camera params file")
      ("source_folder,s", po::value<std::string>(&source_folder), "folder with source")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("groundtruth,g", po::value<std::string>(&gt_filepath), "file with motion GT")
      ("deformation_field,d", po::value<std::string>(&deformation_field_path),
       "file with deformation field");

    std::ifstream ifs(config_file);
    if (!ifs) {
      throw "can not open config file: " + config_file + "\n";
    }
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    return -1;
  }

  std::cout << "Using track config = " << config_file << '\n';
  RunEgomotionExperiment(source_folder, imagelistfn, config_file, experiment_config,
                         cam_params_file, gt_filepath, deformation_field_path, output_folder);

  return 0;
}


#include "mapwidget.h"
#include "mapviewer.h"

#include <QStatusBar>

#include <boost/utility.hpp>

using namespace std;

MapWidget::MapWidget (MapViewer *mapviewer,QWidget *parent) : QGLWidget(parent), mv(mapviewer) {

  // get map range in cartesian coordinates
  Objects::tRange map_range = mv->objects->getMapRange();
  Coordinates::tCartesian cart_min = mv->coord->latLonToCartesian(map_range.geo_min);
  Coordinates::tCartesian cart_max = mv->coord->latLonToCartesian(map_range.geo_max);
  x_min = cart_min.x; y_min = cart_min.y;
  x_max = cart_max.x; y_max = cart_max.y;
  delta = min(x_max-x_min,y_max-y_min);

  // init map scale and offset
  scale = 1;
  x_pos = 0;
  y_pos = 0;

  // enable mouse tracking for continuous updates
  setMouseTracking(true);

  // init marker to disabled
  marker = 0;
}

MapWidget::~MapWidget () {
}

void MapWidget::cartesianToScreen (Coordinates::tCartesian cart,double &u,double &v) {
  u = scale_x*(scale*(cart.x-x_min)/delta+x_pos);
  v = scale_y*(scale*(cart.y-y_min)/delta+y_pos);
}

void MapWidget::cartesianToUnit (Coordinates::tCartesian cart,double &u,double &v) {
  u = scale*(cart.x-x_min)/delta+x_pos;
  v = scale*(cart.y-y_min)/delta+y_pos;
}

Coordinates::tCartesian MapWidget::screenToCartesian (double u,double v) {
  Coordinates::tCartesian cart;
  cart.x = (u/scale_x-x_pos)*delta/scale+x_min;
  cart.y = (v/scale_y-y_pos)*delta/scale+y_min;
  return cart;
}

Coordinates::tGeographic MapWidget::screenToLatLon (double u,double v) {
  return mv->coord->cartesianToLatLon(screenToCartesian(u,v));
}

void MapWidget::plotEndPoint (Objects::tReference &ref1,Objects::tReference &ref2) {

  // get references to points
  Objects::tPoint &p1 = mv->objects->getPayload<Objects::tPoint>(ref1.ptr);
  Objects::tPoint &p2 = mv->objects->getPayload<Objects::tPoint>(ref2.ptr);

  // get cartesian coordinates
  double p1x,p1y,p2x,p2y;
  Coordinates::tCartesian cart1 = mv->coord->latLonToCartesian(p1.geo);
  Coordinates::tCartesian cart2 = mv->coord->latLonToCartesian(p2.geo);
  p1x = cart1.x; p1y = cart1.y;
  p2x = cart2.x; p2y = cart2.y;

  // get vector in direction of polyline
  double vx = p2x-p1x;
  double vy = p2y-p1y;

  // compute vector norm
  double norm = sqrt(vx*vx+vy*vy);
  if (norm<1e-5) return;

  // normalize vector in direction of polyline
  vx /= 2.0*norm;
  vy /= 2.0*norm;

  // compute normalized screen coordinates
  double p1u,p1v,p2u,p2v;
  cartesianToScreen(Coordinates::tCartesian(p1x+vx+vy,p1y+vy-vx),p1u,p1v);
  cartesianToScreen(Coordinates::tCartesian(p1x+vx-vy,p1y+vy+vx),p2u,p2v);

  // draw line
  glBegin(GL_LINES);
  glVertex3f(p1u*2.0-1.0,p1v*2.0-1.0,0);
  glVertex3f(p2u*2.0-1.0,p2v*2.0-1.0,0);
  glEnd();
}

void MapWidget::plotPolyline (Objects::tObject *object) {

  // get polyline
  Objects::tPolyline &polyline = mv->objects->getPayload<Objects::tPolyline>(object);
  vector<Objects::tReference> &points = polyline.points;

  // start polyline
  glBegin(GL_LINE_STRIP);

  // plot polyline
  for (vector<Objects::tReference>::iterator it=points.begin(); it!=points.end(); it++) {
    Objects::tPoint &point = mv->objects->getPayload<Objects::tPoint>(it->ptr);
    Coordinates::tCartesian cart = mv->coord->latLonToCartesian(point.geo);
    double u,v;
    cartesianToScreen(cart,u,v);
    glVertex3f(u*2.0-1.0,v*2.0-1.0,0);
  }

  // end polyline
  glEnd();

  // plot end points
  int n = points.size();
  if (n>1) {
    plotEndPoint(points[0],points[1]);
    plotEndPoint(points[n-1],points[n-2]);
  }
}

void MapWidget::plotPoint (Objects::tPoint &point) {

  // map central point to screen
  Coordinates::tCartesian cart = mv->coord->latLonToCartesian(point.geo);
  double u,v;
  cartesianToScreen(cart,u,v);

  // draw map central point
  glBegin(GL_POINTS);
  glVertex3f(u*2.0-1.0,v*2.0-1.0,0);
  glEnd();
}

void MapWidget::plotRange () {

  // compute corners in normalized screen coordinates
  double p1u,p1v,p2u,p2v,p3u,p3v,p4u,p4v;
  cartesianToScreen(Coordinates::tCartesian(x_min,y_min),p1u,p1v);
  cartesianToScreen(Coordinates::tCartesian(x_max,y_min),p2u,p2v);
  cartesianToScreen(Coordinates::tCartesian(x_max,y_max),p3u,p3v);
  cartesianToScreen(Coordinates::tCartesian(x_min,y_max),p4u,p4v);

  // start dotted lines
  glLineStipple(1, 0xAAAA);
  glEnable(GL_LINE_STIPPLE);

  // start closed polyline
  glBegin(GL_LINE_LOOP);

  // draw rectangle
  glVertex3f(p1u*2.0-1.0,p1v*2.0-1.0,0);
  glVertex3f(p2u*2.0-1.0,p2v*2.0-1.0,0);
  glVertex3f(p3u*2.0-1.0,p3v*2.0-1.0,0);
  glVertex3f(p4u*2.0-1.0,p4v*2.0-1.0,0);

  // end polyline
  glEnd();

  // end dotted lines
  glDisable(GL_LINE_STIPPLE);
}

void MapWidget::paintGL () {

  // clear screen
  glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
  glClear(GL_COLOR_BUFFER_BIT);

  // get screen range in geographic coordinates
  Objects::tRange range;
  range.geo_min = screenToLatLon(0,0);
  range.geo_max = screenToLatLon(1,1);

  // retrieve all objects which are currently visible
  Objects::objects_t objects = mv->objects->getObjectsInRange(range);

  // draw streets
  glColor3f(1,0,0);
  for (Objects::objects_t::iterator it=objects.begin(); it!=objects.end(); it++)
    if (mv->objects->isType<Objects::tPolyline>(*it,"street"))
      plotPolyline(*it);

  // draw buildings
  glColor3f(0,1,0);
  for (Objects::objects_t::iterator it=objects.begin(); it!=objects.end(); it++)
    if (mv->objects->isType<Objects::tPolyline>(*it,"building"))
      plotPolyline(*it);

  // draw location marker
  if (marker) {
    glPointSize(5);
    glColor3f(1,1,0);
    plotPoint(*marker);
  }

  // draw rectangular range of the map
  if (0) {
    glColor3f(0.8,0.8,0.8);
    plotRange();
  }
}

void MapWidget::resizeGL (int w, int h) {

  // compute viewport scalings
  scale_x = 1;
  scale_y = 1;
  if (w<h) scale_y = (double)w/max((double)h,1.0);
  else     scale_x = (double)h/max((double)w,1.0);

  // set viewport
  glViewport(0,0,w,h);
  paintGL();
  updateStatusBar();
}

void MapWidget::mousePressEvent (QMouseEvent* event) {

  // set mouse position
  x_mouse = event->posF().x();
  y_mouse = event->posF().y();

  // on right click, retrieve all polylines which are connected to a point
  if (event->buttons()==Qt::RightButton) {

    Coordinates::tCartesian  cart = getCurrentLocationCartesian();
    Coordinates::tGeographic geo  = mv->coord->cartesianToLatLon(cart);
    Objects::tObject* object      = mv->objects->getClosestPoint (geo);

    // plot infos
    if (object) {
      cout << endl;
      cout << object->type << " (sha: " << object->sha;
      Objects::tPoint &point = mv->objects->getPayload<Objects::tPoint>(object);
      marker = &point;
      cout << ", lat: " << point.geo.lat << ", lon: " << point.geo.lon << ", alt: " << point.alt << ")" << endl;
      for (Objects::objects_t::iterator it=object->use.begin(); it!=object->use.end(); it++) {
        if (mv->objects->isType<Objects::tPolyline>(*it)) {
          cout << ">> type: '" << (*it)->type << "'";
          if ((*it)->kind.size()>0)
            cout << ", kind: '" << (*it)->kind << "'";
          cout << " (sha: " << (*it)->sha << ")" << endl;
          if ((*it)->tag.size()>0) {
            cout << "   ";
            for (Objects::tags_t::iterator it_tag=(*it)->tag.begin(); it_tag!=(*it)->tag.end(); it_tag++) {
              cout << it_tag->first << ": '" << it_tag->second << "'";
              if (boost::next(it_tag)!=(*it)->tag.end())
                cout << ", ";
            }
            cout << endl;
          }
        }
      }
    } else {
      marker = 0;
    }
  }

  // refresh
  updateGL();
  updateStatusBar();
}

Coordinates::tCartesian MapWidget::getCurrentLocationCartesian () {
  double u = x_mouse/(double)size().width();
  double v = 1.0-y_mouse/(double)size().height();
  Coordinates::tCartesian cart = screenToCartesian(u,v);
  return cart;
}

void MapWidget::updateStatusBar () {

  // set status message
  Coordinates::tCartesian  cart  = getCurrentLocationCartesian();
  Coordinates::tGeographic geo   = mv->coord->cartesianToLatLon(cart);
  Coordinates::tCartesian  dcart = screenToCartesian(1,1)-screenToCartesian(0,0);
  char msg[2048];
  sprintf(msg,"x: %1.2f m, y: %1.2f m, lat: %1.5f, lon: %1.5f, scale: %1.2f, w: %1.2f m, h: %1.2f m",cart.x,cart.y,geo.lat,geo.lon,scale,dcart.x,dcart.y);
  mv->statusBar()->showMessage(msg);
}

void MapWidget::mouseMoveEvent (QMouseEvent* event){

  // translate map
  if (event->buttons()==Qt::LeftButton) {
    double s  = max(min(size().width(),size().height()),1);
    double dx = ((double)event->posF().x()-x_mouse)/s;
    double dy = ((double)event->posF().y()-y_mouse)/s;
    x_pos += dx;
    y_pos -= dy;
  }

  // update current mouse position
  x_mouse = event->posF().x();
  y_mouse = event->posF().y();

  // refresh
  updateGL();
  updateStatusBar();
}

void MapWidget::wheelEvent (QWheelEvent* event){

  // mouse position
  x_mouse = event->pos().x();
  y_mouse = event->pos().y();

  // widget size
  double w = max(size().width(),1);
  double h = max(size().height(),1);

  // compute new scaling
  double delta = (double)event->delta();
  double scale_old = scale;
  if (delta<0 && scale>0.001) scale /= 1.2;
  if (delta>0 && scale<100.0) scale *= 1.2;

  // adjust viewport position (centered on mouse)
  double x_screen = (x_mouse/(scale_x*w)-x_pos)/scale_old;
  double y_screen = (1.0-y_mouse/(scale_y*h)-y_pos)/scale_old;
  x_pos -= (scale-scale_old)*x_screen;
  y_pos -= (scale-scale_old)*y_screen;

  // refresh
  updateGL();
  updateStatusBar();
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ScreenedRutherfordElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The screened Rutherford elastic electronscattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

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

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "Utility_TabularDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp"

namespace MonteCarlo{

//! The scattering distribution base class
class ScreenedRutherfordElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                    public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the array of energy dependent screened rutherford paramters
  //! (first = energy, second = Moliere screening constant,
  //!  third = normalization constant
  typedef Teuchos::Array<Utility::Trip<double,double,double> > ParameterArray;

  typedef Teuchos::RCP<const AnalogElasticElectronScatteringDistribution>
            ElasticDistribution;

  //! Constructor from ACE table data
  ScreenedRutherfordElasticElectronScatteringDistribution(
    const ElasticDistribution& elastic_cutoff_distribution,
    const int atomic_number,
    const double upper_cutoff_angle = 1.0e-6 );

  //! Constructor from ENDL table data
  ScreenedRutherfordElasticElectronScatteringDistribution(
    const ParameterArray& screened_rutherford_parameters,
    const double upper_cutoff_angle = 1.0e-6 );

  //! Destructor
  virtual ~ScreenedRutherfordElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  double evaluate( const double incoming_energy,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluatePDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the integrated PDF
  double evaluateIntegratedPDF( const double incoming_energy) const;

  //! Evaluate the CDF
  double evaluateCDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

//protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

  //! Evaluate Moliere's atomic screening constant at the given electron energy
  double evaluateMoliereScreeningConstant( const double energy ) const;

private:

  // Find the lower and upper bin boundary
  void findLowerAndUpperBinBoundary(
        const double incoming_energy,
        ParameterArray::const_iterator& lower_bin_boundary,
        ParameterArray::const_iterator& upper_bin_boundary,
        double& interpolation_fraction ) const;

  // evaluate the pdf integrated from 0 to angle
  double evaluateIntegratedPDF(
        const double& scattering_angle,
        const ParameterArray::const_iterator& lower_bin_boundary,
        const ParameterArray::const_iterator& upper_bin_boundary,
        const double& interpolation_fraction ) const;

  // The fine structure constant (fsc) squared
  static double s_fine_structure_const_squared;

  // A parameter for moliere's screening factor  (1/2*(fsc/0.885)**2)
  static double s_screening_param1;

  // Atomic number (Z) of the target atom
  int d_atomic_number;

  // Atomic number (Z) of the target atom to the 2/3 power (Z^2/3)
  double d_Z_two_thirds_power;

  // A parameter for moliere's screening factor (3.76*fsc**2*Z**2)
  double d_screening_param2;

  // The scattering angle below which the screened Rutherford distribution is used
  double d_upper_cutoff_angle;

  // The scattering angle cosine above which the screened Rutherford distribution is used
  double d_lower_cutoff_angle_cosine;

  // Flag to indicate that tabulated screened rutherford parameters are used
  bool d_using_endl_tables;

  // Analog elastic scattering distribution
  ElasticDistribution d_elastic_cutoff_distribution;

  // Screened Rutherford energy depended paramters: Moliere's screening constant and normalization constant
  ParameterArray d_screened_rutherford_parameters;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ScreenedRutherfordElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//


#ifndef INCLUDE_OKVIS_LOOP_FRAME_AND_MATCHES_HPP_
#define INCLUDE_OKVIS_LOOP_FRAME_AND_MATCHES_HPP_
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>

#include <okvis/kinematics/Transformation.hpp>
#include <okvis/Time.hpp>

namespace swift_vio {
struct MatchedKeypoint {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  MatchedKeypoint(const Eigen::Vector3f& kp, size_t kpIndex,
                  const Eigen::Matrix<double, 4, 1>& pBl)
      : kp_(kp), kpIndexInQueryFrame_(kpIndex), hp_Bl_(pBl) {}

  MatchedKeypoint(const Eigen::Vector3f& kp, size_t kpIndex)
      : kp_(kp), kpIndexInQueryFrame_(kpIndex) {
    hp_Bl_[3] = 0;
  }

  bool isLandmarkInitialized() {
    return hp_Bl_[3] > 1e-7;
  }

  Eigen::Vector3f kp_; ///< each entry: x, y, size.
  size_t kpIndexInQueryFrame_;  ///< keypoint index in the query frame.
  Eigen::Matrix<double, 4, 1>
      hp_Bl_;  ///< landmark position in loop frame's body frame
};

class LoopFrameAndMatches {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LoopFrameAndMatches();

  LoopFrameAndMatches(uint64_t id, okvis::Time stamp, size_t dbowId,
                      uint64_t queryKeyframeId, okvis::Time queryKeyframeStamp,
                      size_t queryKeyframeDbowId,
                      const okvis::kinematics::Transformation& T_BlBq);

  ~LoopFrameAndMatches();

  const Eigen::Matrix<double, 6, 6>& relativePoseSqrtInfo() const {
    return squareRootInfo_T_BlBq_;
  }

  const Eigen::Matrix<double, 6, 6>& poseCovariance() const {
    return cov_vio_T_WB_;
  }

  std::vector<MatchedKeypoint, Eigen::aligned_allocator<MatchedKeypoint>>&
  MatchedKeypointsMutable() {
    return matchedKeypoints_;
  }

  void setPoseCovariance(const Eigen::Matrix<double, 6, 6>& cov_T_WB) {
    cov_vio_T_WB_ = cov_T_WB;
  }

  void setRelativePoseSqrtInfo(const Eigen::Matrix<double, 6, 6>& sqrtInfo_Bl_Pose_Bq) {
    squareRootInfo_T_BlBq_ = sqrtInfo_Bl_Pose_Bq;
  }

  uint64_t id_; ///< id of the loop keyframe earlier assigned by vio.
  okvis::Time stamp_;
  size_t dbowId_; ///< id of the loop keyframe in keyframe dbow database.

  uint64_t queryKeyframeId_; ///< id of the query keyframe assigned by vio.
  okvis::Time queryKeyframeStamp_;
  size_t queryKeyframeDbowId_; ///< id of the query keyframe in keyframe dbow database.

  okvis::kinematics::Transformation
      T_BlBq_;  ///< Bl loop frame, Bq query keyframe.

  okvis::kinematics::Transformation
      pgo_T_WBl_;  ///< Pose of the found loop frame in the world frame of the latest
             ///< pose graph.
 private:
  // We do not compute cov of the pose provided by PGO because
  // it is expensive for optimization-based PGO.
  Eigen::Matrix<double, 6, 6> cov_vio_T_WB_; ///< cov of the pose of the loop frame provided by VIO.

  Eigen::Matrix<double, 6, 6>
      squareRootInfo_T_BlBq_;  ///< square root L' of the info matrix $\Lambda$
                               ///< for the error in T_BlBq such that $LL'=
                               ///< \Lambda$.

  std::vector<MatchedKeypoint, Eigen::aligned_allocator<MatchedKeypoint>>
      matchedKeypoints_; ///< keypoints in the loop frame matched to query frame.
};
}  // namespace swift_vio
#endif  // INCLUDE_OKVIS_LOOP_FRAME_AND_MATCHES_HPP_


#include <votca/ctp/ewaldactor.h>
#include <boost/format.hpp>

namespace votca {
namespace ctp {

void EwdInteractor::FPU12_ShapeField_At_By(vector<PolarSeg*> &at,
    vector<PolarSeg*> &by, string shape, double V) {
    // This function requires neutrality of &by but gets around
    // the double sum (see ::F12_XYSlab_At_By)
    // ATTENTION Only increments FP, not FU

    double fx = 0.0;
    double fy = 0.0;
    double fz = 0.0;

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Compute net dipole moment of field-generating density (&by)
    vec Q1 = vec(0,0,0);
    vec U1 = vec(0,0,0);
    for (sit = by.begin(); sit < by.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            vec u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            // Increment moments
            Q1 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1 += q1;
            U1 += u1;
        }
    }

    // Shape-dependent fields
    if (shape == "xyslab") {
        fx = 0.0;
        fy = 0.0;
        fz = Q1.getZ() + U1.getZ();
        fx *= 4*M_PI/V;
        fy *= 4*M_PI/V;
        fz *= 4*M_PI/V;
    }
    else if (shape == "cube" || shape == "sphere") {
        fx = Q1.getX() + U1.getX();
        fy = Q1.getY() + U1.getY();
        fz = Q1.getZ() + U1.getZ();
        fx *= 4*M_PI/(3*V);
        fy *= 4*M_PI/(3*V);
        fz *= 4*M_PI/(3*V);
    }
    else if (shape == "none") {
    	fx = 0.0;
    	fy = 0.0;
    	fz = 0.0;
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    // Increment fields of target density (&at)
    for (sit = at.begin(); sit < at.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            (*pit)->FPx += fx;
            (*pit)->FPy += fy;
            (*pit)->FPz += fz;
        }
    }
    return;
}

void EwdInteractor::FP12_ShapeField_At_By(vector<PolarSeg*> &at,
    vector<PolarSeg*> &by, string shape, double V) {
    // This function requires neutrality of &by but gets around
    // the double sum (see ::F12_XYSlab_At_By)
    // ATTENTION Only increments FP, not FU

    double fx = 0.0;
    double fy = 0.0;
    double fz = 0.0;

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Compute net dipole moment of field-generating density (&by)
    vec Q1 = vec(0,0,0);
    for (sit = by.begin(); sit < by.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            // Increment moments
            Q1 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1 += q1;
        }
    }

    // Shape-dependent fields
    if (shape == "xyslab") {
        fx = 0.0;
        fy = 0.0;
        fz = Q1.getZ();
        fx *= 4*M_PI/V;
        fy *= 4*M_PI/V;
        fz *= 4*M_PI/V;
    }
    else if (shape == "cube" || shape == "sphere") {
        fx = Q1.getX();
        fy = Q1.getY();
        fz = Q1.getZ();
        fx *= 4*M_PI/(3*V);
        fy *= 4*M_PI/(3*V);
        fz *= 4*M_PI/(3*V);
    }
    else if (shape == "none") {
    	fx = 0.0;
    	fy = 0.0;
    	fz = 0.0;
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    // Increment fields of target density (&at)
    for (sit = at.begin(); sit < at.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            (*pit)->FPx += fx;
            (*pit)->FPy += fy;
            (*pit)->FPz += fz;
        }
    }
    return;
}

void EwdInteractor::FU12_ShapeField_At_By(vector<PolarSeg*> &at,
    vector<PolarSeg*> &by, string shape, double V) {
    // This function requires neutrality of &by but gets around
    // the double sum (see ::F12_XYSlab_At_By)
    // ATTENTION Only increments FP, not FU

    double fx = 0.0;
    double fy = 0.0;
    double fz = 0.0;

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Compute net dipole moment of field-generating density (&by)
    vec U1 = vec(0,0,0);
    for (sit = by.begin(); sit < by.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            vec u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            // Increment moments
            U1 += u1;
        }
    }

    // Shape-dependent fields
    if (shape == "xyslab") {
        fx = 0.0;
        fy = 0.0;
        fz = U1.getZ();
        fx *= 4*M_PI/V;
        fy *= 4*M_PI/V;
        fz *= 4*M_PI/V;
    }
    else if (shape == "cube" || shape == "sphere") {
        fx = U1.getX();
        fy = U1.getY();
        fz = U1.getZ();
        fx *= 4*M_PI/(3*V);
        fy *= 4*M_PI/(3*V);
        fz *= 4*M_PI/(3*V);
    }
    else if (shape == "none") {
		fx = 0.0;
		fy = 0.0;
		fz = 0.0;
	}
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    // Increment fields of target density (&at)
    for (sit = at.begin(); sit < at.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            (*pit)->FUx += fx;
            (*pit)->FUy += fy;
            (*pit)->FUz += fz;
        }
    }
    return;
}

void EwdInteractor::PhiPU12_ShapeField_At_By(vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, string shape, double V) {
    // NOTE : WITH PREFACTOR = -4*PI/V (xyslab) v -4*PI/3V (cube, sphere)
    // NOTE : Similar operations in PhiPU12_..., PhiP12_..., PhiU12_...

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Charge, dipole, quadrupole for <s2>:
    // Q... <> permanent, U... <> induced
    double Q0_S2 = 0.0;
    votca::tools::vec Q1_S2 = vec(0,0,0);
    votca::tools::vec U1_S2 = vec(0,0,0);
    votca::tools::matrix Q2_S2;
    Q2_S2.ZeroMatrix();
    votca::tools::matrix U2_S2;
    U2_S2.ZeroMatrix();

    for (sit = s2.begin(); sit != s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec    q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            vec    u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            matrix q2 = matrix(
                vec((*pit)->Qxx, (*pit)->Qxy, (*pit)->Qxz),
                vec((*pit)->Qxy, (*pit)->Qyy, (*pit)->Qyz),
                vec((*pit)->Qxz, (*pit)->Qyz, (*pit)->Qzz));
            // Charge
            Q0_S2 += q0;
            // Dipole
            Q1_S2 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1_S2 += q1;
            U1_S2 += u1;
            // Quadrupole
            Q2_S2 += 0.5*q0*(r|r);
            if ((*pit)->getRank() > 0) {
                Q2_S2 += (q1|r);
                if ((*pit)->getRank() > 1) {
                    Q2_S2 += q2;
                }
            }
            U2_S2 += (u1|r);
        }
    }

    // Traces
    double TrQ2_S2 = Q2_S2.get(0,0)+Q2_S2.get(1,1)+Q2_S2.get(2,2);
    double TrU2_S2 = U2_S2.get(0,0)+U2_S2.get(1,1)+U2_S2.get(2,2);

    // Increment potentials
    if (shape == "xyslab") {
        double prefac = -4*M_PI/V;
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiP += prefac*(Q2_S2.get(2,2) - (*pit)->getPos().getZ()*Q1_S2.getZ());
                (*pit)->PhiU += prefac*(U2_S2.get(2,2) - (*pit)->getPos().getZ()*U1_S2.getZ());
            }
        }
    }
    else if (shape == "cube" || shape == "sphere") {
        double prefac = -4*M_PI/(3*V);
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiP += prefac*(TrQ2_S2 - (*pit)->getPos()*Q1_S2);
                (*pit)->PhiU += prefac*(TrU2_S2 - (*pit)->getPos()*U1_S2);
            }
        }
    }
    else if (shape == "none") {
    	; // Nothing to do here
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    return;
}

void EwdInteractor::PhiP12_ShapeField_At_By(vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, string shape, double V) {
    // NOTE : WITH PREFACTOR = -4*PI/V (xyslab) v -4*PI/3V (cube, sphere)
    // NOTE : Similar operations in PhiPU12_..., PhiP12_..., PhiU12_...

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Charge, dipole, quadrupole for <s2>:
    // Q... <> permanent, U... <> induced
    double Q0_S2 = 0.0;
    votca::tools::vec Q1_S2 = vec(0,0,0);
    votca::tools::matrix Q2_S2;
    Q2_S2.ZeroMatrix();

    for (sit = s2.begin(); sit != s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec    q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            matrix q2 = matrix(
                vec((*pit)->Qxx, (*pit)->Qxy, (*pit)->Qxz),
                vec((*pit)->Qxy, (*pit)->Qyy, (*pit)->Qyz),
                vec((*pit)->Qxz, (*pit)->Qyz, (*pit)->Qzz));
            // Charge
            Q0_S2 += q0;
            // Dipole
            Q1_S2 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1_S2 += q1;
            // Quadrupole
            Q2_S2 += 0.5*q0*(r|r);
            if ((*pit)->getRank() > 0) {
                Q2_S2 += (q1|r);
                if ((*pit)->getRank() > 1) {
                    Q2_S2 += q2;
                }
            }
        }
    }

    // Traces
    double TrQ2_S2 = Q2_S2.get(0,0)+Q2_S2.get(1,1)+Q2_S2.get(2,2);

    // Increment potentials
    if (shape == "xyslab") {
        double prefac = -4*M_PI/V;
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiP += prefac*(Q2_S2.get(2,2) - (*pit)->getPos().getZ()*Q1_S2.getZ());
            }
        }
    }
    else if (shape == "cube" || shape == "sphere") {
        double prefac = -4*M_PI/(3*V);
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiP += prefac*(TrQ2_S2 - (*pit)->getPos()*Q1_S2);
            }
        }
    }
    else if (shape == "none") {
    	; // Nothing to do here
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    return;
}

void EwdInteractor::PhiU12_ShapeField_At_By(vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, string shape, double V) {
    // NOTE : WITH PREFACTOR = -4*PI/V (xyslab) v -4*PI/3V (cube, sphere)
    // NOTE : Similar operations in PhiPU12_..., PhiP12_..., PhiU12_...

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Charge, dipole, quadrupole for <s2>:
    // Q... <> permanent, U... <> induced
    votca::tools::vec U1_S2 = vec(0,0,0);
    votca::tools::matrix U2_S2;
    U2_S2.ZeroMatrix();

    for (sit = s2.begin(); sit != s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            vec    u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            // Dipole
            U1_S2 += u1;
            // Quadrupole
            U2_S2 += (u1|r);
        }
    }

    // Traces
    double TrU2_S2 = U2_S2.get(0,0)+U2_S2.get(1,1)+U2_S2.get(2,2);

    // Increment potentials
    if (shape == "xyslab") {
        double prefac = -4*M_PI/V;
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiU += prefac*(U2_S2.get(2,2) - (*pit)->getPos().getZ()*U1_S2.getZ());
            }
        }
    }
    else if (shape == "cube" || shape == "sphere") {
        double prefac = -4*M_PI/(3*V);
        for (sit = s1.begin(); sit != s1.end(); ++sit) {
            for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
                (*pit)->PhiU += prefac*(TrU2_S2 - (*pit)->getPos()*U1_S2);
            }
        }
    }
    else if (shape == "none") {
    	; // Nothing to do here
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    return;
}

//void EwdInteractor::FPU12_XYSlab_ShapeField_At_By(vector<PolarSeg*> &at,
//    vector<PolarSeg*> &by, double &TwoPi_V) {
//    // This function requires neutrality of &by but gets around
//    // the double sum (see ::F12_XYSlab_At_By)
//    // ATTENTION Only increments FPz, not FUz
//    double fz = 0.0;
//    vector<PolarSeg*>::iterator sit;
//    vector<APolarSite*> ::iterator pit;
//    for (sit = by.begin(); sit < by.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            fz += 2*( (*pit)->Q00*(*pit)->getPos().getZ() + (*pit)->U1z);
//            if ((*pit)->_rank > 0)
//                fz += 2*(*pit)->Q1z;
//        }
//    }
//    fz *= TwoPi_V;
//    // Increment fields
//    for (sit = at.begin(); sit < at.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            (*pit)->FPz += fz;
//        }
//    }
//    return;
//}
//
//
//void EwdInteractor::FP12_XYSlab_ShapeField_At_By(vector<PolarSeg*> &at,
//    vector<PolarSeg*> &by, double &TwoPi_V) {
//    // This function requires neutrality of &by but gets around
//    // the double sum (see ::F12_XYSlab_At_By)
//    double fz = 0.0;
//    vector<PolarSeg*>::iterator sit;
//    vector<APolarSite*> ::iterator pit;
//    for (sit = by.begin(); sit < by.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            fz += 2*( (*pit)->Q00*(*pit)->getPos().getZ() );
//            if ((*pit)->_rank > 0)
//                fz += 2*(*pit)->Q1z;
//        }
//    }
//    fz *= TwoPi_V;
//    // Increment fields
//    for (sit = at.begin(); sit < at.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            (*pit)->FPz += fz;
//        }
//    }
//    return;
//}
//
//
//void EwdInteractor::FU12_XYSlab_ShapeField_At_By(vector<PolarSeg*> &at,
//    vector<PolarSeg*> &by, double &TwoPi_V) {
//    // This function requires neutrality of &by but gets around
//    // the double sum (see ::F12_XYSlab_At_By)
//    double fz = 0.0;
//    vector<PolarSeg*>::iterator sit;
//    vector<APolarSite*> ::iterator pit;
//    for (sit = by.begin(); sit < by.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            fz += 2*(*pit)->U1z;
//        }
//    }
//    fz *= TwoPi_V;
//    // Increment fields
//    for (sit = at.begin(); sit < at.end(); ++sit) {
//        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
//            (*pit)->FUz += fz;
//        }
//    }
//    return;
//}

// ============================ RECIPROCAL SPACE ============================ //
//                                 S-FACTORS                                  //

EWD::cmplx EwdInteractor::PUStructureAmplitude(vector<PolarSeg*> &s) {
    // Requires ApplyBias(k) to be called previously
    double re_S = 0.0;
    double im_S = 0.0;
    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    for (sit = s.begin(); sit < s.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PUApplyBiasK(*(*pit));
            re_S += re_s;
            re_S += u_re_s;
            im_S += im_s;
            im_S += u_im_s;
        }
    }
    return EWD::cmplx(re_S, im_S);
}

EWD::cmplx EwdInteractor::PStructureAmplitude(vector<PolarSeg*> &s) {
    // Requires ApplyBias(k) to be called previously
    double re_S = 0.0;
    double im_S = 0.0;
    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    for (sit = s.begin(); sit < s.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PApplyBiasK(*(*pit));
            re_S += re_s;
            im_S += im_s;
        }
    }
    return EWD::cmplx(re_S, im_S);
}

EWD::cmplx EwdInteractor::UStructureAmplitude(vector<PolarSeg*> &s) {
    // Requires ApplyBias(k) to be called previously
    double re_S = 0.0;
    double im_S = 0.0;
    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    for (sit = s.begin(); sit < s.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            UApplyBiasK(*(*pit));
            re_S += u_re_s;
            im_S += u_im_s;
        }
    }
    return EWD::cmplx(re_S, im_S);
}

EWD::cmplx EwdInteractor::PUStructureAmplitude(vector<PolarSeg*> &s, const vec &k) {
    ApplyBiasK(k);
    return PUStructureAmplitude(s);
}

EWD::cmplx EwdInteractor::PStructureAmplitude(vector<PolarSeg*> &s, const vec &k) {
    ApplyBiasK(k);
    return PStructureAmplitude(s);
}

EWD::cmplx EwdInteractor::UStructureAmplitude(vector<PolarSeg*> &s, const vec &k) {
    ApplyBiasK(k);
    return UStructureAmplitude(s);
}

// ============================ RECIPROCAL SPACE ============================ //
//                                   FIELD                                    //

EWD::cmplx EwdInteractor::FP12_At_ByS2(const vec &k, vector<PolarSeg*> &s1,
    const EWD::cmplx &S2, double &rV) {
    // ATTENTION Increments PERMANENT fields of s1
    // ATTENTION Structure factor S2 from PERM & INDU moments of s2, k
    double re_S2 = S2._re;
    double im_S2 = - S2._im; // !! NOTE THE (-) !!

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    double sum_re_f_rms = 0.0;
    double sum_im_f_xyz = 0.0;
    int rms_count = 0;

    // Compute k-component of fields acting on s1 = A(c)
    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double fx = -rV*AK * kx * (sinkr*re_S2 + coskr*im_S2);
            double fy = -rV*AK * ky * (sinkr*re_S2 + coskr*im_S2);
            double fz = -rV*AK * kz * (sinkr*re_S2 + coskr*im_S2);

            (*pit)->FPx += fx;
            (*pit)->FPy += fy;
            (*pit)->FPz += fz;

            // Imaginary component (error check)
            double ifx = -rV*AK * kx * (sinkr*im_S2 - coskr*re_S2);
            double ify = -rV*AK * ky * (sinkr*im_S2 - coskr*re_S2);
            double ifz = -rV*AK * kz * (sinkr*im_S2 - coskr*re_S2);

            rms_count += 1;
            sum_re_f_rms += fx*fx + fy*fy + fz*fz;
            sum_im_f_xyz += ifx + ify + ifz;
        }
    }

    sum_re_f_rms /= rms_count;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_f_rms, sum_im_f_xyz);
}

EWD::cmplx EwdInteractor::FU12_At_ByS2(const vec &k, vector<PolarSeg*> &s1,
    const EWD::cmplx &S2, double &rV) {
    // ATTENTION Increments PERMANENT fields of s1
    // ATTENTION Structure factor S2 from PERM & INDU moments of s2, k
    double re_S2 = S2._re;
    double im_S2 = - S2._im; // !! NOTE THE (-) !!

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    double sum_re_f_rms = 0.0;
    double sum_im_f_xyz = 0.0;
    int rms_count = 0;

    // Compute k-component of fields acting on s1 = A(c)
    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double fx = -rV*AK * kx * (sinkr*re_S2 + coskr*im_S2);
            double fy = -rV*AK * ky * (sinkr*re_S2 + coskr*im_S2);
            double fz = -rV*AK * kz * (sinkr*re_S2 + coskr*im_S2);

            (*pit)->FUx += fx;
            (*pit)->FUy += fy;
            (*pit)->FUz += fz;

            // Imaginary component (error check)
            double ifx = -rV*AK * kx * (sinkr*im_S2 - coskr*re_S2);
            double ify = -rV*AK * ky * (sinkr*im_S2 - coskr*re_S2);
            double ifz = -rV*AK * kz * (sinkr*im_S2 - coskr*re_S2);

            rms_count += 1;
            sum_re_f_rms += fx*fx + fy*fy + fz*fz;
            sum_im_f_xyz += ifx + ify + ifz;
        }
    }

    sum_re_f_rms /= rms_count;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_f_rms, sum_im_f_xyz);
}

EWD::cmplx EwdInteractor::FPU12_AS1S2_At_By(const vec &k,
    vector<PolarSeg*> &s1, vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments PERMANENT fields of s1
    // ATTENTION Structure factors include PERMANENT & INDUCED moments of s2

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    double sum_re_f_rms = 0.0;
    double sum_im_f_xyz = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = PUStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of fields acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double fx = -rV*AK * kx * (sinkr*re_S2 + coskr*im_S2);
            double fy = -rV*AK * ky * (sinkr*re_S2 + coskr*im_S2);
            double fz = -rV*AK * kz * (sinkr*re_S2 + coskr*im_S2);

            (*pit)->FPx += fx;
            (*pit)->FPy += fy;
            (*pit)->FPz += fz;

            // Imaginary component (error check)
            double ifx = -rV*AK * kx * (sinkr*im_S2 - coskr*re_S2);
            double ify = -rV*AK * ky * (sinkr*im_S2 - coskr*re_S2);
            double ifz = -rV*AK * kz * (sinkr*im_S2 - coskr*re_S2);

            rms_count += 1;
            sum_re_f_rms += fx*fx + fy*fy + fz*fz;
            sum_im_f_xyz += ifx + ify + ifz;
        }
    }

    sum_re_f_rms /= rms_count;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_f_rms, sum_im_f_xyz);
}

EWD::cmplx EwdInteractor::FP12_AS1S2_At_By(const vec &k,
    vector<PolarSeg*> &s1, vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments PERMANENT fields of s1
    // ATTENTION Structure factors include PERMANENT moments of s2

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    double sum_re_f_rms = 0.0;
    double sum_im_f_xyz = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = PStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of fields acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double fx = -rV*AK * kx * (sinkr*re_S2 + coskr*im_S2);
            double fy = -rV*AK * ky * (sinkr*re_S2 + coskr*im_S2);
            double fz = -rV*AK * kz * (sinkr*re_S2 + coskr*im_S2);

            (*pit)->FPx += fx;
            (*pit)->FPy += fy;
            (*pit)->FPz += fz;

            // Imaginary component (error check)
            double ifx = -rV*AK * kx * (sinkr*im_S2 - coskr*re_S2);
            double ify = -rV*AK * ky * (sinkr*im_S2 - coskr*re_S2);
            double ifz = -rV*AK * kz * (sinkr*im_S2 - coskr*re_S2);

            rms_count += 1;
            sum_re_f_rms += fx*fx + fy*fy + fz*fz;
            sum_im_f_xyz += ifx + ify + ifz;
        }
    }

    sum_re_f_rms /= rms_count;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_f_rms, sum_im_f_xyz);
}

EWD::cmplx EwdInteractor::FU12_AS1S2_At_By(const vec &k,
    vector<PolarSeg*> &s1, vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments INDUCED fields of s1
    // ATTENTION Structure factors include INDUCED moments of s2

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    double sum_re_f_rms = 0.0;
    double sum_im_f_xyz = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = UStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of fields acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double fx = -rV*AK * kx * (sinkr*re_S2 + coskr*im_S2);
            double fy = -rV*AK * ky * (sinkr*re_S2 + coskr*im_S2);
            double fz = -rV*AK * kz * (sinkr*re_S2 + coskr*im_S2);

            (*pit)->FUx += fx;
            (*pit)->FUy += fy;
            (*pit)->FUz += fz;

            // Imaginary component (error check)
            double ifx = -rV*AK * kx * (sinkr*im_S2 - coskr*re_S2);
            double ify = -rV*AK * ky * (sinkr*im_S2 - coskr*re_S2);
            double ifz = -rV*AK * kz * (sinkr*im_S2 - coskr*re_S2);

            rms_count += 1;
            sum_re_f_rms += fx*fx + fy*fy + fz*fz;
            sum_im_f_xyz += ifx + ify + ifz;
        }
    }

    sum_re_f_rms /= rms_count;

    // NOTE sum_re_f_rms => convergence check (to be performed by caller)
    // NOTE sum_im_f_xyz => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_f_rms, sum_im_f_xyz);
}

// ============================ RECIPROCAL SPACE ============================ //
//                                 POTENTIALS                                 //

EWD::cmplx EwdInteractor::PhiPU12_AS1S2_At_By(const vec &k, vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments *permanent* potential PhiP of s1 only
    // ATTENTION Structure factors include PERMANENT & INDUCED moments of s2
    // NOTE Analogous operations in PhiPU12_..., PhiP12_..., PhiU12_...

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi_xyz => sanity check      (to be performed by caller)
    double sum_re_phi_ms = 0.0;
    double sum_im_phi = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = PUStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of potential acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double phi  = rV*AK * (coskr*re_S2 - sinkr*im_S2);
            (*pit)->PhiP += phi;

            // Imaginary component (error check)
            double iphi = rV*AK * (coskr*im_S2 + sinkr*re_S2);

            rms_count += 1;
            sum_re_phi_ms += phi*phi;
            sum_im_phi += iphi;
        }
    }

    sum_re_phi_ms /= rms_count;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi     => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_phi_ms, sum_im_phi);
}

EWD::cmplx EwdInteractor::PhiP12_AS1S2_At_By(const vec &k, vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments *permanent* potential PhiP of s1 only
    // ATTENTION Structure factors include PERMANENT moments of s2 only
    // NOTE Analogous operations in PhiPU12_..., PhiP12_..., PhiU12_...

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi_xyz => sanity check      (to be performed by caller)
    double sum_re_phi_ms = 0.0;
    double sum_im_phi = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = PStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of potential acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double phi  = rV*AK * (coskr*re_S2 - sinkr*im_S2);
            (*pit)->PhiP += phi;

            // Imaginary component (error check)
            double iphi = rV*AK * (coskr*im_S2 + sinkr*re_S2);

            rms_count += 1;
            sum_re_phi_ms += phi*phi;
            sum_im_phi += iphi;
        }
    }

    sum_re_phi_ms /= rms_count;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi     => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_phi_ms, sum_im_phi);
}

EWD::cmplx EwdInteractor::PhiU12_AS1S2_At_By(const vec &k, vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, double &rV) {
    // ATTENTION Increments *permanent* potential PhiP of s1 only
    // ATTENTION Structure factors include PERMANENT moments of s2 only
    // NOTE Analogous operations in PhiPU12_..., PhiP12_..., PhiU12_...

    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi_xyz => sanity check      (to be performed by caller)
    double sum_re_phi_ms = 0.0;
    double sum_im_phi = 0.0;
    int rms_count = 0;

    // Structure amplitude S2* from s2 = B*
    EWD::cmplx cmplx_S2 = UStructureAmplitude(s2).Conjugate();
    double re_S2 = cmplx_S2._re;
    double im_S2 = cmplx_S2._im;

    // Compute k-component of potential acting on s1 = A(c)
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            kr = kx * (*pit)->getPos().getX()
               + ky * (*pit)->getPos().getY()
               + kz * (*pit)->getPos().getZ();
            coskr = cos(kr);
            sinkr = sin(kr);

            // Real component
            double phi  = rV*AK * (coskr*re_S2 - sinkr*im_S2);
            (*pit)->PhiU += phi;

            // Imaginary component (error check)
            double iphi = rV*AK * (coskr*im_S2 + sinkr*re_S2);

            rms_count += 1;
            sum_re_phi_ms += phi*phi;
            sum_im_phi += iphi;
        }
    }

    sum_re_phi_ms /= rms_count;

    // NOTE sum_re_phi_rms => convergence check (to be performed by caller)
    // NOTE sum_im_phi     => sanity check      (to be performed by caller)
    return EWD::cmplx(sum_re_phi_ms, sum_im_phi);
}

// ============================ RECIPROCAL SPACE ============================ //
//                                  ENERGIES                                  //

EWD::triple<EWD::cmplx> EwdInteractor::AS1S2(const vec &k,
    vector<PolarSeg*> &s1, vector<PolarSeg*> &s2) {
    // NOTE : w/o 1/V
    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // Structure amplitude S1
    double re_S1 = 0.0;
    double im_S1 = 0.0;
    double u_re_S1 = 0.0;
    double u_im_S1 = 0.0;
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PUApplyBiasK(*(*pit));
            re_S1 += re_s;
            im_S1 += im_s;     // NOTE THE (+)
            u_re_S1 += u_re_s;
            u_im_S1 += u_im_s; // NOTE THE (+)
        }
    }

    // Structure amplitude S2
    double re_S2 = 0.0;
    double im_S2 = 0.0;
    double u_re_S2 = 0.0;
    double u_im_S2 = 0.0;
    for (sit = s2.begin(); sit < s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PUApplyBiasK(*(*pit));
            re_S2 += re_s;
            im_S2 -= im_s;     // NOTE THE (-)
            u_re_S2 += u_re_s;
            u_im_S2 -= u_im_s; // NOTE THE (-)
        }
    }

    double pp_re_AS1S2 = AK * (re_S1*re_S2 - im_S1*im_S2);
    double pp_im_AS1S2 = AK * (re_S1*im_S2 + im_S1*re_S2);

    double uu_re_AS1S2 = AK * (u_re_S1*u_re_S2 - u_im_S1*u_im_S2);
    double uu_im_AS1S2 = AK * (u_re_S1*u_im_S2 + u_im_S1*u_re_S2);

    double pu_re_AS1S2 = AK * (u_re_S1*re_S2 + re_S1*u_re_S2 - u_im_S1*im_S2 - im_S1*u_im_S2);
    double pu_im_AS1S2 = AK * (u_re_S1*im_S2 + re_S1*u_im_S2 + u_im_S1*re_S2 + im_S1*u_re_S2);

    return EWD::triple<EWD::cmplx>(EWD::cmplx(pp_re_AS1S2, pp_im_AS1S2),
                                   EWD::cmplx(pu_re_AS1S2, pu_im_AS1S2),
                                   EWD::cmplx(uu_re_AS1S2, uu_im_AS1S2));
}

EWD::triple<EWD::cmplx> EwdInteractor::S1S2(const vec &k,
    vector<PolarSeg*> &s1, vector<PolarSeg*> &s2) {
    // NOTE : w/o 1/V
    ApplyBiasK(k);

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // Structure amplitude S1
    double re_S1 = 0.0;
    double im_S1 = 0.0;
    double u_re_S1 = 0.0;
    double u_im_S1 = 0.0;
    for (sit = s1.begin(); sit < s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PUApplyBiasK(*(*pit));
            re_S1 += re_s;
            im_S1 += im_s;     // NOTE THE (+)
            u_re_S1 += u_re_s;
            u_im_S1 += u_im_s; // NOTE THE (+)
        }
    }

    // Structure amplitude S2
    double re_S2 = 0.0;
    double im_S2 = 0.0;
    double u_re_S2 = 0.0;
    double u_im_S2 = 0.0;
    for (sit = s2.begin(); sit < s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            PUApplyBiasK(*(*pit));
            re_S2 += re_s;
            im_S2 -= im_s;     // NOTE THE (-)
            u_re_S2 += u_re_s;
            u_im_S2 -= u_im_s; // NOTE THE (-)
        }
    }

    double pp_re_S1S2 = (re_S1*re_S2 - im_S1*im_S2);
    double pp_im_S1S2 = (re_S1*im_S2 + im_S1*re_S2);

    double uu_re_S1S2 = (u_re_S1*u_re_S2 - u_im_S1*u_im_S2);
    double uu_im_S1S2 = (u_re_S1*u_im_S2 + u_im_S1*u_re_S2);

    double pu_re_S1S2 = (u_re_S1*re_S2 + re_S1*u_re_S2 - u_im_S1*im_S2 - im_S1*u_im_S2);
    double pu_im_S1S2 = (u_re_S1*im_S2 + re_S1*u_im_S2 + u_im_S1*re_S2 + im_S1*u_re_S2);

    return EWD::triple<EWD::cmplx>(EWD::cmplx(pp_re_S1S2, pp_im_S1S2),
                                   EWD::cmplx(pu_re_S1S2, pu_im_S1S2),
                                   EWD::cmplx(uu_re_S1S2, uu_im_S1S2));
}

EWD::triple<double> EwdInteractor::U12_ShapeTerm(vector<PolarSeg*> &s1,
    vector<PolarSeg*> &s2, string shape, double V, Logger *log) {

    // NOTE : WITH PREFACTOR = -4*PI/V (xylab) v -4*PI/3V (cube, sphere)
    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*>::iterator pit;

    // Charge, dipole, quadrupole for <s1>: Q... <> permanent, U... <> induced
    double Q0_S1 = 0.0;
    votca::tools::vec Q1_S1 = vec(0,0,0);
    votca::tools::vec U1_S1 = vec(0,0,0);
    votca::tools::matrix Q2_S1;
    Q2_S1.ZeroMatrix();
    votca::tools::matrix U2_S1;
    U2_S1.ZeroMatrix();

    for (sit = s1.begin(); sit != s1.end(); ++sit) {
        for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
            vec    r  = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec    q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            vec    u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            matrix q2 = matrix(
                vec((*pit)->Qxx, (*pit)->Qxy, (*pit)->Qxz),
                vec((*pit)->Qxy, (*pit)->Qyy, (*pit)->Qyz),
                vec((*pit)->Qxz, (*pit)->Qyz, (*pit)->Qzz));
            // Charge
            Q0_S1 += q0;
            // Dipole
            Q1_S1 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1_S1 += q1;
            U1_S1 += u1;
            // Quadrupole
            Q2_S1 += 0.5*q0*(r|r);
            if ((*pit)->getRank() > 0) {
                Q2_S1 += (q1|r);
                if ((*pit)->getRank() > 1) {
                    Q2_S1 += q2;
                }
            }
            U2_S1 += (u1|r);
        }
    }

    // Charge, dipole, quadrupole for <s2>: Q... <> permanent, U... <> induced
    double Q0_S2 = 0.0;
    votca::tools::vec Q1_S2 = vec(0,0,0);
    votca::tools::vec U1_S2 = vec(0,0,0);
    votca::tools::matrix Q2_S2;
    Q2_S2.ZeroMatrix();
    votca::tools::matrix U2_S2;
    U2_S2.ZeroMatrix();

    for (sit = s2.begin(); sit != s2.end(); ++sit) {
        for (pit = (*sit)->begin(); pit != (*sit)->end(); ++pit) {
            vec r = (*pit)->getPos();
            double q0 = (*pit)->Q00;
            vec    q1 = vec((*pit)->Q1x, (*pit)->Q1y, (*pit)->Q1z);
            vec    u1 = vec((*pit)->U1x, (*pit)->U1y, (*pit)->U1z);
            matrix q2 = matrix(
                vec((*pit)->Qxx, (*pit)->Qxy, (*pit)->Qxz),
                vec((*pit)->Qxy, (*pit)->Qyy, (*pit)->Qyz),
                vec((*pit)->Qxz, (*pit)->Qyz, (*pit)->Qzz));
            // Charge
            Q0_S2 += q0;
            // Dipole
            Q1_S2 += q0*r;
            if ((*pit)->getRank() > 0)
                Q1_S2 += q1;
            U1_S2 += u1;
            // Quadrupole
            Q2_S2 += 0.5*q0*(r|r);
            if ((*pit)->getRank() > 0) {
                Q2_S2 += (q1|r);
                if ((*pit)->getRank() > 1) {
                    Q2_S2 += q2;
                }
            }
            U2_S2 += (u1|r);
        }
    }

    // Traces
    double TrQ2_S1 = Q2_S1.get(0,0)+Q2_S1.get(1,1)+Q2_S1.get(2,2);
    double TrU2_S1 = U2_S1.get(0,0)+U2_S1.get(1,1)+U2_S1.get(2,2);
    double TrQ2_S2 = Q2_S2.get(0,0)+Q2_S2.get(1,1)+Q2_S2.get(2,2);
    double TrU2_S2 = U2_S2.get(0,0)+U2_S2.get(1,1)+U2_S2.get(2,2);

    if (log != NULL) {
        CTP_LOG(logDEBUG, *log) << "S1 moments: " << flush;
        CTP_LOG(logDEBUG, *log) << "  Q0   = "
            << (boost::format("%1$+1.7e") % Q0_S1) << flush;
        CTP_LOG(logDEBUG, *log) << "  Q1   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q1_S1.getX() % Q1_S1.getY() % Q1_S1.getZ()) << flush;
        CTP_LOG(logDEBUG, *log) << "  U1   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U1_S1.getX() % U1_S1.getY() % U1_S1.getZ()) << flush;

        CTP_LOG(logDEBUG, *log) << "  Q2   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S1.get(0,0) % Q2_S1.get(0,1) % Q2_S1.get(0,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S1.get(1,0) % Q2_S1.get(1,1) % Q2_S1.get(1,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S1.get(2,0) % Q2_S1.get(2,1) % Q2_S1.get(2,2)) << flush;

        CTP_LOG(logDEBUG, *log) << "  U2   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S1.get(0,0) % U2_S1.get(0,1) % U2_S1.get(0,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S1.get(1,0) % U2_S1.get(1,1) % U2_S1.get(1,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S1.get(2,0) % U2_S1.get(2,1) % U2_S1.get(2,2)) << flush;

        CTP_LOG(logDEBUG, *log) << "  TrQ2 = " << TrQ2_S1 << flush;
        CTP_LOG(logDEBUG, *log) << "  TrU2 = " << TrU2_S1 << flush;

        CTP_LOG(logDEBUG, *log) << "S2 moments: " << flush;
        CTP_LOG(logDEBUG, *log) << "  Q0   = "
            << (boost::format("%1$+1.7e") % Q0_S2) << flush;
        CTP_LOG(logDEBUG, *log) << "  Q1   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q1_S2.getX() % Q1_S2.getY() % Q1_S2.getZ()) << flush;
        CTP_LOG(logDEBUG, *log) << "  U1   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U1_S2.getX() % U1_S2.getY() % U1_S2.getZ()) << flush;

        CTP_LOG(logDEBUG, *log) << "  Q2   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S2.get(0,0) % Q2_S2.get(0,1) % Q2_S2.get(0,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S2.get(1,0) % Q2_S2.get(1,1) % Q2_S2.get(1,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % Q2_S2.get(2,0) % Q2_S2.get(2,1) % Q2_S2.get(2,2)) << flush;

        CTP_LOG(logDEBUG, *log) << "  U2   = "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S2.get(0,0) % U2_S2.get(0,1) % U2_S2.get(0,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S2.get(1,0) % U2_S2.get(1,1) % U2_S2.get(1,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "         "
            << (boost::format("%1$+1.7e %2$+1.7e %3$+1.7e")
                % U2_S2.get(2,0) % U2_S2.get(2,1) % U2_S2.get(2,2)) << flush;
        CTP_LOG(logDEBUG, *log) << "  TrQ2 = " << TrQ2_S2 << flush;
        CTP_LOG(logDEBUG, *log) << "  TrU2 = " << TrU2_S2 << flush;
    }

    // Shape-dependent energies
    double pp = 0.0;
    double pu = 0.0;
    double uu = 0.0;

    if (shape == "xyslab") {
        pp = Q0_S1*Q2_S2.get(2,2)
           + Q0_S2*Q2_S1.get(2,2)
           - Q1_S1.getZ()*Q1_S2.getZ();
        pu = Q0_S1*U2_S2.get(2,2)
           + Q0_S2*U2_S1.get(2,2)
           - Q1_S1.getZ()*U1_S2.getZ()
           - Q1_S2.getZ()*U1_S1.getZ();
        uu = - U1_S1.getZ()*U1_S2.getZ();
        pp *= -4*M_PI/V;
        pu *= -4*M_PI/V;
        uu *= -4*M_PI/V;

        double dd = - Q1_S1.getZ()*Q1_S2.getZ()
                    - Q1_S1.getZ()*U1_S2.getZ()
                    - Q1_S2.getZ()*U1_S1.getZ();
        double qQ =   Q0_S1*Q2_S2.get(2,2)
                    + Q0_S2*Q2_S1.get(2,2)
                    + Q0_S1*U2_S2.get(2,2)
                    + Q0_S2*U2_S1.get(2,2);

        dd *= -4*M_PI/V * EWD::int2eV;
        qQ *= -4*M_PI/V * EWD::int2eV;

        CTP_LOG(logDEBUG, *log) << (boost::format("  DD %1$+1.7e eV qQ %2$+1.7e eV")
                % dd % qQ) << flush;

    }
    else if (shape == "cube" || shape == "sphere") {
        pp = Q0_S1*TrQ2_S2 + Q0_S2*TrQ2_S1 - Q1_S1*Q1_S2;
        pu = Q0_S1*TrU2_S2 + Q0_S2*TrU2_S1 - Q1_S1*U1_S2 - Q1_S2*U1_S1;
        uu = - U1_S1*U1_S2;
        pp *= -4*M_PI/(3*V);
        pu *= -4*M_PI/(3*V);
        uu *= -4*M_PI/(3*V);

        if (log != NULL) {
            double pp_qQ = Q0_S1*TrQ2_S2 + Q0_S2*TrQ2_S1;
            double pp_dd =  - Q1_S1*Q1_S2;
            double pu_qQ = Q0_S1*TrU2_S2 + Q0_S2*TrU2_S1;
            double pu_dd =  - Q1_S1*U1_S2 - Q1_S2*U1_S1;
            double uu_dd = - U1_S1*U1_S2;
            pp_qQ *= -4*M_PI/(3*V)*EWD::int2eV;
            pp_dd *= -4*M_PI/(3*V)*EWD::int2eV;
            pu_qQ *= -4*M_PI/(3*V)*EWD::int2eV;
            pu_dd *= -4*M_PI/(3*V)*EWD::int2eV;
            uu_dd *= -4*M_PI/(3*V)*EWD::int2eV;
            CTP_LOG(logDEBUG, *log) << (boost::format("Energy-moment decomposition")) << flush;
            CTP_LOG(logDEBUG, *log) << (boost::format("  qQ (pp) = %1$+1.7e") % pp_qQ) << flush;
            CTP_LOG(logDEBUG, *log) << (boost::format("  dd (pp) = %1$+1.7e") % pp_dd) << flush;
            CTP_LOG(logDEBUG, *log) << (boost::format("  qQ (pu) = %1$+1.7e") % pu_qQ) << flush;
            CTP_LOG(logDEBUG, *log) << (boost::format("  dd (pu) = %1$+1.7e") % pu_dd) << flush;
            CTP_LOG(logDEBUG, *log) << (boost::format("  dd (uu) = %1$+1.7e") % uu_dd) << flush;
        }
    }
    else if (shape == "none") {
    	CTP_LOG(logDEBUG, *log) << (boost::format("Assuming isotropic limit")) << flush;
    	pp = 0.0;
    	pu = 0.0;
    	uu = 0.0;
    }
    else {
        cout << endl;
        throw std::runtime_error("Shape '" + shape + "' not implemented");
    }

    //cout << endl << "S1 " << Q0_S1 << " " << Q1_S1 << " " << TrQ2_S1 << flush;
    //cout << endl << "S2 " << Q0_S2 << " " << Q1_S2 << " " << TrQ2_S2 << flush;

    return EWD::triple<double>(pp, pu, uu);
}

}}