#pragma once

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Odometry.h>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>

#include <Eigen/Dense>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

// for recording timestamp
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>

int check_element_in_vector(const int element, const VectorXi &vec_check)
{
  for (int i=0;i<vec_check.rows();i++)
    if (element==vec_check(i))
	return i;
  return -1;
}

bool read_evaluation_img_list(const std::string truth_img_list, VectorXi& image_list)
{
    if (std::ifstream(truth_img_list))
    {
	std::ifstream fPoses;
	fPoses.open(truth_img_list.c_str());
	int counter=0;
	std::vector<int> image_list_v;
	while(!fPoses.eof()){
	    std::string s;
	    std::getline(fPoses,s);
	    if(!s.empty()){
		stringstream ss;
		ss << s;
		int t;
		ss>>t;
		image_list_v.push_back(t);
		counter++;
	    }
	}
	fPoses.close();
	image_list.resize(counter);
	for (int i=0;i<counter;i++)
	{
	  image_list(i)=image_list_v[i];
	}
	return true;
    }
    else
      return false;
}

bool read_all_pose(const std::string trajectory_file, const int total_img_number,Eigen::MatrixXf& all_poses)
{
    if (std::ifstream(trajectory_file))
    {
      all_poses.resize(total_img_number,12);
      std::ifstream fPoses;
      fPoses.open(trajectory_file.c_str());
      int counter=0;
      while(!fPoses.eof()){
	  std::string s;
	  std::getline(fPoses,s);
	  if(!s.empty()){
	      std::stringstream ss;
	      ss << s;
	      float t;
	      for (int i=0;i<12;i++){
		  ss >> t;
		  all_poses(counter,i)=t;
	      }
	      counter++;
	      if (counter>=total_img_number)
		break;
	  }
      }
      fPoses.close();
      return true;
    }
    else
      return false;
}

bool read_label_prob(const std::string label_txt, MatrixXf_row& frame_label_prob)  // assumed mat size correct
{
    std::ifstream fLables;
    fLables.open(label_txt.c_str());
    int counter=0;
    while(!fLables.eof()){
	std::string s;
	std::getline(fLables,s);
	if(!s.empty()){
	    std::stringstream ss;
	    ss << s;
	    float t;
	    for (int i=0;i<frame_label_prob.cols();i++){
		ss >> t;
		frame_label_prob(counter,i)=t;
	    }
	    counter++;
	}
    }
    fLables.close();
}

bool read_label_prob_bin(const std::string label_bin, MatrixXf_row& frame_label_prob)  // assumed mat size correct
{
    if (std::ifstream(label_bin))
    {
	std::ifstream fLables(label_bin.c_str(),std::ios::in|ios::binary);
	if (fLables.is_open()){
	    int mat_byte_size=sizeof(float)*frame_label_prob.rows()*frame_label_prob.cols(); // byte number, make sure size is correct, or can use tellg to get the file size
	    float *mat_field=frame_label_prob.data();

	    fLables.read((char*)mat_field,mat_byte_size);
	    fLables.close();
	}
	else{
	  ROS_ERROR_STREAM("Cannot open bianry label file "<<label_bin);
	  return false;
	}
	return true;
    }
    else
      return false;
}

bool read_label_prob_mat(const std::string label_txt, cv::Mat& frame_label_mat, Eigen::MatrixXf& frame_label_prob)  // assumed mat size correct
{
    std::ifstream fLables;
    fLables.open(label_txt.c_str());
    int counter=0;
    while(!fLables.eof()){
	std::string s;
	std::getline(fLables,s);
	float t;
	if(!s.empty()){
	    std::stringstream ss;
	    ss << s;
	    for (int i=0;i<frame_label_mat.cols;i++){
		ss >> t;
		frame_label_mat.at<float>(counter,i)=t;
	    }
	    counter++;
	}
    }
    fLables.close();
    cv::Mat large_label_mat;
    cv::resize(frame_label_mat,large_label_mat,cv::Size(),2,2);  //WRONG, resize is not easy 3D
    Eigen::Map<MatrixXf> frame_label_prob2(large_label_mat.ptr<float>(),large_label_mat.rows,large_label_mat.cols);
    std::cout<<frame_label_prob2.cols()<<" "<<frame_label_prob2.rows()<<std::endl;
}

nav_msgs::Odometry mattoOdom(const Eigen::Matrix4f transToWorld){
    nav_msgs::Odometry odom_msg;
    odom_msg.pose.pose.position.x=transToWorld(0,3);
    odom_msg.pose.pose.position.y=transToWorld(1,3);
    odom_msg.pose.pose.position.z=transToWorld(2,3);
    Matrix3f pose_rot=transToWorld.block(0,0,3,3);
    Eigen::Quaternionf pose_quat(pose_rot);

    odom_msg.pose.pose.orientation.w=pose_quat.w();
    odom_msg.pose.pose.orientation.x=pose_quat.x();
    odom_msg.pose.pose.orientation.y=pose_quat.y();
    odom_msg.pose.pose.orientation.z=pose_quat.z();
    return odom_msg;
}

Eigen::VectorXf homo_to_real_coord_vec(const Eigen::VectorXf &pt_homo_in){
    Eigen::VectorXf pt_out;
    if (pt_homo_in.rows()==4)
      pt_out=pt_homo_in.head(3)/pt_homo_in(3);
    else if (pt_homo_in.rows()==3)
      pt_out=pt_homo_in.head(2)/pt_homo_in(2);
    return pt_out;
}
Eigen::VectorXf real_to_homo_coord_vec(const Eigen::VectorXf &pt_in)
{
    Eigen::VectorXf pt_homo_out;
    int raw_rows=pt_in.rows();

    pt_homo_out.resize(raw_rows+1);
    pt_homo_out<<pt_in,
		 1;
    return pt_homo_out;
}

/**
 * given a bounding box represented as two corners,
 * calculate the lines representing the edges of this box.
 * each line is represented as a pair of points in bb.
 */
template<class Scalar>
void BoundingBoxToLines(const Eigen::Matrix<Scalar, 3, 1>& min_pt,
                        const Eigen::Matrix<Scalar, 3, 1>& max_pt,
                        std::vector<geometry_msgs::Point>& bb) {

  Eigen::Matrix<Scalar, 3, 2> minmax;
  minmax.col(0) = min_pt;
  minmax.col(1) = max_pt;
  static const int seq[3*24] = {
    0,0,0,
    0,1,0,
    0,1,0,
    1,1,0,
    1,1,0,
    1,0,0,
    1,0,0,
    1,0,1,
    1,0,1,
    1,1,1,
    1,1,1,
    0,1,1,
    0,1,1,
    0,0,1,
    0,0,1,
    0,0,0,
    0,1,0,
    0,1,1,
    1,1,1,
    1,1,0,
    0,0,1,
    1,0,1,
    1,0,0,
    0,0,0
  };

  for (int row=0; row < 24; ++row) {
    int i = seq[3*row + 0];
    int j = seq[3*row + 1];
    int k = seq[3*row + 2];
    geometry_msgs::Point pm;
    pm.x = minmax(0, i);
    pm.y = minmax(1, j);
    pm.z = minmax(2, k);
    bb.push_back(pm);
  }

}

/**
 * make a wireframe box marker out of the box represented by min_pt, max_pt
 */
template<class Scalar>
void WireframeBoxMarker(const Eigen::Matrix<Scalar, 3, 1>& min_pt,
                        const Eigen::Matrix<Scalar, 3, 1>& max_pt,
                        visualization_msgs::Marker& bb_marker) {
  bb_marker.ns = "bb";
  bb_marker.id = 0;
  bb_marker.type = visualization_msgs::Marker::LINE_LIST;
  bb_marker.action = visualization_msgs::Marker::ADD;
  bb_marker.scale.x = 0.25;
  bb_marker.color.r = 0.5;
  bb_marker.color.g = 1.0;
  bb_marker.color.b = 0.2;
  bb_marker.color.a = 1.0;
  bb_marker.lifetime = ros::Duration();
  BoundingBoxToLines(min_pt, max_pt, bb_marker.points);
}

// Vectors should just be rotated as it is vector not a point, but for backwards compatibility the default does both rotation and translation
void transformVector3f(const tf::Transform &transform_c,
                       Eigen::Vector3f &vec, bool vector_just_does_rotation = false) {
  tf::Vector3 v;
  tf::Transform transform = transform_c;
  if(vector_just_does_rotation){
      tf::Vector3 zeros(0.0,0.0,0.0);
      transform.setOrigin(zeros);
  }
#if ROS_VERSION_MINIMUM(1, 9, 50) // groovy
  tf::vectorEigenToTF(vec.cast<double>(), v);
  Eigen::Vector3d vecd;
  tf::vectorTFToEigen(transform*v, vecd);
  vec = vecd.cast<float>();
#else
  tf::VectorEigenToTF(vec.cast<double>(), v);
  Eigen::Vector3d vecd;
  tf::VectorTFToEigen(transform*v, vecd);
  vec = vecd.cast<float>();
#endif
}

bool getCoordinateTransform(const tf::TransformListener &listener,
                            std::string fromFrame,
                            std::string toFrame,
                            double waitTime,
                            tf::StampedTransform &transform) {
  bool flag = true;
  double timeResolution = 0.001;
  unsigned int timeout = ceil(waitTime/timeResolution);
  unsigned int count = 0;

  while(flag) {
    flag = false;
    try {
      count++;
      listener.lookupTransform(toFrame,fromFrame,ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      flag = true;
      if(count>timeout) {
        flag = false;
        ROS_ERROR_STREAM("Cannot find transform from::"<<fromFrame << " to::"<< toFrame);
        return flag;
      }
      ros::Duration(timeResolution).sleep();
    }
  }
  return (!flag);
}


/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// test_iterators.cpp

// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com .
// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithm>
#include <list>

#if (defined _MSC_VER) && (_MSC_VER == 1200)
#  pragma warning (disable : 4786) // too long name, harmless warning
#endif

#include <cstdlib>
#include <cstddef> // size_t

#include <boost/config.hpp>
#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{
    using ::rand;
    using ::size_t;
}
#endif

#include <boost/serialization/pfto.hpp>

#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/insert_linebreaks.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <boost/archive/iterators/transform_width.hpp>

#include "test_tools.hpp"

#include <iostream>

template<typename CharType>
void test_base64(){
    CharType rawdata[150];
    std::size_t size = sizeof(rawdata) / sizeof(CharType);
    CharType * rptr;
    for(rptr = rawdata + size; rptr-- > rawdata;)
        *rptr = static_cast<CharType>(std::rand());

    // convert to base64
    typedef std::list<CharType> text_base64_type;
    text_base64_type text_base64;

    typedef
        boost::archive::iterators::insert_linebreaks<
            boost::archive::iterators::base64_from_binary<
                boost::archive::iterators::transform_width<
                    CharType *
                    ,6
                    ,sizeof(CharType) * 8
                >
            >
            ,72
        >
        translate_out;

    std::copy(
        translate_out(BOOST_MAKE_PFTO_WRAPPER(static_cast<CharType *>(rawdata))),
        translate_out(BOOST_MAKE_PFTO_WRAPPER(rawdata + size)),
        std::back_inserter(text_base64)
    );

    // convert from base64 to binary and compare with the original
    typedef
        boost::archive::iterators::transform_width<
            boost::archive::iterators::binary_from_base64<
                boost::archive::iterators::remove_whitespace<
                    BOOST_DEDUCED_TYPENAME text_base64_type::iterator
                >
            >,
            sizeof(CharType) * 8,
            6
        > translate_in;

    BOOST_CHECK(
        std::equal(
            rawdata,
            rawdata + size,
            translate_in(BOOST_MAKE_PFTO_WRAPPER(text_base64.begin()))
        )
    );

}

int
test_main( int /*argc*/, char* /*argv*/[] )
{
    test_base64<char>();
    #ifndef BOOST_NO_CWCHAR
    test_base64<wchar_t>();
    #endif
    return EXIT_SUCCESS;
}


/**
 * puzzle input - list of reactions
 * every reaction: quantites of input chemicals -> some quantity of output chemical
 * almost every chemical is produced by exactly one reaction, exception:
 * ORE - raw material input to the entire process and is not produced by a reaction
 * how much ORE is needed to collect before one unit of fuel is produced
 * for example : 1 A, 2 B, 3 C => 2 D...10 D is produced by consuming 5 A, 10 B and 15 C
 * */

#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <bits/stdc++.h>
#include <unordered_map>
#include <vector>
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>

#define FUEL "FUEL"
#define ORE "ORE"
#define MAX_ORE 999999999
#define GOAL 1000000000000

class Chemical {
    public:
        Chemical(std::string _name, long long int _quantity) : name(_name), quantity(_quantity) {}
        std::string name;
        long long int quantity;

        bool operator == (const Chemical &other) const {
            return (name == other.name && quantity == other.quantity);
        }

    private:
};

struct DataHasher
{
  std::size_t operator () (const std::string &key) const
  {
    std::size_t seed = 0;
    boost::hash_combine(seed, boost::hash_value(key));
    return seed;
  }
};

struct DataHasher2
{
  std::size_t operator () (const Chemical &key) const
  {
    std::size_t seed = 0;
    return seed;
  }
};

// key = inputs, value = outout
/**
 * To be able to use std::unordered_map (or one of the other unordered associative containers) with a user-defined key-type, you need to define two things:
 * A Hash function -  this must be a class that overrides operator() and calculates the hash value given an object of the key-type. One particularly
 *                    straight-forward way of doing this is to specialize the std::hash template for your key-type.
 * A comparison function for equality
*/
typedef std::unordered_map<std::string, std::vector<Chemical>, DataHasher> DataHashMap;
DataHashMap reactions;
std::map<std::string, int> reactions_name_quantity;
std::vector<Chemical> fuel_inputs;
std::vector<Chemical> final_elements;
typedef std::unordered_map<Chemical, int, DataHasher2> OccurrencesMap;
OccurrencesMap final_elements_occurrences_map;

int count_occurrence(Chemical output, Chemical input, int counter) {

    if(output.name == input.name) {
        return output.quantity;
    }

    std::vector<Chemical> inputs = reactions[output.name];

    for(int i = 0; i < inputs.size(); i++) {
        counter += count_occurrence(inputs[i], input, 0);
    }

}

bool is_final_element(Chemical element) {
    for(int i = 0; i < final_elements.size(); i++) {
        if(element.name == final_elements[i].name) {
            return true;
        }
    }
    return false;
}

bool are_final_elements(std::vector<Chemical> check_elements) {
    bool element_ok = false;
    for(int i = 0; i < check_elements.size(); i++) {
        if(!is_final_element(check_elements[i])) {
            return false;
        }
    }
    return true;
}

int index_in_vector(std::vector<Chemical> solution, Chemical element) {
    for(int i = 0; i < solution.size(); i++) {
        if(solution[i].name == element.name) {
            return i;
        }
    }
    return -1;
}

int get_reaction_quantity(std::string element_name) {
    for(std::unordered_map<std::string, std::vector<Chemical>, DataHasher>::iterator it = reactions.begin();
     it != reactions.end(); it++) {
        if(it->first == element_name) {
            return reactions_name_quantity[it->first];
        }
    }
    return 0;
}

long long int number_of_ore(long long int required_number_of_fuel) {
    long long int count = 0;

    std::vector<Chemical> solution;
    for(int i = 0; i < fuel_inputs.size(); i++) {
        Chemical tmp(fuel_inputs[i].name, fuel_inputs[i].quantity * required_number_of_fuel);
        solution.push_back(tmp);
    }

    // key : chemical name, velue: residue quantity
    std::unordered_map<std::string, int> residue_map;

    for(std::unordered_map<std::string, std::vector<Chemical>, DataHasher>::iterator it = reactions.begin();
    it != reactions.end(); it++) {
        residue_map[it -> first] = 0;
    }

    long long int index = 0;
    long long int multiplier = 0;
    long long int help = 0;
    long long int solution_index = 0;
    while(!are_final_elements(solution)) {
        Chemical element = solution[index];

        if(is_final_element(element)) {
            index++;
            continue;
        }

        solution.erase(solution.begin() + index);

        std::vector<Chemical> inputs = reactions[element.name];
        multiplier = get_reaction_quantity(element.name);

        while(residue_map[element.name] > 0) {
            element.quantity--;
            residue_map[element.name]--;
        }

        if(element.quantity <= 0) {
            continue;
        }

        help = multiplier;
        // multiplier is lower than element.quantity
        if(multiplier <= element.quantity) {
            // element.quantity is divided by multiplier
            if(element.quantity % help == 0) {
                multiplier = element.quantity / help;
            } else {
                // there is residue
                multiplier = element.quantity / help + 1;
            }
        } else {
            multiplier = 1;
        }

        residue_map[element.name] += (multiplier * help - element.quantity);

        for(int i = 0; i < inputs.size(); i++) {
            solution_index = index_in_vector(solution, inputs[i]);
            if(solution_index == -1) {
                Chemical help_chemical(inputs[i].name, inputs[i].quantity * multiplier);
                solution.push_back(help_chemical);
            } else {
                solution[solution_index].quantity += (inputs[i].quantity * multiplier);
            }
        }
    }

    for(int i = 0; i < solution.size(); i++) {
        Chemical element = solution[i];
        multiplier = get_reaction_quantity(element.name);

        if(multiplier < element.quantity) {
            help = multiplier;
            if(element.quantity % multiplier == 0) {
                multiplier = element.quantity / help;
            } else {
                multiplier = element.quantity / help + 1;
            }
        } else {
            multiplier = 1;
        }

        std::vector<Chemical> inputs = reactions[element.name];

        // inputs[0] = 0RE
        count += (inputs[0].quantity * multiplier);
    }
    return count;
}

long long int binarySearch(int l, int r)
{
    while (r >= l)
    {
        long long index = (r + l) / 2;
        long long ore_quantity = number_of_ore(index);
        if (index == r || index == l)
            break;
        if (ore_quantity >= GOAL)
            r = index;
        else
            l = index;
    }
    return l;
}

int main() {
    std::ifstream file("./day14.txt");
    std::string line;

    while(getline(file, line)) {
        bool fuel_elements = false;
        std::vector<std::string> elements;

        if (line.find(FUEL) != std::string::npos) {
            fuel_elements = true;
        }

        boost::split(elements, line, boost::is_any_of(",=>"));

        std::vector<Chemical> inputs_and_outputs;
        for(int i = 0; i < elements.size(); i++) {
            if(elements[i].empty()) {
                continue;
            }
            boost::algorithm::trim(elements[i]);

            std::string quantity_string = "";
            std::string name = "";

            for(int j = 0; j < elements[i].size(); j++) {
                std::string part = elements[i].substr(j, 1);

                if(isspace(elements[i].at(j))) {
                    continue;
                }

                if(isdigit(elements[i].at(j))) {
                    quantity_string.append(part);
                } else {
                    name.append(part);
                }
            }
            Chemical chemical(name, std::stoi(quantity_string));
            inputs_and_outputs.push_back(chemical);
        }

        std::vector<Chemical> inputs(inputs_and_outputs.begin(), inputs_and_outputs.begin() + inputs_and_outputs.size() - 1);

        Chemical output = inputs_and_outputs[inputs_and_outputs.size()-1];

        if(fuel_elements) {
            fuel_inputs = inputs;
            reactions["FUEL"] = inputs;
            reactions_name_quantity["FUEL"] = 1;
        } else {
            if(inputs.size() == 1 && inputs[0].name == ORE) {
                final_elements.push_back(output);
                reactions[output.name] = inputs;
                reactions_name_quantity[output.name] = output.quantity;
            } else {
                reactions[output.name] = inputs;
                reactions_name_quantity[output.name] = output.quantity;
            }
        }
    }

    long long int part1 = number_of_ore(1);
    long long int part2 =  binarySearch(0, MAX_ORE);

    std::cout << "part1 = " << part1 << std::endl;
    std::cout << "part2 = " << part2 << std::endl;

    return 0;
}


#include "Align4.hpp"
#include "html.hpp"
#include "orderPairs.hpp"
#include "PngImage.hpp"
#include "timestamp.hpp"
using namespace shasta;

#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/iteration_macros.hpp>

#include "algorithm.hpp"
#include "fstream.hpp"
#include <queue>
#include "tuple.hpp"

// Align two arbitrary sequences  using alignment method 4.
// If debug is true, detailed output to html is produced.
// Otherwise, html is not used.
void shasta::align4(
    const span<const CompressedMarker>& markers0,
    const span<const CompressedMarker>& markers1,
    const Align4Options& options,
    Alignment& alignment,
    AlignmentInfo& alignmentInfo,
    bool debug)
{
    switch(options.m) {
    case 1:
        align4<1>(markers0, markers1, options, alignment, alignmentInfo, debug);
        return;
    case 2:
        align4<2>(markers0, markers1, options, alignment, alignmentInfo, debug);
        return;
    case 3:
        align4<3>(markers0, markers1, options, alignment, alignmentInfo, debug);
        return;
    case 4:
        align4<4>(markers0, markers1, options, alignment, alignmentInfo, debug);
        return;
    default:
        SHASTA_ASSERT(0);
    }
}

// Version templated on m, the number of markers that define
// a "feature" used in the alignment.
template<uint64_t m> void shasta::align4(
    const span<const CompressedMarker>& markers0,
    const span<const CompressedMarker>& markers1,
    const Align4Options& options,
    Alignment& alignment,
    AlignmentInfo& alignmentInfo,
    bool debug)
{
    Align4<m> graph(markers0, markers1,
        options, alignment, alignmentInfo,
        debug);
}

template<uint64_t m> shasta::Align4<m>::Align4(
    const Sequence& sequence0,
    const Sequence& sequence1,
    const Align4Options& options,
    Alignment& alignment,
    AlignmentInfo& alignmentInfo,
    bool debug) :
    deltaX(int32_t(options.deltaX)),
    deltaY(int32_t(options.deltaY))
{
    // Check that we are in the templated version consistent with
    /// the options.
    SHASTA_ASSERT(options.m == m);

    if(debug) {
        cout << timestamp << "Computing a marker alignment of two sequences with " <<
            sequence0.size() << " and " << sequence1.size() << " markers." << endl;
    }

    // Create the FeatureMap for sequence0.
    // It is needed to create the alignment matrix.
    const uint64_t inverseLoadFactor = 2;
    FeatureMap featureMap0(inverseLoadFactor * sequence0.size());
    if(debug) {
        cout << timestamp << "Creating the feature map." << endl;
    }
    fillFeatureMap(sequence0, featureMap0);

    // Create the alignment matrix.
    if(debug) {
        cout << timestamp << "Creating the alignment matrix." << endl;
    }
    const uint32_t nx = uint32_t(sequence0.size()-(m-1));
    const uint32_t ny = uint32_t(sequence1.size()-(m-1));
    fillAlignmentMatrix(featureMap0, sequence1, nx, ny);

    // Compute reachability flags.
    if(debug) {
        cout << timestamp << "Computing reachability flags." << endl;
    }
    computeReachability();

    // Debug output including unreachable entries.
    if(debug) {
        cout << timestamp << "Writing csv output." << endl;
        writeMatrixCsv("Align4-Matrix-Initial.csv");
        cout << timestamp << "Writing png output." << endl;
        writeMatrixPng(nx, ny, "Align4-Matrix-Initial.png");
    }

    // Remove unreachable entries.
    if(debug) {
        cout << "Before removing unreachable entries, the alignment matrix has " <<
            alignmentMatrix.size() << " entries." << endl;
    }
    if(debug) {
        cout << timestamp << "Removing unreachable entries." << endl;
    }
    removeUnreachable();
    if(debug) {
        cout << "After removing unreachable entries, the alignment matrix has " <<
            alignmentMatrix.size() << " entries." << endl;
    }

    // Debug output without unreachable entries.
    if(debug) {
        cout << timestamp << "Writing csv output." << endl;
        writeMatrixCsv("Align4-Matrix.csv");
        cout << timestamp << "Writing png output." << endl;
        writeMatrixPng(nx, ny, "Align4-Matrix.png");
    }

    // Create the Boost graph, keeping only the surviving vertices.
    if(debug) {
        cout << timestamp << "Creating the graph." << endl;
    }
    createGraph();
    if(debug) {
        cout << "The graph has " << boost::num_vertices(graph) <<
            " vertices and " << boost::num_edges(graph) << " edges." << endl;
        cout << timestamp << "Finding shortest paths." << endl;
    }
    findShortestPaths(debug);
    computePathInfos();
    if(debug) {
        writePaths("Align4-Paths.csv");
        writePathInfos("Align4-PathInfos.csv");
    }

    if(debug) {
        cout << timestamp << "Done." << endl;
    }
}

template<uint64_t m> void shasta::Align4<m>::fillFeatureMap(
    const Sequence& sequence,
    FeatureMap& featureMap)
{
    SHASTA_ASSERT(featureMap.empty());

    // If the sequence is too short, it has no features.
    if(sequence.size() < m) {
        return;
    }

    // Start with the first m kmerIds.
    Feature feature;
    uint64_t nextOrdinal = 0;
    for(uint64_t i=0; i<m; i++) {
        feature[i] = sequence[nextOrdinal++].kmerId;
    }

    // Add the features.
    for(uint32_t ordinal=0; ; ordinal++) {
        featureMap.insert(make_pair(feature, ordinal));

        // Check if done.
        if(nextOrdinal == sequence.size()) {
            break;
        }

        // Shift by one.
        for(uint64_t i=1; i<m; i++) {
            feature[i-1] = feature[i];
        }
        feature.back() = sequence[nextOrdinal++].kmerId;
    }

    if(sequence.size() >= m) {
        SHASTA_ASSERT(featureMap.size() == sequence.size() + 1 - m);
    } else {
        SHASTA_ASSERT(featureMap.empty());
    }
}

template<uint64_t m> void shasta::Align4<m>::fillAlignmentMatrix(
    const FeatureMap& featureMap0,
    const Sequence& sequence1,
    int32_t nx,
    int32_t ny)
{
    // Start with the first m kmerIds.
    Feature feature;
    uint64_t nextOrdinal1 = 0;
    for(uint64_t i=0; i<m; i++) {
        feature[i] = sequence1[nextOrdinal1++].kmerId;
    }

    // Add the features.
    for(int32_t y=0; ; y++) {

        // Look up this feature in the feature map for sequence 0.
        typename FeatureMap::const_iterator begin, end;
        tie(begin, end) = featureMap0.equal_range(feature);

        // Loop over all the hits. Each hit generates an
        // entry in the alignment matrix.
        for(auto it=begin; it!=end; ++it) {
            const int32_t x = it->second;
            const int32_t X = x + y;
            const int32_t Y = y + nx - 1 - x;
            const int32_t iX = X / deltaX;
            const int32_t iY = Y / deltaY;
            AlignmentMatrixEntry alignmentMatrixEntry;
            alignmentMatrixEntry.xy = make_pair(x, y);
            alignmentMatrixEntry.XY = make_pair(X, Y);
            alignmentMatrixEntry.isNearLeft = (x < deltaX) ? 1 : 0;
            alignmentMatrixEntry.isNearTop  = (y < deltaX) ? 1 : 0;
            alignmentMatrixEntry.isNearRight = (nx-1-x < deltaX) ? 1 : 0;
            alignmentMatrixEntry.isNearBottom = (ny-1-y < deltaX) ? 1 : 0;
            alignmentMatrixEntry.isForwardReachableFromTopOrLeft = 0;
            alignmentMatrixEntry.isBackwardReachableFromBottomOrRight = 0;
            alignmentMatrix.insert(make_pair(Coordinates(iX, iY), alignmentMatrixEntry));
        }

        // Check if done.
        if(nextOrdinal1 == sequence1.size()) {
            break;
        }

        // Shift by one.
        for(uint64_t i=1; i<m; i++) {
            feature[i-1] = feature[i];
        }
        feature.back() = sequence1[nextOrdinal1++].kmerId;
    }
}

template<uint64_t m> void shasta::Align4<m>::writeMatrixCsv(
    const string& fileName)
{
    ofstream csv(fileName);
    csv << "x,y,X,Y,iX,iY,Near left,Near top,Near right,Near bottom\n";

    for(const auto& p: alignmentMatrix) {
        const Coordinates& iXY = p.first;
        const AlignmentMatrixEntry& alignmentMatrixEntry = p.second;
        csv <<
            alignmentMatrixEntry.xy.first << "," <<
            alignmentMatrixEntry.xy.second << "," <<
            alignmentMatrixEntry.XY.first << "," <<
            alignmentMatrixEntry.XY.second << "," <<
            iXY.first << "," <<
            iXY.second << "," <<
            int(alignmentMatrixEntry.isNearLeft) << "," <<
            int(alignmentMatrixEntry.isNearTop) << "," <<
            int(alignmentMatrixEntry.isNearRight) << "," <<
            int(alignmentMatrixEntry.isNearBottom) << "\n";
    }
}

template<uint64_t m> void shasta::Align4<m>::writeMatrixPng(
    uint32_t nx, uint32_t ny,
    const string& fileName)
{
    // Create the image, which gets initialized to black.
    PngImage image(nx, ny);

    // Write a grid.
    vector<int> gridSpacing;
    vector< array<int, 3> > gridRgb;
    gridSpacing.push_back(   10); gridRgb.push_back({ 15,  15,  15});  // Grey
    gridSpacing.push_back(   50); gridRgb.push_back({ 30,  30,  30});  // Grey
    gridSpacing.push_back(  100); gridRgb.push_back({ 90,  90,  90});  // Grey
    gridSpacing.push_back(  500); gridRgb.push_back({160, 160, 160});  // Grey
    gridSpacing.push_back( 1000); gridRgb.push_back({255, 255, 255});  // White
    gridSpacing.push_back( 5000); gridRgb.push_back({255, 120, 255});  // Purple
    gridSpacing.push_back(10000); gridRgb.push_back({255, 255,  60});  // Yellow
    gridSpacing.push_back(50000); gridRgb.push_back({255, 255, 120});  // Yellow
    for(size_t i=0; i<gridSpacing.size(); i++) {
        const int spacing = gridSpacing[i];

        const array<int, 3>& rgb = gridRgb[i];
        for(uint32_t x=0; x<nx; x+=spacing) {
            for(uint32_t y=0; y<ny; y++) {
                image.setPixel(x, y, rgb[0], rgb[1], rgb[2]);
            }
        }
        for(uint32_t y=0; y<ny; y+=spacing) {
            for(uint32_t x=0; x<nx; x++) {
                image.setPixel(x, y, rgb[0], rgb[1], rgb[2]);
            }
        }
    }

    // Write the alignment matrix.
    for(const auto& p: alignmentMatrix) {
        const AlignmentMatrixEntry& entry = p.second;
        const Coordinates& coordinates = entry.xy;
        if(entry.isForwardReachableFromTopOrLeft and entry.isBackwardReachableFromBottomOrRight) {
            image.setPixel(coordinates.first, coordinates.second, 0, 255, 0);
        } else {
            image.setPixel(coordinates.first, coordinates.second, 255, 0, 0);
        }
    }

    // Write it out.
    image.write(fileName);
}

template<uint64_t m> void shasta::Align4<m>::clearDiscoveredFlags()
{
    for(auto& p: alignmentMatrix) {
        p.second.wasDiscovered = 0;
    }
}

// Compute the reachability flags in the alignment matrix
template<uint64_t m> void shasta::Align4<m>::computeReachability()
{
    using iterator = typename AlignmentMatrix::iterator;
    std::queue<iterator> q;
    vector<iterator> neighbors;

    // Do a forward BFS starting from all matrix entries near the
    // top or left of the alignment matrix.
    clearDiscoveredFlags();
    for(iterator it=alignmentMatrix.begin(); it!=alignmentMatrix.end(); ++it) {
        AlignmentMatrixEntry& entry = it->second;
        if(entry.isNearTop or entry.isNearLeft) {
            q.push(it);
            entry.wasDiscovered = 1;
            entry.isForwardReachableFromTopOrLeft = 1;
        }
    }
    while(not q.empty()) {
        auto it0 = q.front();
        q.pop();
        findAndFlagUndiscoveredChildren(it0, neighbors);
        for(const auto& it1: neighbors) {
            q.push(it1);
            AlignmentMatrixEntry& entry1 = it1->second;
            entry1.isForwardReachableFromTopOrLeft = 1;
        }
    }

    // Do a backward BFS starting from all matrix entries near the
    // top or left of the alignment matrix.
    SHASTA_ASSERT(q.empty());
    clearDiscoveredFlags();
    for(iterator it=alignmentMatrix.begin(); it!=alignmentMatrix.end(); ++it) {
        AlignmentMatrixEntry& entry = it->second;
        if(entry.isNearBottom or entry.isNearRight) {
            q.push(it);
            entry.wasDiscovered = 1;
            entry.isBackwardReachableFromBottomOrRight = 1;
        }
    }
    while(not q.empty()) {
        auto it0 = q.front();
        q.pop();
        findAndFlagUndiscoveredParents(it0, neighbors);
        for(const auto& it1: neighbors) {
            q.push(it1);
            AlignmentMatrixEntry& entry1 = it1->second;
            entry1.isBackwardReachableFromBottomOrRight = 1;
        }
    }

}

// Remove alignment matrix entries that are not reachable in both directions.
template<uint64_t m> void shasta::Align4<m>:: removeUnreachable()
{
    // The loop must be done with care to avoid using invalid iterators
    // pointing to removed elements.
    for(auto it=alignmentMatrix.begin(); it!=alignmentMatrix.end();) {

        const AlignmentMatrixEntry& entry = it->second;
        if(not(entry.isForwardReachableFromTopOrLeft and entry.isBackwardReachableFromBottomOrRight)) {
            auto jt = it;
            ++it;
            alignmentMatrix.erase(jt);
        } else {
            ++it;
        }
    }
}

template<uint64_t m> void shasta::Align4<m>::findAndFlagUndiscoveredNeighbors(
    typename AlignmentMatrix::iterator it0,
    vector<typename AlignmentMatrix::iterator>& neighbors)
{
    using iterator = typename AlignmentMatrix::iterator;
    neighbors.clear();

    const AlignmentMatrixEntry& entry0 = it0->second;
    const Coordinates& iXY0 = it0->first;
    const int32_t iX0 = iXY0.first;
    const int32_t iY0 = iXY0.second;

    // Loop over the 9 cells centered here.
    for(int32_t diX=-1; diX<=1; diX++) {
        const int iX1 = iX0 + diX;
        for(int32_t diY=-1; diY<=1; diY++) {
            const int iY1 = iY0 + diY;

            // Loop over all alignment matrix entries in this cell.
            iterator begin, end;
            tie(begin, end) = alignmentMatrix.equal_range(Coordinates(iX1, iY1));
            for(iterator it1=begin; it1!=end; ++it1) {
                if(it1 == it0) {
                    continue;
                }
                AlignmentMatrixEntry& entry1 = it1->second;
                if(entry1.wasDiscovered) {
                    continue;
                }
                if(not entry0.isNeighbor(entry1, deltaX, deltaY)) {
                    continue;
                }
                entry1.wasDiscovered = true;
                neighbors.push_back(it1);
            }
        }
    }
}

template<uint64_t m> void shasta::Align4<m>::findChildren(
    typename AlignmentMatrix::iterator it0,
    vector<typename AlignmentMatrix::iterator>& neighbors)
{
    using iterator = typename AlignmentMatrix::iterator;
    neighbors.clear();

    const AlignmentMatrixEntry& entry0 = it0->second;
    const Coordinates& iXY0 = it0->first;
    const int32_t iX0 = iXY0.first;
    const int32_t iY0 = iXY0.second;

    // Loop over the 6 cells that could contain children.
    for(int32_t diX=0; diX<=1; diX++) {
        const int iX1 = iX0 + diX;
        for(int32_t diY=-1; diY<=1; diY++) {
            const int iY1 = iY0 + diY;

            // Loop over all alignment matrix entries in this cell.
            iterator begin, end;
            tie(begin, end) = alignmentMatrix.equal_range(Coordinates(iX1, iY1));
            for(iterator it1=begin; it1!=end; ++it1) {
                if(it1 == it0) {
                    continue;
                }
                AlignmentMatrixEntry& entry1 = it1->second;
                if(not entry1.isChild(entry0, deltaX, deltaY)) {
                    continue;
                }
                neighbors.push_back(it1);
            }
        }
    }
}

template<uint64_t m> void shasta::Align4<m>::findAndFlagUndiscoveredChildren(
    typename AlignmentMatrix::iterator it0,
    vector<typename AlignmentMatrix::iterator>& neighbors)
{
    using iterator = typename AlignmentMatrix::iterator;
    neighbors.clear();

    const AlignmentMatrixEntry& entry0 = it0->second;
    const Coordinates& iXY0 = it0->first;
    const int32_t iX0 = iXY0.first;
    const int32_t iY0 = iXY0.second;

    // Loop over the 6 cells that could contain children.
    for(int32_t diX=0; diX<=1; diX++) {
        const int iX1 = iX0 + diX;
        for(int32_t diY=-1; diY<=1; diY++) {
            const int iY1 = iY0 + diY;

            // Loop over all alignment matrix entries in this cell.
            iterator begin, end;
            tie(begin, end) = alignmentMatrix.equal_range(Coordinates(iX1, iY1));
            for(iterator it1=begin; it1!=end; ++it1) {
                if(it1 == it0) {
                    continue;
                }
                AlignmentMatrixEntry& entry1 = it1->second;
                if(entry1.wasDiscovered) {
                    continue;
                }
                if(not entry1.isChild(entry0, deltaX, deltaY)) {
                    continue;
                }
                entry1.wasDiscovered = true;
                neighbors.push_back(it1);
            }
        }
    }
}

template<uint64_t m> void shasta::Align4<m>::findAndFlagUndiscoveredParents(
    typename AlignmentMatrix::iterator it0,
    vector<typename AlignmentMatrix::iterator>& neighbors)
{
    using iterator = typename AlignmentMatrix::iterator;
    neighbors.clear();

    const AlignmentMatrixEntry& entry0 = it0->second;
    const Coordinates& iXY0 = it0->first;
    const int32_t iX0 = iXY0.first;
    const int32_t iY0 = iXY0.second;

    // Loop over the 6 cells that could contain parents.
    for(int32_t diX=-1; diX<=0; diX++) {
        const int iX1 = iX0 + diX;
        for(int32_t diY=-1; diY<=1; diY++) {
            const int iY1 = iY0 + diY;

            // Loop over all alignment matrix entries in this cell.
            iterator begin, end;
            tie(begin, end) = alignmentMatrix.equal_range(Coordinates(iX1, iY1));
            for(iterator it1=begin; it1!=end; ++it1) {
                if(it1 == it0) {
                    continue;
                }
                AlignmentMatrixEntry& entry1 = it1->second;
                if(entry1.wasDiscovered) {
                    continue;
                }
                if(not entry1.isParent(entry0, deltaX, deltaY)) {
                    continue;
                }
                entry1.wasDiscovered = true;
                neighbors.push_back(it1);
            }
        }
    }
}

template<uint64_t m> bool shasta::Align4<m>::AlignmentMatrixEntry::isNeighbor(
    const AlignmentMatrixEntry& that,
    int32_t deltaX,
    int32_t deltaY) const
{

    if(isChild(that, deltaX, deltaY)) {
        return true;
    }
    if(that.isChild(*this, deltaX, deltaY)) {
        return true;
    }
    return false;
}

// Return true if (*this) is a child of that.
template<uint64_t m> bool shasta::Align4<m>::AlignmentMatrixEntry::isChild(
    const AlignmentMatrixEntry& that,
    int32_t deltaX,
    int32_t deltaY) const
{
    SHASTA_ASSERT(xy != that.xy);

    // x and y must be strictly increasing.
    if(xy.first <= that.xy.first) {
        return false;
    }
    if(xy.second <= that.xy.second) {
        return false;
    }

    const Coordinates& thisXY = XY;
    const Coordinates& thatXY = that.XY;

    const int32_t thisX = thisXY.first;
    const int32_t thatX = thatXY.first;
    if(thisX <= thatX) {
        return false;
    }
    if(thisX >= thatX + deltaX) {
        return false;
    }

    const int32_t thisY = thisXY.second;
    const int32_t thatY = thatXY.second;
    if(thisY <= thatY - deltaY) {
        return false;
    }
    if(thisY >= thatY + deltaY) {
        return false;
    }

    return true;
}

// Return true if (*this) is a parent of that.
template<uint64_t m> bool shasta::Align4<m>::AlignmentMatrixEntry::isParent(
    const AlignmentMatrixEntry& that,
    int32_t deltaX,
    int32_t deltaY) const
{
    return that.isChild(*this, deltaX, deltaY);
}

// After unreachable vertices are removed from the alignment matrix,
// we create the equivalent Boost graph.
// Each vertex contains an AlignmentMatrixEntry.
template<uint64_t m> void shasta::Align4<m>::createGraph()
{
    // Create the vertices.
    graph.m_vertices.reserve(alignmentMatrix.size());
    for(auto it=alignmentMatrix.begin(); it!=alignmentMatrix.end(); ++it) {
        AlignmentMatrixEntry& entry = it->second;
        entry.v = boost::add_vertex(it, graph);
    }

    // Create the edges.
    using iterator = typename AlignmentMatrix::iterator;
    vector<iterator> children;
    BGL_FORALL_VERTICES_T(v0, graph, Graph) {
        const iterator it0 = graph[v0];
        const AlignmentMatrixEntry& entry0 = it0->second;
        SHASTA_ASSERT(entry0.hasValidVertex());
        SHASTA_ASSERT(entry0.v == v0);
        findChildren(it0, children);

        for(const iterator it1: children) {
            const AlignmentMatrixEntry& entry1 = it1->second;
            SHASTA_ASSERT(entry1.hasValidVertex());
            const vertex_descriptor v1 = entry1.v;

            const int32_t dx = entry1.xy.first - entry0.xy.first;
            SHASTA_ASSERT(dx > 0);
            const int32_t dy = entry1.xy.second - entry0.xy.second;
            SHASTA_ASSERT(dy > 0);
            const int32_t weight = dx + dy - 2;
            SHASTA_ASSERT(weight >= 0);

            edge_descriptor e;
            tie(e, ignore) = boost::add_edge(v0, v1, uint64_t(weight), graph);
            SHASTA_ASSERT(boost::get(boost::edge_weight, graph)[e] == uint64_t(weight));

        }
    }
}

template<uint64_t m> void shasta::Align4<m>::findShortestPaths(bool debug)
{
    using boost::make_iterator_property_map;
    using boost::get;
    using boost::vertex_index;
    using boost::dijkstra_shortest_paths_no_color_map;
    using boost::predecessor_map;

    // Find all possible begin/end points.
    vector<vertex_descriptor> beginPoints;
    vector<vertex_descriptor> endPoints;
    BGL_FORALL_VERTICES_T(v, graph, Graph) {
        if(boost::in_degree(v, graph) == 0) {
            beginPoints.push_back(v);
        }
        if(boost::out_degree(v, graph) == 0) {
            endPoints.push_back(v);
        }
    }

    vector<vertex_descriptor> predecessor(num_vertices(graph));
    for(const vertex_descriptor beginPoint: beginPoints) {
        auto predecessorMap = make_iterator_property_map(predecessor.begin(), get(vertex_index, graph));
        dijkstra_shortest_paths_no_color_map(graph, beginPoint, predecessor_map(predecessorMap));

        for(const vertex_descriptor endPoint: endPoints) {
            if(predecessorMap[endPoint] != endPoint) {

                // Walk back the predecessor map to create the path.
                paths.resize(paths.size() + 1);
                Path& path = paths.back();
                vertex_descriptor v = endPoint;
                while(true) {
                    path.push_back(v);
                    const vertex_descriptor p = predecessorMap[v];
                    if(p == v) {
                        break;
                    }
                    v = p;
                }
                std::reverse(path.begin(), path.end());
            }
        }
    }

}

template<uint64_t m> void shasta::Align4<m>::writePaths(const string& fileName) const
{
    ofstream csv(fileName);
    csv << "PathId,Index,x,y,X,Y\n";
    for(uint64_t pathId=0; pathId<paths.size(); pathId++) {
        const Path& path = paths[pathId];
        for(uint64_t i=0; i<path.size(); i++) {
            const vertex_descriptor v = path[i];
            const AlignmentMatrixEntry& entry = graph[v]->second;

            csv << pathId << ",";
            csv << i << ",";
            csv << entry.xy.first << ",";
            csv << entry.xy.second << ",";
            csv << entry.XY.first << ",";
            csv << entry.XY.second << "\n";
        }
    }
}

template<uint64_t m> void shasta::Align4<m>::computePathInfo(
    const Path& path,
    PathInfo& pathInfo)
{
    pathInfo.featureCount = path.size();
    const uint32_t firstX = graph[path.front()]->second.XY.first;
    const uint32_t lastX = graph[path.back()]->second.XY.first;
    pathInfo.alignedFraction = double(path.size()-1) / (0.5 * double(lastX - firstX));

    pathInfo.dX = 0.;
    pathInfo.dY = 0.;
    for(uint64_t i=1; i<path.size(); i++) {
        const AlignmentMatrixEntry& entry = graph[path[i]]->second;
        const AlignmentMatrixEntry& previousEntry = graph[path[i-1]]->second;
        const double dX = double(entry.XY.first - previousEntry.XY.first);
        const double dY = double(entry.XY.second - previousEntry.XY.second);
        pathInfo.dX += dX * dX;
        pathInfo.dY += dY * dY;
    }
    pathInfo.dX = sqrt(pathInfo.dX / double(path.size()-1));
    pathInfo.dY = sqrt(pathInfo.dY / double(path.size()-1));

    pathInfo.minY = std::numeric_limits<int32_t>::max();
    pathInfo.maxY = std::numeric_limits<int32_t>::min();
    for(const vertex_descriptor v: path) {
        const AlignmentMatrixEntry& entry = graph[v]->second;
        const int32_t Y = entry.XY.second;
        pathInfo.minY = min(pathInfo.minY, Y);
        pathInfo.maxY = max(pathInfo.maxY, Y);
    }
}

template<uint64_t m> void shasta::Align4<m>::computePathInfos()
{
    pathInfos.resize(paths.size());
    for(uint64_t i=0; i<paths.size(); i++) {
        computePathInfo(paths[i], pathInfos[i]);
    }
}

template<uint64_t m> void shasta::Align4<m>::writePathInfos(
    const string& fileName)
{
    ofstream csv(fileName);
    csv << "PathId,xFirst,yFirst,xLast,yLast,FeatureCount,AlignedFraction,dX,dY,minY,maxY,Bandwidth\n";
    for(uint64_t i=0; i<paths.size(); i++) {
        const Path& path = paths[i];
        const PathInfo& pathInfo = pathInfos[i];
        const AlignmentMatrixEntry& firstEntry = graph[path.front()]->second;
        const AlignmentMatrixEntry& lastEntry = graph[path.back()]->second;

        csv << i << ",";
        csv << firstEntry.xy.first << ",";
        csv << firstEntry.xy.second << ",";
        csv << lastEntry.xy.first << ",";
        csv << lastEntry.xy.second << ",";
        csv << pathInfo.featureCount << ",";
        csv << pathInfo.alignedFraction << ",";
        csv << pathInfo.dX << ",";
        csv << pathInfo.dY << ",";
        csv << pathInfo.minY << ",";
        csv << pathInfo.maxY << ",";
        csv << pathInfo.bandWidth() << "\n";
    }
}


// (-*- C++ -*- header)

/*
 *
 * Copyright (c) Kresimir Fresl 2003
 * Copyright (c) Georg Baum 2004
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_SUPERLU_DECL_H
#define BOOST_NUMERIC_BINDINGS_SUPERLU_DECL_H

#include <boost/numeric/bindings/superlu/superlu_inc.hpp>

extern "C" {

  // float versions (from superlu/ssp_defs.h)

  void sgssv (superlu_options_t *, SuperMatrix* A, int* perm_c, int* perm_r,
              SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t *, int* info);

  void sCreate_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               float* nzval, int* rowind, int* colptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void sCreate_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               float* nzval, int* colind, int* rowptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void sCreate_Dense_Matrix (SuperMatrix* A, int m, int n,
                             float* vals, int ld_vals,
                             Stype_t stype, Dtype_t dtype, Mtype_t mtype);

  void sgstrf (superlu_options_t*, SuperMatrix*, float, int, int, int*, void*, int, int*, int*,
               SuperMatrix*, SuperMatrix*, SuperLUStat_t*, int*);
  void sgstrs (trans_t, SuperMatrix*, SuperMatrix*, int*, int*,
               SuperMatrix*, SuperLUStat_t*, int*);

  void sPrint_CompCol_Matrix (char* what, SuperMatrix* A);
  void sPrint_CompRow_Matrix (char* what, SuperMatrix* A);
  void sPrint_SuperNode_Matrix (char* what, SuperMatrix* A);
  void sPrint_Dense_Matrix (char* what, SuperMatrix* A);

  // double versions (from superlu/dsp_defs.h)

  void dgssv (superlu_options_t *, SuperMatrix* A, int* perm_c, int* perm_r,
              SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t *, int* info);

  void dCreate_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               double* nzval, int* rowind, int* colptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void dCreate_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               double* nzval, int* colind, int* rowptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void dCreate_Dense_Matrix (SuperMatrix* A, int m, int n,
                             double* vals, int ld_vals,
                             Stype_t stype, Dtype_t dtype, Mtype_t mtype);

  void dgstrf (superlu_options_t*, SuperMatrix*, double, int, int, int*, void*, int, int*, int*,
               SuperMatrix*, SuperMatrix*, SuperLUStat_t*, int*);
  void dgstrs (trans_t, SuperMatrix*, SuperMatrix*, int*, int*,
               SuperMatrix*, SuperLUStat_t*, int*);

  void dPrint_CompCol_Matrix (char* what, SuperMatrix* A);
  void dPrint_CompRow_Matrix (char* what, SuperMatrix* A);
  void dPrint_SuperNode_Matrix (char* what, SuperMatrix* A);
  void dPrint_Dense_Matrix (char* what, SuperMatrix* A);

  // complex float versions (from superlu/csp_defs.h)

  typedef struct { float r, i; } flt_complex;

  void cgssv (superlu_options_t *, SuperMatrix* A, int* perm_c, int* perm_r,
              SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t *, int* info);

  void cCreate_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               flt_complex* nzval, int* rowind, int* colptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void cCreate_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               flt_complex* nzval, int* colind, int* rowptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void cCreate_Dense_Matrix (SuperMatrix* A, int m, int n,
                             flt_complex* vals, int ld_vals,
                             Stype_t stype, Dtype_t dtype, Mtype_t mtype);

  void cgstrf (superlu_options_t*, SuperMatrix*, float, int, int, int*, void*, int, int*, int*,
               SuperMatrix*, SuperMatrix*, SuperLUStat_t*, int*);
  void cgstrs (trans_t, SuperMatrix*, SuperMatrix*, int*, int*,
               SuperMatrix*, SuperLUStat_t*, int *);

  void cPrint_CompCol_Matrix (char* what, SuperMatrix* A);
  void cPrint_CompRow_Matrix (char* what, SuperMatrix* A);
  void cPrint_SuperNode_Matrix (char* what, SuperMatrix* A);
  void cPrint_Dense_Matrix (char* what, SuperMatrix* A);

  // complex float versions (from superlu/zsp_defs.h)

  typedef struct { double r, i; } dbl_complex;

  void zgssv (superlu_options_t *, SuperMatrix* A, int* perm_c, int* perm_r,
              SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t *, int* info);

  void zCreate_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               dbl_complex* nzval, int* rowind, int* colptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void zCreate_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                               dbl_complex* nzval, int* colind, int* rowptr,
                               Stype_t stype, Dtype_t dtype, Mtype_t mtype);
  void zCreate_Dense_Matrix (SuperMatrix* A, int m, int n,
                             dbl_complex* vals, int ld_vals,
                             Stype_t stype, Dtype_t dtype, Mtype_t mtype);

  void zgstrf (superlu_options_t*, SuperMatrix*, double, int, int, int*, void*, int, int*, int*,
               SuperMatrix*, SuperMatrix*, SuperLUStat_t*, int*);
  void zgstrs (trans_t, SuperMatrix*, SuperMatrix*, int*, int*,
               SuperMatrix*, SuperLUStat_t*, int*);

  void zPrint_CompCol_Matrix (char* what, SuperMatrix* A);
  void zPrint_CompRow_Matrix (char* what, SuperMatrix* A);
  void zPrint_SuperNode_Matrix (char* what, SuperMatrix* A);
  void zPrint_Dense_Matrix (char* what, SuperMatrix* A);

  // all types

  void get_perm_c (int permc_spec, SuperMatrix* A, int* perm_c);

  void Destroy_SuperMatrix_Store (SuperMatrix* A);
  void Destroy_CompCol_Matrix (SuperMatrix* A);
  void Destroy_CompRow_Matrix (SuperMatrix* A);
  void Destroy_SuperNode_Matrix (SuperMatrix* A);
  void Destroy_CompCol_Permuted (SuperMatrix* A);
  void Destroy_Dense_Matrix (SuperMatrix* A);

}

#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkDiffusionFunctionCollection.h"
#include "mitkNumericTypes.h"

#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/version.hpp>
#include <itkPointShell.h>
#include "itkVectorContainer.h"
#include "vnl/vnl_vector.h"
#include <vtkBox.h>
#include <vtkMath.h>
#include <itksys/SystemTools.hxx>
#include <boost/algorithm/string.hpp>

// Intersect a finite line (with end points p0 and p1) with all of the
// cells of a vtkImageData
std::vector< std::pair< itk::Index<3>, double > > mitk::imv::IntersectImage(const itk::Vector<double,3>& spacing, itk::Index<3>& si, itk::Index<3>& ei, itk::ContinuousIndex<float, 3>& sf, itk::ContinuousIndex<float, 3>& ef)
{
  std::vector< std::pair< itk::Index<3>, double > > out;
  if (si == ei)
  {
    double d[3];
    for (int i=0; i<3; ++i)
      d[i] = (sf[i]-ef[i])*spacing[i];
    double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );
    out.push_back(  std::pair< itk::Index<3>, double >(si, len) );
    return out;
  }

  double bounds[6];

  double entrancePoint[3];
  double exitPoint[3];

  double startPoint[3];
  double endPoint[3];

  double t0, t1;
  for (int i=0; i<3; ++i)
  {
    startPoint[i] = sf[i];
    endPoint[i] = ef[i];

    if (si[i]>ei[i])
    {
      int t = si[i];
      si[i] = ei[i];
      ei[i] = t;
    }
  }

  for (int x = si[0]; x<=ei[0]; ++x)
    for (int y = si[1]; y<=ei[1]; ++y)
      for (int z = si[2]; z<=ei[2]; ++z)
      {
        bounds[0] = (double)x - 0.5;
        bounds[1] = (double)x + 0.5;
        bounds[2] = (double)y - 0.5;
        bounds[3] = (double)y + 0.5;
        bounds[4] = (double)z - 0.5;
        bounds[5] = (double)z + 0.5;

        int entryPlane;
        int exitPlane;
        int hit = vtkBox::IntersectWithLine(bounds,
                                            startPoint,
                                            endPoint,
                                            t0,
                                            t1,
                                            entrancePoint,
                                            exitPoint,
                                            entryPlane,
                                            exitPlane);
        if (hit)
        {
          if (entryPlane>=0 && exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i] - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (entryPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (ef[i] - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i]-sf[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
        }
      }
  return out;
}

//------------------------- SH-function ------------------------------------

double mitk::sh::factorial(int number) {
  if(number <= 1) return 1;
  double result = 1.0;
  for(int i=1; i<=number; i++)
    result *= i;
  return result;
}

void mitk::sh::Cart2Sph(double x, double y, double z, double *spherical)
{
  double phi, th, rad;
  rad = sqrt(x*x+y*y+z*z);
  if( rad < mitk::eps )
  {
    th = itk::Math::pi/2;
    phi = itk::Math::pi/2;
  }
  else
  {
    th = acos(z/rad);
    phi = atan2(y, x);
  }
  spherical[0] = phi;
  spherical[1] = th;
  spherical[2] = rad;
}

vnl_vector_fixed<double, 3> mitk::sh::Sph2Cart(const double& theta, const double& phi, const double& rad)
{
  vnl_vector_fixed<double, 3> dir;
  dir[0] = rad * sin(theta) * cos(phi);
  dir[1] = rad * sin(theta) * sin(phi);
  dir[2] = rad * cos(theta);
  return dir;
}

double mitk::sh::legendre0(int l)
{
  if( l%2 != 0 )
  {
    return 0;
  }
  else
  {
    double prod1 = 1.0;
    for(int i=1;i<l;i+=2) prod1 *= i;
    double prod2 = 1.0;
    for(int i=2;i<=l;i+=2) prod2 *= i;
    return pow(-1.0,l/2.0)*(prod1/prod2);
  }
}

double mitk::sh::Yj(int m, int l, float theta, float phi, bool mrtrix)
{
  if (!mrtrix)
  {
    if (m<0)
      return sqrt(2.0)*::boost::math::spherical_harmonic_r(l, -m, theta, phi);
    else if (m==0)
      return ::boost::math::spherical_harmonic_r(l, m, theta, phi);
    else
      return pow(-1.0,m)*sqrt(2.0)*::boost::math::spherical_harmonic_i(l, m, theta, phi);
  }
  else
  {
    double plm = ::boost::math::legendre_p<float>(l,abs(m),-cos(theta));
    double mag = sqrt((double)(2*l+1)/(4.0*itk::Math::pi)*::boost::math::factorial<float>(l-abs(m))/::boost::math::factorial<float>(l+abs(m)))*plm;
    if (m>0)
      return mag*cos(m*phi);
    else if (m==0)
      return mag;
    else
      return mag*sin(-m*phi);
  }

  return 0;
}

vnl_matrix<float> mitk::sh::CalcShBasisForDirections(int sh_order, vnl_matrix<double> U, bool mrtrix)
{
  vnl_matrix<float> sh_basis  = vnl_matrix<float>(U.cols(), (sh_order*sh_order + sh_order + 2)/2 + sh_order );
  for(unsigned int i=0; i<U.cols(); i++)
  {
    double x = U(0,i);
    double y = U(1,i);
    double z = U(2,i);
    double spherical[3];
    mitk::sh::Cart2Sph(x,y,z,spherical);
    U(0,i) = spherical[0];
    U(1,i) = spherical[1];
    U(2,i) = spherical[2];
  }

  for(unsigned int i=0; i<U.cols(); i++)
  {
    for(int k=0; k<=sh_order; k+=2)
    {
      for(int m=-k; m<=k; m++)
      {
        int j = (k*k + k + 2)/2 + m - 1;
        double phi = U(0,i);
        double th = U(1,i);
        sh_basis(i,j) = mitk::sh::Yj(m,k,th,phi, mrtrix);
      }
    }
  }

  return sh_basis;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const double theta, const double phi, const bool mrtrix)
{
  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      int j = (k*k + k + 2)/2 + m - 1;
      val += coefficients[j] * mitk::sh::Yj(m, k, theta, phi, mrtrix);
    }
  }

  return val;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const vnl_vector_fixed<double, 3> &dir, const bool mrtrix)
{
  double spherical[3];
  mitk::sh::Cart2Sph(dir[0], dir[1], dir[2], spherical);

  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      int j = (k*k + k + 2)/2 + m - 1;
      val += coefficients[j] * mitk::sh::Yj(m, k, spherical[1], spherical[0], mrtrix);
    }
  }

  return val;
}

//------------------------- gradients-function ------------------------------------

mitk::gradients::GradientDirectionContainerType::Pointer mitk::gradients::ReadBvalsBvecs(std::string bvals_file, std::string bvecs_file, double& reference_bval)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();

  std::vector<float> bvec_entries;
  if (!itksys::SystemTools::FileExists(bvecs_file))
    mitkThrow() << "bvecs file not existing: " << bvecs_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvecs_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try
            {
              bvec_entries.push_back(boost::lexical_cast<float>(token));
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvecs file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvecs file could not be opened: " << bvals_file;
    }
  }

  reference_bval = -1;
  std::vector<float> bval_entries;
  if (!itksys::SystemTools::FileExists(bvals_file))
    mitkThrow() << "bvals file not existing: " << bvals_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvals_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try {
              bval_entries.push_back(boost::lexical_cast<float>(token));
              if (bval_entries.back()>reference_bval)
                reference_bval = bval_entries.back();
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvals file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvals file could not be opened: " << bvals_file;
    }
  }

  for(unsigned int i=0; i<bval_entries.size(); i++)
  {
    double b_val = bval_entries.at(i);

    mitk::gradients::GradientDirectionType vec;
    vec[0] = bvec_entries.at(i);
    vec[1] = bvec_entries.at(i+bval_entries.size());
    vec[2] = bvec_entries.at(i+2*bval_entries.size());

    // Adjust the vector length to encode gradient strength
    double factor = b_val/reference_bval;
    if(vec.magnitude() > 0)
    {
      vec.normalize();
      vec[0] = sqrt(factor)*vec[0];
      vec[1] = sqrt(factor)*vec[1];
      vec[2] = sqrt(factor)*vec[2];
    }

    directioncontainer->InsertElement(i,vec);
  }

  return directioncontainer;
}

void mitk::gradients::WriteBvalsBvecs(std::string bvals_file, std::string bvecs_file, GradientDirectionContainerType::Pointer gradients, double reference_bval)
{
  std::ofstream myfile;
  myfile.open (bvals_file.c_str());
  for(unsigned int i=0; i<gradients->Size(); i++)
  {
    double twonorm = gradients->ElementAt(i).two_norm();
    myfile << std::round(reference_bval*twonorm*twonorm) << " ";
  }
  myfile.close();

  std::ofstream myfile2;
  myfile2.open (bvecs_file.c_str());
  for(int j=0; j<3; j++)
  {
    for(unsigned int i=0; i<gradients->Size(); i++)
    {
      GradientDirectionType direction = gradients->ElementAt(i);
      direction.normalize();
      myfile2 << direction.get(j) << " ";
    }
    myfile2 << std::endl;
  }
}

std::vector<unsigned int> mitk::gradients::GetAllUniqueDirections(const BValueMap & refBValueMap, GradientDirectionContainerType *refGradientsContainer )
{

  IndiciesVector directioncontainer;
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      auto containerIt = directioncontainer.begin();
      bool directionExist = false;
      while(containerIt != directioncontainer.end())
      {
        if (fabs(dot_product(refGradientsContainer->ElementAt(*containerIt), refGradientsContainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        directioncontainer.push_back(wntIndex);
      }
    }
  }

  return directioncontainer;
}

bool mitk::gradients::CheckForDifferingShellDirections(const BValueMap & refBValueMap, GradientDirectionContainerType::ConstPointer refGradientsContainer)
{
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    auto mapIterator_2 = refBValueMap.begin();
    if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
      mapIterator_2++; //skip bzero Values

    for( ; mapIterator_2 != refBValueMap.end(); mapIterator_2++){

      if(mapIterator_2 == mapIterator) continue;

      IndiciesVector currentShell = mapIterator->second;
      IndiciesVector testShell = mapIterator_2->second;
      for (unsigned int i = 0; i< currentShell.size(); i++)
        if (fabs(dot_product(refGradientsContainer->ElementAt(currentShell[i]), refGradientsContainer->ElementAt(testShell[i])))  <= 0.9998) { return true; }

    }
  }
  return false;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalFromCartesian(const IndiciesVector & refShell, const GradientDirectionContainerType * refGradientsContainer)
{

  vnl_matrix<double> Q(3, refShell.size());
  Q.fill(0.0);

  for(unsigned int i = 0; i < refShell.size(); i++)
  {
    GradientDirectionType dir = refGradientsContainer->ElementAt(refShell[i]);
    double x = dir.normalize().get(0);
    double y = dir.normalize().get(1);
    double z = dir.normalize().get(2);
    double cart[3];
    mitk::sh::Cart2Sph(x,y,z,cart);
    Q(0,i) = cart[0];
    Q(1,i) = cart[1];
    Q(2,i) = cart[2];
  }
  return Q;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalHarmonicsBasis(const vnl_matrix<double> & QBallReference, const unsigned int & LOrder)
{
  vnl_matrix<double> SHBasisOutput(QBallReference.cols(), (LOrder+1)*(LOrder+2)*0.5);
  SHBasisOutput.fill(0.0);
  for(int i=0; i< (int)SHBasisOutput.rows(); i++)
    for(int k = 0; k <= (int)LOrder; k += 2)
      for(int m =- k; m <= k; m++)
      {
        int j = ( k * k + k + 2 ) / 2.0 + m - 1;
        double phi = QBallReference(0,i);
        double th = QBallReference(1,i);
        double val = mitk::sh::Yj(m,k,th,phi);
        SHBasisOutput(i,j) = val;
      }
  return SHBasisOutput;
}

mitk::gradients::GradientDirectionContainerType::Pointer mitk::gradients::CreateNormalizedUniqueGradientDirectionContainer(const mitk::gradients::BValueMap & bValueMap,
                                                                                const GradientDirectionContainerType *origninalGradentcontainer)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();
  auto mapIterator = bValueMap.begin();

  if(bValueMap.find(0) != bValueMap.end() && bValueMap.size() > 1){
    mapIterator++; //skip bzero Values
    vnl_vector_fixed<double, 3> vec;
    vec.fill(0.0);
    directioncontainer->push_back(vec);
  }

  for( ; mapIterator != bValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      mitk::gradients::GradientDirectionContainerType::Iterator containerIt = directioncontainer->Begin();
      bool directionExist = false;
      while(containerIt != directioncontainer->End())
      {
        if (fabs(dot_product(containerIt.Value(), origninalGradentcontainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        GradientDirectionType dir(origninalGradentcontainer->ElementAt(wntIndex));
        directioncontainer->push_back(dir.normalize());
      }
    }
  }

  return directioncontainer;
}


#ifndef YASMIC_COMPRESSED_ROW_MATRIX_GRAPH
#define YASMIC_COMPRESSED_ROW_MATRIX_GRAPH

#if _MSC_VER >= 1400
    // disable the warning for deprecated c++ commands
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400

#include <boost/iterator.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/pending/integer_range.hpp>
#include <boost/graph/detail/adj_list_edge_iterator.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map.hpp>

#include <yasmic/compressed_row_matrix.hpp>
#include <yasmic/tuple_utility.hpp>

namespace boost
{
	struct yasmic_compressed_row_matrix_traversal_category :
		public vertex_list_graph_tag,
		public adjacency_graph_tag,
		public incidence_graph_tag,
		public edge_list_graph_tag { };

	// forward declarations (prototypes) for the detailed implementation

	namespace impl
	{
		//template <class EdgeList> struct val_out_edge_ret;
		template <class RowIter, class ColIter, class ValIter> class crm_graph_edge_iter;
		template <class RowIter, class ColIter, class ValIter> struct crm_graph_edge;

		template <class RowIter, class ColIter, class ValIter>
		struct crm_graph
		{
			typedef yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> type;
			typedef yasmic::smatrix_traits<type> traits;
		};

		/*template <class RowIter, class ColIter, class ValIter>
		struct nonzero_to_edge_transform
		{
			typedef
				typename impl::crm_graph_edge<RowIter, ColIter, ValIter>::type result_type;

			typedef
				const typename crm_graph<RowIter, ColIter, ValIter>::traits::row_nonzero_descriptor argument_type;

			result_type operator() (argument_type arg) const
			{
				return (result_type(arg._row, arg._column, arg._nzi));
			}
		};*/

		template <class RowIter, class ColIter, class ValIter>
		struct nonzero_to_adjacency_transform
		{
			typedef
				typename crm_graph<RowIter, ColIter, ValIter>::traits::index_type result_type;
			typedef
				const typename crm_graph<RowIter, ColIter, ValIter>::traits::row_nonzero_descriptor argument_type;

			result_type operator() (argument_type arg) const
			{
				return (arg._column);
			}
		};
	}

	template <class RowIter, class ColIter, class ValIter>
	struct graph_traits< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >
	{
		typedef yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> smatrix_type;

		typedef typename yasmic::smatrix_traits<smatrix_type>::index_type vertex_descriptor;
		typedef typename yasmic::smatrix_traits<smatrix_type>::row_nonzero_descriptor edge_descriptor;

		typedef
			boost::transform_iterator<
				impl::nonzero_to_adjacency_transform<RowIter, ColIter, ValIter>,
				typename yasmic::smatrix_traits<smatrix_type>::row_nonzero_iterator >
			adjacency_iterator;

		typedef typename yasmic::smatrix_traits<smatrix_type>::row_nonzero_iterator
			out_edge_iterator;

		typedef void in_edge_iterator;

		typedef typename yasmic::smatrix_traits<smatrix_type>::row_iterator vertex_iterator;

		typedef typename yasmic::smatrix_traits<smatrix_type>::nonzero_iterator edge_iterator;

		typedef directed_tag directed_category;
		typedef allow_parallel_edge_tag edge_parallel_category;
		typedef yasmic_compressed_row_matrix_traversal_category traversal_category;
		typedef typename yasmic::smatrix_traits<smatrix_type>::index_type vertices_size_type;
		typedef typename yasmic::smatrix_traits<smatrix_type>::size_type edges_size_type;
		typedef typename yasmic::smatrix_traits<smatrix_type>::size_type degree_size_type;

        static vertex_descriptor null_vertex()
        {
            return std::numeric_limits<vertex_descriptor>::max();
        }
	};
    template <class RowIter, class ColIter, class ValIter>
	struct graph_traits< const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >
        : graph_traits< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >
    {};

    template <class RowIter, class ColIter, class ValIter>
    struct edge_property_type< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >  {
        typedef void type;
    };

    template <class RowIter, class ColIter, class ValIter>
    struct vertex_property_type< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >  {
        typedef void type;
    };

	namespace impl
	{

		template <class V, class S>
		class crm_graph_edge_type :
	    	public std::pair<V,V>
		{
		public:
    		crm_graph_edge_type() { }
    		crm_graph_edge_type(V s, V d, S id)
    			: std::pair<V,V>(s, d), _id(id) { }

    		S id() { return (_id); }

		protected:
	    	S _id;
	    };

		template <class RowIter, class ColIter, class ValIter>
		struct crm_graph_edge
		{
			typedef typename yasmic::smatrix_traits<typename impl::crm_graph<RowIter, ColIter, ValIter>::type >::index_type V;
			typedef typename yasmic::smatrix_traits<typename impl::crm_graph<RowIter, ColIter, ValIter>::type >::size_type S;

			typedef crm_graph_edge_type<V,S> type;
		};

		template <class RowIter, class ColIter, class ValIter>
		class crm_graph_edge_iter
		: public boost::iterator_facade<
            crm_graph_edge_iter<RowIter, ColIter, ValIter>,
			typename crm_graph_edge<RowIter, ColIter, ValIter>::type const,
            boost::forward_traversal_tag,
            typename crm_graph_edge<RowIter, ColIter, ValIter>::type const >
		{
		public:
            crm_graph_edge_iter() {}

			crm_graph_edge_iter(
                RowIter ri, RowIter rend, ColIter ci)
            : _ri(ri), _rend(rend), _ci(ci), _id(0), _row(0)
            {
				_ri++;
            	// skip over any empty rows
            	while ( _ri != _rend && *(_ri) == _id )
                {
                   	// keep incrementing the row
                   	++_ri; ++_row;
                }
            }

			crm_graph_edge_iter(
                RowIter ri, RowIter rend, ColIter ci,
				typename std::iterator_traits<RowIter>::value_type id)
			: _ri(ri), _rend(rend), _ci(ci), _id(id), _row(0)
			{
			}

		private:
            friend class boost::iterator_core_access;

            void increment()
            {
            	// just increment everything!
            	++_ci; ++_id;

                if (_id == *_ri)
                {
                	++_ri; ++_row;

                	// while we aren't at the end and the row isn't empty
                	// (if *_ri == _id, then the row is empty because _id
                	// is the current index in the column/val array.)
                	while ( _ri != _rend && *(_ri) == _id )
                    {
                    	// keep incrementing the row
                    	++_ri; ++_row;
                    }
                }
            }

			bool equal(crm_graph_edge_iter const& other) const
            {
				return (_ri == other._ri && _ci == other._ci);
            }

            typename crm_graph_edge<RowIter, ColIter, ValIter>::type
            dereference() const
            {
            	//return boost::make_tuple(_row, *_ci, *_vi);
				return edge_type(_row, *_ci, _id);
            }

			typedef typename crm_graph_edge<RowIter, ColIter, ValIter>::type edge_type;
            typename std::iterator_traits<RowIter>::value_type _row;
            typename std::iterator_traits<RowIter>::value_type _id;
            RowIter _ri, _rend;
            ColIter _ci;
		};

		template <class RowIter, class ColIter, class ValIter>
		struct crm_graph_ret
		{
			typedef yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> smatrix_type;
			typedef yasmic::smatrix_traits<smatrix_type> smatrix_traits;

			typedef boost::graph_traits<smatrix_type> g_traits;

			typedef std::pair< typename g_traits::vertex_iterator, typename g_traits::vertex_iterator >
				vertices_ret;

			typedef std::pair< typename g_traits::adjacency_iterator, typename g_traits::adjacency_iterator >
				adjacent_ret;

			typedef std::pair < typename g_traits::out_edge_iterator, typename g_traits::out_edge_iterator >
				out_edges_ret;

			typedef std::pair < typename g_traits::edge_iterator, typename g_traits::edge_iterator >
				edges_ret;
		};
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename boost::graph_traits< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >::vertices_size_type
		num_vertices(const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return nrows(g);
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename boost::graph_traits< yasmic::compressed_row_matrix<RowIter, ColIter, ValIter> >::vertices_size_type
		num_edges(const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return nnz(g);
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_descriptor
	source(
		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::edge_descriptor e,
		const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return (e._row);
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_descriptor
	target(
		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::edge_descriptor e,
		const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return (e._column);
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::out_edges_ret
		out_edges(typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_descriptor v,
			  const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return (row_nonzeros(v,g));

		/*typedef typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::out_edge_iterator Iter;
		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::out_edges_ret return_type;

		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::smatrix_traits::row_nonzero_iterator rnzi, rnziend;

		tie(rnzi, rnziend) = yasmic::row_nonzeros(v, g);

		typedef typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::out_edge_iterator iter;

		return std::make_pair(iter(rnzi), iter(rnziend));*/

		/*typedef typename detail::val_out_edge_iter<EdgeList>::type Iter;
		typedef typename detail::val_out_edge_ret<EdgeList>::type return_type;
		return return_type(Iter(v, ++g[v].begin(), *(g[v].begin())),
    		Iter(v, g[v].end(), *(g[v].begin())));*/
	}

	template <class RowIter, class ColIter, class ValIter>
	typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::degree_size_type
	out_degree(
		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_descriptor v,
		const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		return (g._rstart[v+1] - g._rstart[v]);

		/*typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::smatrix_traits::row_nonzero_iterator rnzi, rnziend;

		tie(rnzi, rnziend) = yasmic::row_nonzeros(v, g);

		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::smatrix_traits::size_type rval = 0;
		while (rnzi != rnziend) { ++rnzi; ++rval; }

		return (rval);*/

	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::adjacent_ret
		adjacent_vertices(
			typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_descriptor v,
			const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::smatrix_traits::row_nonzero_iterator rnzi, rnziend;

		tie(rnzi, rnziend) = yasmic::row_nonzeros(v, g);

		typedef typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::adjacency_iterator iter;

		return std::make_pair(iter(rnzi), iter(rnziend));
	}

	// source() and target() already provided for pairs in graph_traits.hpp

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::vertices_ret
		vertices(const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		//typedef typename boost::integer_range<typename EdgeList::value_type>
		//::iterator Iter;
		typedef typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_iterator iter;
		return std::make_pair(iter(0), iter(nrows(g)));
	}

	template <class RowIter, class ColIter, class ValIter>
	inline typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::edges_ret
		edges(const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
		//typedef typename boost::integer_range<typename EdgeList::value_type>
		//::iterator Iter;
		//typedef typename impl::crm_graph_ret<RowIter, ColIter, ValIter>::g_traits::vertex_iterator iter;
		//return std::make_pair(iter(0), iter(nrows(g)));

		return (nonzeros(g));
	}

	template <class RowIter, class ColIter, class ValIter>
	class compressed_row_matrix_graph_id_map
		: public put_get_helper<
			typename yasmic::smatrix_traits< typename impl::crm_graph<RowIter, ColIter, ValIter>::type >::size_type,
    		compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> >
	{
	protected:
  		typedef typename yasmic::smatrix_traits< typename impl::crm_graph<RowIter, ColIter, ValIter>::type >::size_type S;
	public:
  		typedef readable_property_map_tag category;
  		typedef S value_type;
  		typedef S reference;
		typedef typename graph_traits< typename impl::crm_graph<RowIter, ColIter, ValIter>::type >::edge_descriptor key_type;
		//typedef typename impl::crm_graph_edge<RowIter, ColIter, ValIter>::type key_type;
  		//typedef typename detail::crm_graph_edge<RowIter, ColIter, ValIter>::type key_type;

  		compressed_row_matrix_graph_id_map() { }
  		template <class T> S operator [] (T x) const { return x._nzi; }
	};

	template <class RowIter, class ColIter, class ValIter>
	inline identity_property_map get(vertex_index_t,
		const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
  		identity_property_map pmap;
  		return (pmap);
	}

	template <class RowIter, class ColIter, class ValIter>
	inline compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> get(edge_index_t,
  		const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
  		compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> pmap;
  		//return (vector_graph_id_map<EdgeList>());
  		return (pmap);
	}

	template <class Tag>
	struct compressed_row_matrix_graph_property_map { };

	template <>
	struct compressed_row_matrix_graph_property_map<vertex_index_t> {
  		template <class RowIter, class ColIter, class ValIter>
  		struct bind_ {
  			typedef identity_property_map type;
  			typedef identity_property_map const_type;
  		};
	};

	template <>
	struct compressed_row_matrix_graph_property_map<edge_index_t> {
  		template <class RowIter, class ColIter, class ValIter>
  		struct bind_ {
  			typedef compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> type;
  			typedef compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> const_type;
  		};
	};

	template <>
	struct compressed_row_matrix_graph_property_map<edge_weight_t> {
		template <class RowIter, class ColIter, class ValIter>
		struct bind_ {
			typedef iterator_property_map<ValIter, compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> > type;
			typedef iterator_property_map<ValIter, compressed_row_matrix_graph_id_map<RowIter, ColIter, ValIter> > const_type;
		};
	};

	template <class RowIter, class ColIter, class ValIter, class Tag>
	struct property_map<yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>, Tag> {
  		typedef typename compressed_row_matrix_graph_property_map<Tag>::template
  			bind_<RowIter, ColIter, ValIter> map_gen;
  		typedef typename map_gen::type type;
  		typedef typename map_gen::const_type const_type;
	};

	template <class RowIter, class ColIter, class ValIter>
	inline typename property_map<yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>, edge_weight_t>::type
		get(edge_weight_t, const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g)
	{
  		return (make_iterator_property_map(g.begin_values(), get(edge_index, g)));
	}

    /*template <class Tag, class RowIter, class ColIter, class ValIter, class Key>
    inline typename typename property_map<yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>, Tag>::value_type
		get(Tag t, const yasmic::compressed_row_matrix<RowIter, ColIter, ValIter>& g, Key key)
	{
        return get(get(t, g), key);
  		//return (make_iterator_property_map(g.begin_values(), get(edge_index, g)));
	}*/

}

#if _MSC_VER >= 1400
	// disable the warning for deprecated c++ commands
	#pragma warning( pop )
#endif // _MSC_VER >= 1400

#endif // YASMIC_COMPRESSED_ROW_MATRIX_GRAPH


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <arrayfire.h>
#include <gauss/clustering.h>
#include <gauss/internal/scopedHostPtr.h>
#include <gauss/normalization.h>
#include <gauss/linalg.h>
#include <gauss/random.h>

#include <Eigen/Eigenvalues>
#include <limits>
#include <random>
#include <tuple>
#include <iostream>
#include <chrono>

namespace gauss::clustering
{
    /**
     * Computes initial k means or centroids.
     *
     * @param tts       The time series.
     * @param k         The number of centroids.
     * @return          The new centroids.
     */
    af::array calculateInitialMeans(const af::array &tss, int k)
    {
        return af::constant(0, tss.dims(0), k, tss.type());
    }

    /**
     * Computes The euclidean distance for a tiled time series agains k-means.
     *
     * @param tts       The tiled time series.
     * @param means     The centroids.
     * @return          The distance from a time series to all k-means.
     */
    af::array kEuclideanDistance(const af::array &tts, const af::array &means)
    {
        return af::reorder(af::sqrt(af::sum(af::pow((tts - means), 2), 0)), 1, 0);
    }

    /**
     * Computes the euclidean distance of each time series w.r.t. all k-means.
     *
     * @param tss           The time series.
     * @param means         The k-means.
     * @param minDistance   The resulting distance for each time series to all k-means.
     * @param labels        The ids of the closes mean for all time series.
     */
    void euclideanDistance(const af::array &tss, const af::array &means, af::array &minDistance, af::array &idxs)
    {
        auto nSeries = tss.dims(1);
        af::array kDistances = af::constant(0.0, means.dims(1), nSeries, tss.type());

        // This for loop could be parallel, not parallelized to keep memory footprint low
        for (int i = 0; i < nSeries; i++)
        {
            af::array tiledSeries = af::tile(tss.col(i), 1, means.dims(1));
            kDistances(af::span, i) = kEuclideanDistance(tiledSeries, means);
        }
        af::min(minDistance, idxs, kDistances, 0);
    }

    /**
     * Compute the new means for the i-th iteration.
     *
     * @param tss       The time series.
     * @param labels    The ids for each time series which indicates the closest mean.
     * @param k         Number of means.
     * @return          The new means.
     */
    af::array computeNewMeans(const af::array &tss, const af::array &labels, int k)
    {
        af::array labelsTiled = af::tile(labels, tss.dims(0));
        af::array newMeans = af::constant(0.0, tss.dims(0), k, tss.type());

        gfor(af::seq ii, k)
        {
            newMeans(af::span, ii) = af::sum(tss * (labelsTiled == ii), 1) / (af::sum(af::sum((labels == ii), 3), 1));
        }
        return newMeans;
    }

    /**
     *  This function generates random labels for n time series.
     *
     * @param nTimeSeries   Number of time series to be labeled.
     * @param k             The number of groups.
     * @return              The random labels.
     */
    af::array generateRandomLabels(int nTimeSeries, int k)
    {
        std::vector<int> idx(nTimeSeries, 0);

        // Fill with sequential data
        for (int i = 0; i < nTimeSeries; i++)
        {
            idx[i] = i % k;
        }

        // Randomize
        std::shuffle(idx.begin(), idx.end(), std::mt19937(std::random_device()()));
        return af::array(nTimeSeries, 1, idx.data());
    }

    /**
     * Computes the means' difference between two iterations.
     *
     * @param means     The last iteration means
     * @param newMeans  The newMeans
     * @return          The accumulated change ratio between iterations.
     */
    float computeError(const af::array &means, const af::array &newMeans)
    {
        auto err = af::sum(af::sqrt(af::sum(af::pow(means - newMeans, 2), 0)));
        if (err.type() != af::dtype::f32)
            err = err.as(af::dtype::f32);
        return err.scalar<float>();
    }

    void kMeans(const af::array &tss, int k, af::array &centroids, af::array &labels, float tolerance, int maxIterations)
    {
        float error = std::numeric_limits<float>::max();

        if (centroids.isempty())
        {
            // initial guess of means, select k random time series
            centroids = calculateInitialMeans(tss, k);
        }

        if (labels.isempty())
        {
            // assigns a random centroid to every time series
            labels = generateRandomLabels(tss.dims(1), k);
        }

        af::array distances = af::constant(0, tss.dims(1), tss.type());
        af::array newMeans;
        int iter = 0;

        // Stop updating after convergence is reached.
        while ((error > tolerance) && (iter < maxIterations))
        {
            // 1. Compute distances to current means
            euclideanDistance(tss, centroids, distances, labels);

            // 2. Compute new means
            newMeans = computeNewMeans(tss, labels, k);

            // 3. Compute convergence
            error = computeError(centroids, newMeans);

            // 4. Update Means
            centroids = newMeans;
            iter++;
        }
    }

    //////////////////
    // K-Shape
    //////////////////

    /**
     * Computes the normalized crosscorrelation for all time series and all centroids.
     *
     * @param tss       The set of time series.
     * @param centroids The set of centroids.
     * @return          The computed normalized CrossCorrelation.
     */
    af::array ncc3Dim(const af::array &tss, const af::array &centroids) {
        auto normtss = af::sqrt(af::sum(af::pow(tss, 2.0), 0));
        auto normcen = af::sqrt(af::sum(af::pow(centroids, 2.0), 0));
        auto den = af::matmulTN(normcen, normtss);
        auto den_tiled = af::tile(den, 1, 1, (centroids.dims(0)*2)-1);

        auto inv_centroids = af::flip(centroids, 0);
        auto batched = af::reorder(inv_centroids, 0, 2, 1, 3);
        auto convolution = af::convolve1(tss, batched, AF_CONV_EXPAND);
        auto shaped = af::reorder(convolution, 2, 1, 0);
        return shaped / den_tiled;
    }

    /**
     * This function computes the assignment step. It is the update of time series labels w.r.t. the dinamics of the
     * centroids.
     *
     * @param tss       The set of time series in columnar manner.
     * @param centroids The set of centroids in columnar mode.
     * @return          The new set of labels.
     */
    af::array assignmentStep(const af::array &tss, const af::array &centroids)
    {
        auto distances = 1.0 - af::max(ncc3Dim(tss, centroids), 2);
        af::array min;
        af::array labels;
        af::min(min, labels, distances, 0);
        return labels.T();
    }

    /**
     * This function returns an updated shape of the centroid passed as argument w.r.t. the tss.
     *
     * @param tss       The subset of time series acting on the centroid.  These are znorm.
     * @param centroid  The given centroid (which could be all zeros in the first iteration or
     *                  znorm centroid in further iterations).
     * @return          The updated shape of the centroid znorm.
     */
    af::array shapeExtraction(const af::array &tss, const af::array &centroid, const af::array &p)
    {
        // since data is always znorm and so are the centroids,
        // it is not necessary to invoke sbd, ever!
        auto s = af::matmulNT(tss, tss);
        auto m = af::matmul(p, s, p);
        af::array eigvec;
        std::tie (std::ignore, eigvec) = gauss::linalg::eigh(m);
        // last column, per eigh, is the eigenvector whose
        // eigenval is max.
        auto c = eigvec.col(af::end);
        auto z_c = gauss::normalization::znorm(c, 0, 1);

        auto findDistance1 = af::sqrt(af::sum(af::pow((tss.col(0) - c), 2.0)));
        auto findDistance2 = af::sqrt(af::sum(af::pow((tss.col(0) + c), 2.0)));
        auto condition = findDistance1 >= findDistance2;

        // use select instead of 'if'
        auto condition_tied = af::tile(condition, tss.dims(0), 1);
        return af::select(condition_tied, -1.0 * z_c, z_c);
    }

    /**
     * This function performs the refinement step.
     *
     * @param tss       The set of time series in columnar manner.
     * @param centroids The set of centroids in columnar mode.
     * @param labels    The set of labels.
     * @return          The new centroids.
     */
    af::array refinementStep(const af::array &tss, const af::array &centroids, const af::array &labels)
    {
        auto ncentroids = centroids.dims(1);
        af::array result = centroids;

        // prepare p array
        // this used to be @ shapeExtraction method but it is a constant throughout
        // the entire process...
        auto nelements = tss.dims(0);
        auto scale = 1.0 / static_cast<double>(nelements);
        auto scale_tiled = af::constant(scale, nelements, nelements, tss.type());
        auto p = af::identity(nelements, nelements, tss.type()) - scale_tiled;

        for (dim_t j = 0; j < ncentroids; j++) {
            // current centroid
            auto centroid = centroids.col(j);
            // select those tss assigned to the centroid j
            auto subset = af::lookup(tss, af::where((labels == j)), 1);
            // if centroid j has at least one labeled time series.
            if (!subset.isempty()) {
                result(af::span, j) = shapeExtraction(subset, centroid, p);
            }
        }

        return result;
    }

    void kshape_calibrate(const af::array &tss, int k, af::array &centroids, af::array &labels, const int maxIterations, const bool rnd_labels)
    {
        auto nTimeseries = static_cast<unsigned int>(tss.dims(1));
        auto nElements = static_cast<unsigned int>(tss.dims(0));

        if (centroids.isempty()) {
            centroids = af::constant(0, nElements, k, tss.type());
        }

        if (labels.isempty()) {
            labels = rnd_labels
                ? gauss::random::randint(0, k, af::dim4(nTimeseries), af::dtype::u32)
                : af::iota(af::dim4(nTimeseries), af::dim4(1), af::dtype::u32) % k;
        }
        else {
            if (labels.dims(0) != nTimeseries)
                throw std::invalid_argument("The number of labels must be equal to the number of time series");

            if (labels.type() != af::dtype::u32)
                labels = labels.as(af::dtype::u32);

            auto unique_labels = af::setUnique(labels, false);
            if (unique_labels.dims(0) != k)
                throw std::invalid_argument("The number of unique labels do not correspond to the number of clusters");

            if (!af::allTrue<bool>(unique_labels >= 0 && unique_labels < k))
                throw std::invalid_argument("The labels should be identified with values ranging from 0 up to the number of clusters");
        }

        auto terminate = false;
        int iter = 0;

        // 0. Ensure tss is normalized
        auto normTSS = gauss::normalization::znorm(tss, 0, 1);

        while (!terminate)
        {
            // 1. Refinement step. New centroids computation.
            auto newCentroids = refinementStep(normTSS, centroids, labels);

            // 2. Assignment step. New labels computation.
            auto new_labels = assignmentStep(normTSS, newCentroids);

            // 3. Update centroids
            centroids = newCentroids;

            // 4. Check if no movement in labels or max iterations reached
            terminate = iter++ == maxIterations || af::allTrue<bool>(new_labels == labels);

            // 5. Update labels.
            labels = new_labels;
        }
    }

    af::array kshape_classify(const af::array &tss, const af::array &centroids) {
        auto normTSS = gauss::normalization::znorm(tss, 0, 1);
        return assignmentStep(normTSS, centroids);
    }
}