/**
 * @file TestSolverLqrHelpers.cpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-07
 */

#include <string>
#include <iostream>
#include <Eigen/Core>
#include <gtest/gtest.h>
#include <yaml_cpp_catkin/yaml_eigen.h>

#include <solver_lqr/SolverLqr.hpp>

class SolverLqrTest : public ::testing::Test
{
  protected:
    virtual void SetUp() {}
    virtual void TearDown() {}
};

using namespace solverlqr;

void check_state_base(const StateBase& S, double scalar)
{
  for (int i=0; i<S.stateVector().size(); i++)
    EXPECT_NEAR(scalar, S.stateVector()[i], 0.01);
}

TEST_F(SolverLqrTest, state_operators_test)
{
  int xdim = 4;
  double lambda = 0.5, ref1 = 1., ref2 = 2.;
  StateBase s1(xdim), s2(xdim), s3(xdim);
  s1.stateVector().setConstant(ref1);
  s2.stateVector().setConstant(ref2);

  // Checking correct initialization
  check_state_base(s1, ref1);
  check_state_base(s2, ref2);

  // Checking += operator
  s1 += s2;
  check_state_base(s1, ref1+ref2);
  check_state_base(s2, ref2);

  // Checking *= operator
  s1 *= lambda;
  s2 *= pow(lambda, 2.);
  check_state_base(s1, lambda*(ref1+ref2));
  check_state_base(s2, pow(lambda,2.)*ref2);

  // Checking + and * operators in different orders
  s3 = (s1 + s2*lambda)*lambda;
  check_state_base(s1, lambda*(ref1+ref2));
  check_state_base(s2, pow(lambda,2.)*ref2);
  check_state_base(s3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));

  s3 = lambda*(lambda*s2 + s1);
  check_state_base(s1, lambda*(ref1+ref2));
  check_state_base(s2, pow(lambda,2.)*ref2);
  check_state_base(s3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));

  // Checking assignment operator
  s1 = s3; s2 = s3;
  check_state_base(s1, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
  check_state_base(s2, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
  check_state_base(s3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
}

void check_control_base(const ControlBase& ctrl, double scalar)
{
  for (int i=0; i<ctrl.feedforward().size(); i++)
    EXPECT_NEAR(scalar, ctrl.feedforward()[i], 0.01);
}

TEST_F(SolverLqrTest, control_operators_test)
{
  int xdim = 4, udim = 2;
  double lambda = 0.5, ref1 = 1., ref2 = 2.;
  ControlBase c1(xdim,udim), c2(xdim,udim), c3(xdim,udim);
  c1.feedforward().setConstant(ref1);  c1.feedback().setConstant(0.0);
  c2.feedforward().setConstant(ref2);  c2.feedback().setConstant(0.0);

  // Checking correct initialization
  check_control_base(c1, ref1);
  check_control_base(c2, ref2);

  // Checking += operator
  c1 += c2;
  check_control_base(c1, ref1+ref2);
  check_control_base(c2, ref2);

  // Checking *= operator
  c1 *= lambda;
  c2 *= pow(lambda, 2.);
  check_control_base(c1, lambda*(ref1+ref2));
  check_control_base(c2, pow(lambda,2.)*ref2);

  // Checking + and * operators in different orders
  c3 = (c1 + c2*lambda)*lambda;
  check_control_base(c1, lambda*(ref1+ref2));
  check_control_base(c2, pow(lambda,2.)*ref2);
  check_control_base(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));

  c3 = lambda*(lambda*c2 + c1);
  check_control_base(c1, lambda*(ref1+ref2));
  check_control_base(c2, pow(lambda,2.)*ref2);
  check_control_base(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));

  // Checking assignment operator
  c1 = c3; c2 = c3;
  check_control_base(c1, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
  check_control_base(c2, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
  check_control_base(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2));
}

void check_control_sequence(const ControlSequence& ctrl, double ref_ff, double ref_fb)
{
  for (int time=0; time<ctrl.size(); time++) {
    for (int row=0; row<ctrl.control(time).feedback().rows(); row++) {
      EXPECT_NEAR(ref_ff, ctrl.control(time).feedforward()[row], 0.01);
      for (int col=0; col<ctrl.control(time).feedback().cols(); col++)
        EXPECT_NEAR(ref_fb, ctrl.control(time).feedback()(row,col), 0.01);
    }
  }
}

TEST_F(SolverLqrTest, control_sequence_operators_test)
{
  int tdim = 10, xdim = 4, udim = 2;
  double lambda = 0.5, ref1 = 1., ref2 = 2., ref3 = 0.1, ref4 = 0.2;
  ControlSequence c1, c2, c3;
  c1.resize(tdim, xdim, udim);
  c2.resize(tdim, xdim, udim);
  c3.resize(tdim, xdim, udim);

  for (int time=0; time<tdim; time++)
  {
    c1.control(time).feedforward().setConstant(ref1);  c1.control(time).feedback().setConstant(ref3);
    c2.control(time).feedforward().setConstant(ref2);  c2.control(time).feedback().setConstant(ref4);
  }

  // Checking correct initialization
  check_control_sequence(c1, ref1, ref3);
  check_control_sequence(c2, ref2, ref4);

  // Checking += operator
  c1 += c2;
  check_control_sequence(c1, ref1+ref2, ref4);
  check_control_sequence(c2, ref2, ref4);

  // Checking *= operator
  c1 *= lambda;
  c2 *= pow(lambda, 2.);
  check_control_sequence(c1, lambda*(ref1+ref2), ref4);
  check_control_sequence(c2, pow(lambda,2.)*ref2, ref4);

  // Checking + and * operators in different orders
  c3 = (c1 + c2*lambda)*lambda;
  check_control_sequence(c1, lambda*(ref1+ref2), ref4);
  check_control_sequence(c2, pow(lambda,2.)*ref2, ref4);
  check_control_sequence(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2), ref4);

  c3 = lambda*(lambda*c2 + c1);
  check_control_sequence(c1, lambda*(ref1+ref2), ref4);
  check_control_sequence(c2, pow(lambda,2.)*ref2, ref4);
  check_control_sequence(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2), ref4);

  // Checking assignment operator
  c1 = c3; c2 = c3;
  check_control_sequence(c1, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2), ref4);
  check_control_sequence(c2, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2), ref4);
  check_control_sequence(c3, lambda*(lambda*(ref1+ref2) + lambda*pow(lambda,2.)*ref2), ref4);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "detail/r1cs_examples.hpp"
#include "detail/sha256_component.hpp"

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/marshalling/status_type.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;

typedef algebra::curves::bls12<381> curve_type;
typedef typename curve_type::scalar_field_type field_type;

typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

int main(int argc, char *argv[]) {
    boost::filesystem::path pout, pkout, vkout, piout, viout;
    boost::program_options::options_description options(
        "R1CS Generic Group PreProcessing Zero-Knowledge Succinct Non-interactive ARgument of Knowledge "
        "(https://eprint.iacr.org/2016/260.pdf) CLI Proof Generator");
    // clang-format off
    options.add_options()("help,h", "Display help message")
    ("version,v", "Display version")
    ("generate", "Generate proofs and/or keys")
    ("verify", "verify proofs and/or keys")
    ("proof-output,po", boost::program_options::value<boost::filesystem::path>(&pout)->default_value("proof"))
    ("primary-input-output,pio", boost::program_options::value<boost::filesystem::path>(&piout)->default_value
("pinput"))
    ("proving-key-output,pko", boost::program_options::value<boost::filesystem::path>(&pkout)->default_value("pkey"))
    ("verifying-key-output,vko", boost::program_options::value<boost::filesystem::path>(&viout)->default_value("vkey"))
    ("verifier-input-output,vio", boost::program_options::value<boost::filesystem::path>(&vkout)->default_value("vio"));
    // clang-format on

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help") || argc < 2) {
        std::cout << options << std::endl;
        return 0;
    }

    std::cout << "SHA2-256 blueprint generation started." << std::endl;

    components::blueprint<field_type> bp = sha2_two_to_one_bp<field_type>();

    std::cout << "SHA2-256 blueprint generation finished." << std::endl;

    std::cout << "R1CS generation started." << std::endl;

    r1cs_example<field_type> example =
        r1cs_example<field_type>(bp.get_constraint_system(), bp.primary_input(), bp.auxiliary_input());

    std::cout << "R1CS generation finished." << std::endl;

    // const bool bit = run_r1cs_gg_ppzksnark<curve_type>(example);

    // zk::snark::detail::r1cs_example<field_type> example =
    //     zk::snark::detail::r1cs_example<field_type>(bp.get_constraint_system(), bp.primary_input(),
    //     bp.auxiliary_input());

    // zk::snark::r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();

    std::cout << "Starting generator" << std::endl;

    typename scheme_type::keypair_type keypair = zk::snark::generate<scheme_type>(example.constraint_system);

    std::cout << "Starting prover" << std::endl;

    const typename scheme_type::proof_type proof =
        prove<scheme_type>(keypair.first, example.primary_input, example.auxiliary_input);

    // std::cout << "Starting verifier" << std::endl;

    // const bool ans = verify<basic_proof_system>(keypair.second, example.primary_input, proof);

    // std::cout << "Verifier finished, result: " << ans << std::endl;

    std::vector<std::uint8_t> proving_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.first);
    std::vector<std::uint8_t> verification_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.second);
    std::vector<std::uint8_t> proof_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(proof);
    std::vector<std::uint8_t> primary_input_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(example.primary_input);

    if (vm.count("proving-key-output")) {
        boost::filesystem::ofstream out(pkout);
        for (const auto &v : proving_key_byteblob) {
            out << v;
        }
        out.close();
    }

    if (vm.count("verifying-key-output")) {
        boost::filesystem::ofstream out(vkout);
        for (const auto &v : verification_key_byteblob) {
            out << v;
        }
        out.close();
    }

    if (vm.count("proof-output")) {
        boost::filesystem::ofstream out(pout);
        for (const auto &v : proof_byteblob) {
            out << v;
        }
        out.close();
    }

    if (vm.count("primary-input-output")) {
        boost::filesystem::ofstream out(piout);
        for (const auto &v : primary_input_byteblob) {
            out << v;
        }
        out.close();
    }

    // nil::marshalling::status_type provingProcessingStatus = nil::marshalling::status_type::success;
    // typename scheme_type::proving_key_type other =
    //             nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proving_key_process(
    //                 proving_key_byteblob.cbegin(),
    //                 proving_key_byteblob.cend(),
    //                 provingProcessingStatus);

    // assert(keypair.first == other);

    if (vm.count("verifier-input-output")) {
        std::vector<std::uint8_t> verifier_input_output_byteblob(proof_byteblob.begin(), proof_byteblob.end());

        verifier_input_output_byteblob.insert(verifier_input_output_byteblob.end(), primary_input_byteblob.begin(),
                                              primary_input_byteblob.end());
        verifier_input_output_byteblob.insert(verifier_input_output_byteblob.end(), verification_key_byteblob.begin(),
                                              verification_key_byteblob.end());

        boost::filesystem::ofstream poutf(pout);
        for (const auto &v : verifier_input_output_byteblob) {
            poutf << v;
        }
        poutf.close();
    }

    return 0;
}


#pragma once

#include <Eigen/Dense>

#include <PnC/WBC/Task.hpp>

class FootRzXYZTask : public Task {
   public:
    FootRzXYZTask(RobotSystem* robot_, int _link_idx_);
    virtual ~FootRzXYZTask();

   private:
    /* Update op_cmd, pos_err, vel_des, acc_des
     *
     * pos_des_ = [quat_w, quat_x, quat_y, quat_z, x, y, z]
     * vel_des_ = [w_x, w_y, w_z, xdot, ydot, zdot]
     * acc_des_ = [a_x, a_y, a_z, xddot, yddot, zddot]
     *
     */
    virtual bool _UpdateCommand(const Eigen::VectorXd& pos_des,
                                const Eigen::VectorXd& vel_des,
                                const Eigen::VectorXd& acc_des);
    virtual bool _UpdateTaskJacobian();
    virtual bool _UpdateTaskJDotQdot();

    int link_idx_;
};


#ifndef STAN_MATH_MPI_COVAR_ESTIMATOR_HPP
#define STAN_MATH_MPI_COVAR_ESTIMATOR_HPP

#ifdef STAN_LANG_MPI

#include <stan/math/torsten/mpi/environment.hpp>
#include <stan/math/prim/fun/welford_covar_estimator.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/mpi.hpp>
#include <algorithm>
#include <vector>

namespace stan {
namespace math {
namespace mpi {

  /*
   * brute force by using welford. Assuming all chains have
   * same nb. of draws.
   */
class mpi_covar_estimator {
  boost::accumulators::accumulator_set<int, boost::accumulators::features<boost::accumulators::tag::count>> count_acc_;
 public:
  size_t num_iterations;

protected:
  Eigen::MatrixXd draws_;

public:
  explicit mpi_covar_estimator(size_t num_params,
                               size_t num_iterations0)
    : num_iterations(num_iterations0),
      draws_(num_params, num_iterations)
  {}

  void restart() { count_acc_ = {}; }

  void restart(size_t num_params) { count_acc_ = {}; }

  inline void add_sample(const Eigen::VectorXd& q) {
    int i = boost::accumulators::count(count_acc_);
    draws_.col(i) = q;
    count_acc_(0);
  }

  inline int num_samples(const Communicator& comm) {
    int n = boost::accumulators::count(count_acc_);
    int n_all = n * comm.size();
    return n_all;
  }

  inline void sample_mean(Eigen::VectorXd& var) {
    // todo
  }

  inline void sample_covariance(Eigen::MatrixXd& covar, const Communicator& comm) {
    sample_covariance(covar, 0, boost::accumulators::count(count_acc_), comm);
  }

  inline void sample_covariance(Eigen::MatrixXd& covar,
                                int col_begin, int n_samples,
                                const Communicator& comm) {
    int num_params = draws_.rows();
    Eigen::MatrixXd all_draws(num_params, n_samples * comm.size());
    MPI_Allgather(draws_.data() + col_begin * num_params,
                  n_samples * num_params, MPI_DOUBLE, all_draws.data(),
                  n_samples * num_params, MPI_DOUBLE, comm.comm());
    welford_covar_estimator est(num_params);
    for (int j = 0; j < all_draws.cols(); ++j) {
      est.add_sample(all_draws.col(j));
    }
    est.sample_covariance(covar);
  }

  inline size_t num_params() {
    return draws_.rows();
  }
};

}
}  // namespace math
}  // namespace stan

#endif

#endif


#include <memory>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/common/memory.h>
#include <aslam/frames/visual-frame.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
#include <ceres-error-terms/visual-error-term.h>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/file-logger.h>
#include <maplab-common/gravity-provider.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <simulation/generic-path-generator.h>
#include <vi-map/landmark.h>
#include <vi-map/mission.h>
#include <vi-map/pose-graph.h>
#include <vi-map/vertex.h>
#include <vi-map/viwls-edge.h>

namespace map_optimization_legacy {

// TODO(dymczykm) merge with other vi_map fixtures?
class ViwlsGraph : public ::testing::Test {
 protected:
  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;

  virtual void SetUp() {
    distortion_param_ = 0.0;
    fu_ = 100;
    fv_ = 100;
    res_u_ = 640;
    res_v_ = 480;
    cu_ = res_u_ / 2.0;
    cv_ = res_v_ / 2.0;

    common::GravityProvider gravity_provider(
        common::locations::kAltitudeZurichMeters,
        common::locations::kLatitudeZurichDegrees);

    // No IMU noise considered.
    settings_.imu_sigmas.acc_bias_random_walk_noise_density = 0.0;
    settings_.imu_sigmas.acc_noise_density = 0.0;
    settings_.gravity_meter_by_second2 = gravity_provider.getGravityMagnitude();
    settings_.imu_sigmas.gyro_bias_random_walk_noise_density = 0.0;
    settings_.imu_sigmas.gyro_noise_density = 0.0;
    // Seed does not matter here, we don't want noise anyways.
    settings_.imu_noise_bias_seed = 1;
    settings_.landmark_seed = 10;
    settings_.landmark_variance_meter = 3.0;
    settings_.mode = test_trajectory_gen::Path::kCircular;
    settings_.num_of_landmarks = 500;
    // 8 points for path constraints.
    settings_.num_of_path_constraints = 8;
    settings_.sampling_time_second = 1.0;
    id_counter_ = 0;
  }

  void constructCamera() {
    Eigen::VectorXd distortion_parameters(1);
    distortion_parameters << distortion_param_;
    aslam::Distortion::UniquePtr distortion(
        new DistortionType(distortion_parameters));

    Eigen::VectorXd intrinsics(4);
    intrinsics << fu_, fv_, cu_, cv_;

    aslam::Camera::Ptr camera = std::shared_ptr<CameraType>(
        new CameraType(intrinsics, res_u_, res_v_, distortion));

    aslam::CameraId camera_id;
    common::generateId(&camera_id);
    camera->setId(camera_id);

    std::vector<aslam::Camera::Ptr> camera_vector;
    camera_vector.push_back(camera);
    aslam::TransformationVector T_C_B_vector;
    // We use identity transformation to T_C_B from default constructor.
    T_C_B_vector.resize(kNumCameras);
    aslam::NCameraId n_camera_id;
    common::generateId(&n_camera_id);
    cameras_.reset(
        new aslam::NCamera(
            n_camera_id, T_C_B_vector, camera_vector, "Test camera rig"));
  }

  void constructTrajectoryGenerator() {
    generator_ = std::shared_ptr<test_trajectory_gen::GenericPathGenerator>(
        new test_trajectory_gen::GenericPathGenerator(settings_));
  }

  void generatePathAndLandmarks() {
    LOG(INFO) << "Generating trajectory, may take a while...";

    generator_->generatePath();
    generator_->generateLandmarks();
    positions_ = generator_->getTruePositions();
    rotations_ = generator_->getTrueRotations();
    Eigen::Matrix3Xd keypoints_ = generator_->getLandmarks();

    // A slight simplification below - all the landmarks are
    // kept on a single map, as agreed they should be kept as continuous
    // memory in one of the vertices.
    for (int i = 0; i < keypoints_.cols(); ++i) {
      vi_map::Landmark::Ptr landmark_ptr(new vi_map::Landmark);
      vi_map::LandmarkId landmark_id;
      generateIdFromInt(id_counter_, &landmark_id);
      landmark_ptr->setId(landmark_id);
      ++id_counter_;

      landmark_ptr->set_p_B(keypoints_.block<3, 1>(0, i));
      landmark_ptr->set_p_B_Covariance(Eigen::Matrix<double, 3, 3>::Identity());
      landmark_store_.insert(std::make_pair(landmark_ptr->id(), landmark_ptr));
      landmark_observation_count_.insert(std::make_pair(landmark_ptr->id(), 0));
    }

    LOG(INFO) << "Number of points: " << positions_.cols();
  }

  void addPosegraphVertices();
  void solve();
  void copyDataFromPosegraph();
  void copyDataToPosegraph();

  vi_map::PoseGraph posegraph_;
  std::vector<pose_graph::VertexId> vertex_ids_;
  std::unordered_map<vi_map::LandmarkId, vi_map::Landmark::Ptr> landmark_store_;
  std::unordered_map<vi_map::LandmarkId, unsigned int>
      landmark_observation_count_;
  std::unordered_map<vi_map::MissionId, std::shared_ptr<vi_map::VIMission> >
      missions_;
  AlignedUnorderedMap<pose_graph::VertexId, Eigen::Vector3d>
      true_vertex_positions_;
  AlignedUnorderedMap<pose_graph::VertexId, Eigen::Quaterniond>
      true_vertex_rotations_;

  aslam::NCamera::Ptr cameras_;

  double distortion_param_;
  double fu_, fv_;
  double cu_, cv_;
  double res_u_, res_v_;

  std::shared_ptr<test_trajectory_gen::GenericPathGenerator> generator_;

  // Path and landmark settings.
  test_trajectory_gen::PathAndLandmarkSettings settings_;

  unsigned int id_counter_;

  Eigen::Matrix3Xd positions_;
  Eigen::Matrix4Xd rotations_;

  // Landmark base pose, common for all.
  Eigen::Vector4d q_G_B_;
  Eigen::Vector3d p_G_B_;

  // Containers for 7-element pose objects.
  Eigen::Matrix<double, 7, 1> base_frame_;
  typedef std::unordered_map<pose_graph::VertexId, int> VertexIdRotationMap;
  VertexIdRotationMap vertex_id_to_pose_idx_;
  Eigen::Matrix<double, 7, Eigen::Dynamic> keyframe_poses_;

  // Ordering is [orientation position] -> [xyzw xyz].
  Eigen::Matrix<double, 7, 1> dummy_7d_0_;
  Eigen::Matrix<double, 7, 1> dummy_7d_1_;

  static constexpr int kNumCameras = 1;
  static constexpr int kVisualFrameIndex = 0;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void ViwlsGraph::addPosegraphVertices() {
  std::mt19937 gen(settings_.landmark_seed);
  std::normal_distribution<> dis(0, 0.3);
  std::normal_distribution<> dis_quat(0, 0.02);

  // We won't use IMU bias in this test scenario.
  Eigen::Matrix<double, 6, 1> imu_bias;
  imu_bias << 1, 2, 3, 4, 5, 6;

  // Base position and rotation.
  q_G_B_ << 0, 0, 0, 1;
  p_G_B_ << 0, 0, 0;
  dummy_7d_0_.setZero();
  dummy_7d_1_.setZero();

  int num_of_poses = positions_.cols();
  vertex_ids_.resize(num_of_poses);

  // Create a mission for all the keyframes and landmarks.
  vi_map::MissionId mission_id;
  generateIdFromInt(id_counter_, &mission_id);
  ++id_counter_;
  vi_map::VIMission::Ptr mission_ptr(new vi_map::VIMission);
  mission_ptr->setId(mission_id);
  missions_.insert(std::make_pair(mission_id, mission_ptr));

  // Rotate the camera 90 deg y so it is looking outwards to the points.
  Eigen::Quaterniond q_I_C(sqrt(2) / 2, 0, sqrt(2) / 2, 0);

  common::FileLogger vertices_log("vertices.csv");
  common::FileLogger landmarks_log("landmarks.csv");

  for (int i = 0; i < num_of_poses; ++i) {
    generateIdFromInt(id_counter_, &vertex_ids_[i]);
    ++id_counter_;

    Eigen::Vector3d p_G_C(positions_.block<3, 1>(0, i));
    Eigen::Quaterniond q_I_G;
    q_I_G.coeffs() = rotations_.block<4, 1>(0, i);
    Eigen::Quaterniond q_G_I = q_I_G.inverse();

    // Make sure the scalar part is positive.
    Eigen::Quaterniond q_G_C = q_G_I * q_I_C;
    if (q_G_C.w() < 0) {
      q_G_C.coeffs() *= -1;
    }

    true_vertex_positions_.insert(std::make_pair(vertex_ids_[i], p_G_C));
    true_vertex_rotations_.insert(std::make_pair(vertex_ids_[i], q_G_C));

    pose::Transformation T_G_C(q_G_C, p_G_C);
    Eigen::Vector3d zaxis(0, 0, 1);
    vertices_log << p_G_C.transpose() << " " << (T_G_C * zaxis).transpose()
                 << std::endl;

    Aligned<std::vector, Eigen::Vector2d> visible_points;
    std::vector<vi_map::LandmarkId> landmark_ids;
    // For each landmark, verify if it's visible from the current pose.
    typedef std::unordered_map<vi_map::LandmarkId,
                               vi_map::Landmark::Ptr>::iterator LandmarkStoreIt;
    for (LandmarkStoreIt iterator = landmark_store_.begin();
         iterator != landmark_store_.end(); ++iterator) {
      vi_map::Landmark::Ptr landmark_ptr = iterator->second;

      Eigen::Vector3d p_G_fi = landmark_ptr->get_p_B();
      Eigen::Vector3d p_C_fi = T_G_C.inverse() * p_G_fi;

      landmarks_log << p_G_fi.transpose() << std::endl;

      Eigen::Vector2d keypoint;
      bool is_visible = static_cast<bool>(
          cameras_->getCamera(kVisualFrameIndex).project3(p_C_fi, &keypoint));

      if (is_visible) {
        landmark_ids.push_back(iterator->first);
        visible_points.push_back(keypoint);
        ++(landmark_observation_count_.find(iterator->first)->second);
      }
    }
    LOG(INFO) << i << "th pose, visible points: " << visible_points.size();

    Eigen::Matrix2Xd points;
    points.resize(Eigen::NoChange, visible_points.size());
    for (unsigned int j = 0; j < visible_points.size(); ++j) {
      points.col(j) << visible_points[j];
    }

    Eigen::VectorXd uncertainties;
    uncertainties.resize(visible_points.size());
    uncertainties.setConstant(0.8);

    aslam::VisualFrame::DescriptorsT descriptors;
    descriptors.resize(48, visible_points.size());
    descriptors.setRandom();

    aslam::FrameId frame_id;
    common::generateId(&frame_id);
    int64_t frame_timestamp = 0;

    CHECK_EQ(landmark_ids.size(), static_cast<unsigned int>(points.cols()));
    posegraph_.addVIVertex(
        vertex_ids_[i], imu_bias, points, uncertainties, descriptors,
        landmark_ids, mission_ptr->id(), frame_id, frame_timestamp, cameras_);

    Eigen::Vector3d vertex_position =  // p_G_C;
        Eigen::Vector3d(
            p_G_C(0) + dis(gen), p_G_C(1) + dis(gen), p_G_C(2) + dis(gen));

    Eigen::Quaterniond delta_q(1, dis_quat(gen), dis_quat(gen), dis_quat(gen));
    delta_q.normalize();
    T_G_C.getRotation().toImplementation() *= delta_q;
    T_G_C.getRotation().toImplementation().normalize();
    if (T_G_C.getRotation().toImplementation().w() < 0) {
      T_G_C.getRotation().toImplementation().coeffs() *= -1;
    }

    vi_map::Vertex& ba_vertex =
        posegraph_.getVertexPtrMutable(vertex_ids_[i])->getAs<vi_map::Vertex>();
    ba_vertex.set_p_M_I(vertex_position);
    ba_vertex.set_q_M_I(T_G_C.getRotation().toImplementation());
  }

  vertices_log.flushBuffer();
  landmarks_log.flushBuffer();
}

void ViwlsGraph::copyDataFromPosegraph() {
  pose_graph::VertexIdList all_vertices;
  posegraph_.getAllVertexIds(&all_vertices);

  keyframe_poses_.resize(Eigen::NoChange, all_vertices.size());

  unsigned int col_idx = 0;
  for (const pose_graph::VertexId& vertex_id : all_vertices) {
    const vi_map::Vertex* ba_vertex =
        dynamic_cast<const vi_map::Vertex*>(posegraph_.getVertexPtr(vertex_id));
    CHECK_NOTNULL(ba_vertex);

    vertex_id_to_pose_idx_.emplace(vertex_id, col_idx);

    pose::Transformation T_G_I = ba_vertex->get_T_M_I();

    keyframe_poses_.col(col_idx)
        << T_G_I.getRotation().toImplementation().coeffs(),
        T_G_I.getPosition();

    ++col_idx;
  }

  base_frame_ << q_G_B_, p_G_B_;
}

void ViwlsGraph::copyDataToPosegraph() {
  for (const VertexIdRotationMap::value_type& vertex_id_to_idx :
       vertex_id_to_pose_idx_) {
    vi_map::Vertex& ba_vertex =
        posegraph_.getVertexPtrMutable(vertex_id_to_idx.first)
            ->getAs<vi_map::Vertex>();

    pose::Transformation T_G_I;
    T_G_I.getPosition() = keyframe_poses_.col(vertex_id_to_idx.second).tail(3);
    T_G_I.getRotation().toImplementation().coeffs() =
        keyframe_poses_.col(vertex_id_to_idx.second).head(4);

    Eigen::Map<Eigen::Vector4d> rotation(ba_vertex.get_q_M_I_Mutable());
    Eigen::Map<Eigen::Vector3d> position(ba_vertex.get_p_M_I_Mutable());

    rotation = T_G_I.getRotation().toImplementation().coeffs();
    position = T_G_I.getPosition();
  }

  q_G_B_ = base_frame_.head(4);
  p_G_B_ = base_frame_.tail(3);
}

void ViwlsGraph::solve() {
  ceres::Problem problem;
  ceres::LocalParameterization* quaternion_parameterization =
      new ceres_error_terms::JplQuaternionParameterization;
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;

  pose_graph::VertexIdList all_vertices;
  posegraph_.getAllVertexIds(&all_vertices);
  CHECK(!all_vertices.empty()) << "No vertices on the posegraph";

  copyDataFromPosegraph();

  for (const pose_graph::VertexId& vertex_id : all_vertices) {
    vi_map::Vertex* ba_vertex = dynamic_cast<vi_map::Vertex*>(
        posegraph_.getVertexPtrMutable(vertex_id));
    CHECK_NOTNULL(ba_vertex);

    const Eigen::Matrix2Xd& image_points_distorted =
        ba_vertex->getVisualFrame(kVisualFrameIndex).getKeypointMeasurements();
    const Eigen::VectorXd& image_points_uncertainties =
        ba_vertex->getVisualFrame(kVisualFrameIndex)
            .getKeypointMeasurementUncertainties();

    const std::shared_ptr<CameraType> camera_ptr =
        std::dynamic_pointer_cast<CameraType>(
            ba_vertex->getCamera(kVisualFrameIndex));
    CHECK(camera_ptr != nullptr);

    // Retrieve keyframe pose idx.
    VertexIdRotationMap::const_iterator it;
    it = vertex_id_to_pose_idx_.find(vertex_id);
    CHECK(it != vertex_id_to_pose_idx_.end());
    int pose_idx = it->second;

    for (int i = 0; i < image_points_distorted.cols(); ++i) {
      vi_map::LandmarkId landmark_id =
          ba_vertex->getObservedLandmarkId(kVisualFrameIndex, i);

      if (landmark_observation_count_.find(landmark_id)->second >= 2) {
        ceres::CostFunction* visual_term_cost =
            new ceres_error_terms::VisualReprojectionError<CameraType,
                                                           DistortionType>(
                image_points_distorted.col(i), image_points_uncertainties(i),
                ceres_error_terms::visual::VisualErrorType::kLocalMission,
                camera_ptr.get());

        problem.AddResidualBlock(
            visual_term_cost, NULL,
            landmark_store_.find(landmark_id)->second->get_p_B_Mutable(),
            base_frame_.data(), dummy_7d_0_.data(), dummy_7d_1_.data(),
            keyframe_poses_.col(pose_idx).data(),
            cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
                .getRotation()
                .toImplementation()
                .coeffs()
                .data(),
            cameras_->get_T_C_B_Mutable(kVisualFrameIndex).getPosition().data(),
            camera_ptr->getParametersMutable(),
            camera_ptr->getDistortionMutable()->getParametersMutable());

        problem.SetParameterBlockConstant(
            landmark_store_.find(landmark_id)->second->get_p_B_Mutable());
      }
    }

    // We fix the dummy parameter blocks because they have no meaning.
    problem.SetParameterBlockConstant(dummy_7d_0_.data());
    problem.SetParameterBlockConstant(dummy_7d_1_.data());

    problem.SetParameterBlockConstant(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
            .getRotation()
            .toImplementation()
            .coeffs()
            .data());
    problem.SetParameterBlockConstant(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex).getPosition().data());
    problem.SetParameterBlockConstant(camera_ptr->getParametersMutable());
    problem.SetParameterBlockConstant(
        camera_ptr->getDistortionMutable()->getParametersMutable());

    problem.SetParameterization(
        keyframe_poses_.col(pose_idx).data(), pose_parameterization);
    problem.SetParameterization(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
            .getRotation()
            .toImplementation()
            .coeffs()
            .data(),
        quaternion_parameterization);
  }

  problem.SetParameterBlockConstant(base_frame_.data());
  problem.SetParameterization(base_frame_.data(), pose_parameterization);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 8;
  options.gradient_tolerance = 1e-30;
  options.function_tolerance = 1e-30;
  options.parameter_tolerance = 1e-30;
  options.num_threads = 8;
  options.num_linear_solver_threads = 8;
  options.check_gradients = false;
  ceres::Solver::Summary summary;

  ceres::Solve(options, &problem, &summary);

  LOG(INFO) << summary.message;
  LOG(INFO) << summary.BriefReport();

  copyDataToPosegraph();
}

TEST_F(ViwlsGraph, 4DofTrajectoryVisualBundleAdj) {
  constructCamera();
  constructTrajectoryGenerator();
  generatePathAndLandmarks();

  addPosegraphVertices();
  pose_graph::VertexIdList all_vertices;
  posegraph_.getAllVertexIds(&all_vertices);
  EXPECT_EQ(positions_.cols(), static_cast<int>(all_vertices.size()));

  solve();

  for (const pose_graph::VertexId& vertex_id : all_vertices) {
    const vi_map::Vertex* ba_vertex =
        dynamic_cast<const vi_map::Vertex*>(posegraph_.getVertexPtr(vertex_id));
    CHECK_NOTNULL(ba_vertex);

    AlignedUnorderedMap<pose_graph::VertexId, Eigen::Vector3d>::iterator it_pos;
    AlignedUnorderedMap<pose_graph::VertexId, Eigen::Quaterniond>::iterator
        it_rot;
    it_pos = true_vertex_positions_.find(vertex_id);
    it_rot = true_vertex_rotations_.find(vertex_id);

    EXPECT_NEAR_EIGEN(it_pos->second, ba_vertex->get_p_M_I(), 1e-10);
    EXPECT_NEAR_EIGEN_QUATERNION(it_rot->second, ba_vertex->get_q_M_I(), 1e-10);
  }
}

}  // namespace map_optimization_legacy

MAPLAB_UNITTEST_ENTRYPOINT


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

// ---
// Author: Zachary Kann

// A set of functions for calculation of electric field / frequency maps
// as outlined in the papers of Skinner et al. See, for example, S.M. Gruenbaum,
// et al. J. Chem. Theory Comput. 2013 9 (7), 3109. These maps are necessary
// for calculation of vibrational spectra using the methodology  outline in the
// aforementioned papers.

// Abbreviations:
//   IR = infrared
//   SFG = sum-frequency generation

#include <complex>
#include <cmath>
#include <armadillo>
#include "z_histogram.hpp"
#include "z_atom_group.hpp"
#include "z_molecule_group.hpp"
#include "z_cx_tcf.hpp"
#include "z_conversions.hpp"
#ifndef _Z_FREQUENCY_MAP_HPP_
#define _Z_FREQUENCY_MAP_HPP_

enum Chromophore {kOH, kOD};
enum ModelType {kThreeSite, kFourSite};

class FrequencyMap {
 public:
  inline FrequencyMap(const MoleculeGroup& water_group,
                      const Chromophore chromophore, const double timestep,
                      const int correlation_length = 200)
      : step_(0), correlation_length_(correlation_length),
        num_chromophores_(2*water_group.num_molecules()),
        chromophore_(chromophore), timestep_(timestep), frequency_bins_(4000),
        min_frequency_(chromophore == kOH ? 1650.0 : 850.0),
        max_frequency_(chromophore == kOH ? 4950.0 : 4150.0),
        frequency_distribution_(frequency_bins_, min_frequency_,
                                max_frequency_),
        spectral_density_(frequency_bins_, min_frequency_, max_frequency_),
        spectra_tcf_(correlation_length,timestep_, 1,
                    (4096+2)/2-correlation_length) {
    steps_guess_ = 1000;
    omega_01_ = arma::zeros<arma::mat>(num_chromophores_, steps_guess_);

    switch(chromophore) {
      case kOH:
        SetupOH();
        break;
      case kOD:
        SetupOD();
        break;
      default:
        assert(false && "Unrecognized chromophore option.");
        break;
    }
    switch(water_group.size()/water_group.num_molecules()) {
      case 3:
        model_type_ = kThreeSite;
        break;
      case 4:
        model_type_ = kFourSite;
        break;
      default:
        assert(false && "Number of atoms in water model is not one 3 or 4.");
        break;
    }
  }

  inline void CalculateFrequencies(const MoleculeGroup& hydrogen_group,
                                   const AtomGroup& oxygen_group,
                                   const arma::rowvec& box) {
    if(step_ == steps_guess_) {
      steps_guess_ += kStepsGuessIncrement_;
      ResizeArrays();
    }
    for (int i_chrom = 0; i_chrom < hydrogen_group.size(); ++i_chrom) {
      int i_oxygen = i_chrom/2;
      oh_vector_ =
          hydrogen_group.position(i_chrom) - oxygen_group.position(i_oxygen);
      oh_unit_vector_ = arma::normalise(oh_vector_);
      field_projection_ =
          arma::dot(oh_vector_, hydrogen_group.electric_field(i_chrom))*
          AU_TO_NM*AU_TO_NM;
      transition_dipole_position_ = oxygen_group.position(i_oxygen) +
                                    transition_dipole_shift_*oh_vector_;
      UseMapping(i_chrom, box);
      frequency_distribution_.Add(omega_01_(i_chrom));
      double spectral_density_weight = SpectralDensityWeight(i_chrom);
      spectral_density_.Add(omega_01_(i_chrom), spectral_density_weight);
    }
    step_++;
  }

  inline void CalculateSpectra() {
    // Two mean functions needed because of armadillo convention.
    avg_frequency_ = arma::mean(arma::mean(omega_01_));
    omega_01_ -= avg_frequency_;
    int num_corr = spectra_tcf_.CalculateNumCorr(step_);
    std::complex<double> exp_argument, exp_omega;
    for (int i_chrom = 0; i_chrom < num_chromophores_; ++i_chrom) {
      for (int i_num = 0; i_num < num_corr; ++i_num) {
        int corr_start = i_num*spectra_tcf_.interval();
        double omega_integral = 0.0;
        exp_argument = -1.0i*omega_integral;
        exp_omega = std::exp(exp_argument);
        double mu_or_alpha_product =
            MuOrAlphaProduct(i_chrom, corr_start, 0);
        spectra_tcf_.AddTCF(0,mu_or_alpha_product*exp_omega);
        for (int i_corr = 1; i_corr < correlation_length_; ++i_corr) {
          omega_integral += timestep_/2.0*TWO_PI_C *
                            (omega_01_(i_chrom,corr_start+i_corr)
                             +omega_01_(i_chrom,corr_start));
          exp_argument = -1.0i*omega_integral;
          exp_omega = std::exp(exp_argument);
          mu_or_alpha_product =
              MuOrAlphaProduct(i_chrom, corr_start, i_corr);
          spectra_tcf_.AddTCF(i_corr,mu_or_alpha_product*exp_omega);
        }
      }
    }
    for (int i_corr = 0; i_corr < correlation_length_; ++i_corr)
      spectra_tcf_.MultiplyTCF(std::exp(-i_corr*timestep_/2.0/lifetime_));
    spectra_tcf_.FourierPlus();
  }

  inline void PrintSpectra(const std::string& filename) {
    spectra_tcf_.PrintFTReal(filename, avg_frequency_);
  }

  //accessors
  inline int num_chromophores() const { return num_chromophores_; }

 protected:
  int step_;
  static const double mu_prime_0_ = 0.1646;
  static const double mu_prime_1_ = 11.39;
  static const double mu_prime_2_ = 63.41;
  int steps_guess_;
  static const int kStepsGuessIncrement_ = 1000;
  arma::mat omega_01_;
  arma::cube mu_01_;
  arma::rowvec transition_dipole_position_;
  arma::rowvec oh_unit_vector_;
  double field_projection_;
  double omega_01_0_;
  double omega_01_1_;
  double omega_01_2_;
  double chi_01_0_;
  double chi_01_1_;

  inline double SpectralDensityWeight(const int chromophore) {
    return MuOrAlphaProduct(chromophore, step_, 0);
  }

 private:
  double avg_frequency_;
  double lifetime_;
  int correlation_length_;
  double num_chromophores_;
  static const double transition_dipole_shift_ = 0.067; // Formerly dipPos
  Chromophore chromophore_;
  double timestep_;
  int frequency_bins_;
  double min_frequency_;
  double max_frequency_;
  ModelType model_type_;
  arma::mat alpha_;
  arma::mat switcher_;
  //arma::cube u; // needed for coupled spectra
  arma::rowvec oh_vector_;
  Histogram frequency_distribution_;
  Histogram spectral_density_;
  CxTCF spectra_tcf_;

  virtual void ResizeArrays() = 0;

  virtual double MuOrAlphaProduct(const int chromophore, const int corr_start,
                                  const int i_corr) = 0;

  inline void SetupOH() {
    lifetime_ = 0.700; //TODO(Zak): change this for coupled spectra
    omega_01_0_ = 3760.2;
    omega_01_1_ = -3541.7;
    omega_01_2_ = -152677.0;
    chi_01_0_ = 0.19285;
    chi_01_1_ = -1.7261E-5;
  }

  inline void SetupOD() {
    lifetime_ = 1.800; //TODO(Zak): change this for coupled spectra
    omega_01_0_ = 2767.8;
    omega_01_1_ = -2630.3;
    omega_01_2_ = -102601.0;
    chi_01_0_ = 0.16593;
    chi_01_1_ = -2.0632E-5;
  }

  // Calculates omega and mu/alpha using the frequency maps.
  virtual void UseMapping(const int chromophore,
                  const arma::rowvec& box) = 0;

  // TODO(Zak): Move this to a coupled spectra class
  // Calculates the intramolecular coupling between OH groups for inclusion
  // in the system Hamiltonian needed for coupled spectra calculations
  //extern double IntraCouple();

  // TODO(Zak): Move this to a coupled spectra class
  // Calculates the intermolecular coupling between OH groups for inclusion
  // in the system Hamiltonian needed for coupled spectra calculations
  //extern void InterCouple(arma::rowvec& omega, const arma::mat& xdipole,
  //                        const arma::rowvec& chi01, const arma::rowvec& muprime,
  //                        const arma::mat& u, const arma::rowvec& box,
  //                        const arma::icube& shift, const int numMols,
  //                        const int numChromos);
};
#endif


#pragma once

#include <Eigen/Core>
#include "point_extlib2.hpp"

//! [extlib2::PointAdapter]
namespace extlib2
{

struct PointAdapter {
  public:
    enum {Dim = 3};
    typedef float Scalar;
    typedef Eigen::Matrix<Scalar, Dim, 1> VectorType;

  private:
    Eigen::Map< const VectorType > m_pos, m_normal, m_color;

  public:
    inline PointAdapter(const extlib2::PointType2& p)
      : m_pos   (Eigen::Map< const VectorType >( p.posBuffer + Dim*p.id )),
        m_normal(Eigen::Map< const VectorType >( p.nBuffer + Dim*p.id )),
        m_color (Eigen::Map< const VectorType >( p.colorBuffer + Dim*p.id ))
    { }

    inline const Eigen::Map< const VectorType >& pos()    const { return m_pos; }
    inline const Eigen::Map< const VectorType >& normal() const { return m_normal; }
    inline const Eigen::Map< const VectorType >& color()  const { return m_color; }
    inline const Eigen::Map< const VectorType >& rgb()  const { return m_color; }

};

}
//! [extlib2::PointAdapter]


/*
 * This file is part of a C++ interface to the Radiative Transfer for Energetics (RTE)
 * and Rapid Radiative Transfer Model for GCM applications Parallel (RRTMGP).
 *
 * The original code is found at https://github.com/RobertPincus/rte-rrtmgp.
 *
 * Contacts: Robert Pincus and Eli Mlawer
 * email: rrtmgp@aer.com
 *
 * Copyright 2015-2020,  Atmospheric and Environmental Research and
 * Regents of the University of Colorado.  All right reserved.
 *
 * This C++ interface can be downloaded from https://github.com/Chiil/rrtmgp_cpp
 *
 * Contact: Chiel van Heerwaarden
 * email: chiel.vanheerwaarden@wur.nl
 *
 * Copyright 2020, Wageningen University & Research.
 *
 * Use and duplication is permitted under the terms of the
 * BSD 3-clause license, see http://opensource.org/licenses/BSD-3-Clause
 *
 */

#include <cmath>
#include <numeric>
#include <boost/algorithm/string.hpp>
// #include <xtensor/xarray.hpp>
// #include <xtensor/xadapt.hpp>
// #include "xtensor/xnoalias.hpp"

#include "Gas_concs.h"
#include "Gas_optics.h"
#include "Array.h"
#include "Optical_props.h"
#include "Source_functions.h"

#include "rrtmgp_kernels.h"
#define restrict __restrict__

namespace
{
    int find_index(
            const Array<std::string,1>& data, const std::string& value)
    {
        auto it = std::find(data.v().begin(), data.v().end(), value);
        if (it == data.v().end())
            return -1;
        else
            return it - data.v().begin() + 1;
    }

    template<typename TF>
    void reduce_minor_arrays(
                const Gas_concs<TF>& available_gases,
                const Array<std::string,1>& gas_names,
                const Array<std::string,1>& gas_minor,
                const Array<std::string,1>& identifier_minor,
                const Array<TF,3>& kminor_atm,
                const Array<std::string,1>& minor_gases_atm,
                const Array<int,2>& minor_limits_gpt_atm,
                const Array<int,1>& minor_scales_with_density_atm, // CvH: logical bool or int?
                const Array<std::string,1>& scaling_gas_atm,
                const Array<int,1>& scale_by_complement_atm, // CvH: bool or int
                const Array<int,1>& kminor_start_atm,

                Array<TF,3>& kminor_atm_red,
                Array<std::string,1>& minor_gases_atm_red,
                Array<int,2>& minor_limits_gpt_atm_red,
                Array<int,1>& minor_scales_with_density_atm_red, // CvH bool or int
                Array<std::string,1>& scaling_gas_atm_red,
                Array<int,1>& scale_by_complement_atm_red,
                Array<int,1>& kminor_start_atm_red)
    {
        int nm = minor_gases_atm.dim(1);
        int tot_g = 0;

        Array<int,1> gas_is_present({nm});

        for (int i=1; i<=nm; ++i)
        {
            const int idx_mnr = find_index(identifier_minor, minor_gases_atm({i}));

            // Search for
            std::string gas_minor_trimmed = gas_minor({idx_mnr});
            boost::trim(gas_minor_trimmed);

            gas_is_present({i}) = available_gases.exists(gas_minor_trimmed);
            if (gas_is_present({i}))
                tot_g += minor_limits_gpt_atm({2,i}) - minor_limits_gpt_atm({1,i}) + 1;
        }

        const int red_nm = std::accumulate(gas_is_present.v().begin(), gas_is_present.v().end(), 0);

        if (red_nm == nm)
        {
            kminor_atm_red = kminor_atm;
            minor_gases_atm_red = minor_gases_atm;
            minor_limits_gpt_atm_red = minor_limits_gpt_atm;
            minor_scales_with_density_atm_red = minor_scales_with_density_atm;
            scaling_gas_atm_red = scaling_gas_atm;
            scale_by_complement_atm_red = scale_by_complement_atm;
            kminor_start_atm_red = kminor_start_atm;
        }
        else
        {
            // Use a lambda function as the operation has to be repeated many times.
            auto resize_and_set = [&](auto& a_red, const auto& a)
            {
                a_red.set_dims({red_nm});
                int counter = 1;
                for (int i=1; i<=gas_is_present.dim(1); ++i)
                {
                    if (gas_is_present({i}))
                    {
                       a_red({counter}) = a({i});
                       ++counter;
                    }
                }
            };

            resize_and_set(minor_gases_atm_red, minor_gases_atm);
            resize_and_set(minor_scales_with_density_atm_red, minor_scales_with_density_atm);
            resize_and_set(scaling_gas_atm_red, scaling_gas_atm);
            resize_and_set(scale_by_complement_atm_red, scale_by_complement_atm);
            resize_and_set(kminor_start_atm_red, kminor_start_atm);

            minor_limits_gpt_atm_red.set_dims({2, red_nm});
            kminor_atm_red.set_dims({tot_g, kminor_atm.dim(2), kminor_atm.dim(3)});

            int icnt = 0;
            int n_elim = 0;
            for (int i=1; i<=nm; ++i)
            {
                int ng = minor_limits_gpt_atm({2,i}) - minor_limits_gpt_atm({1,i}) + 1;
                if (gas_is_present({i}))
                {
                    ++icnt;
                    minor_limits_gpt_atm_red({1,icnt}) = minor_limits_gpt_atm({1,i});
                    minor_limits_gpt_atm_red({2,icnt}) = minor_limits_gpt_atm({2,i});
                    kminor_start_atm_red({icnt}) = kminor_start_atm({i}) - n_elim;

                    for (int j=1; j<=ng; ++j)
                        for (int i2=1; i2<=kminor_atm.dim(2); ++i2)
                            for (int i3=1; i3<=kminor_atm.dim(3); ++i3)
                                kminor_atm_red({kminor_start_atm_red({icnt})+j-1,i2,i3}) =
                                        kminor_atm({kminor_start_atm({i})+j-1,i2,i3});
                }
                else
                    n_elim += ng;
            }
        }
    }

    void create_idx_minor(
            const Array<std::string,1>& gas_names,
            const Array<std::string,1>& gas_minor,
            const Array<std::string,1>& identifier_minor,
            const Array<std::string,1>& minor_gases_atm,
            Array<int,1>& idx_minor_atm)
    {
        Array<int,1> idx_minor_atm_out({minor_gases_atm.dim(1)});

        for (int imnr=1; imnr<=minor_gases_atm.dim(1); ++imnr)
        {
            // Find identifying string for minor species in list of possible identifiers (e.g. h2o_slf)
            const int idx_mnr = find_index(identifier_minor, minor_gases_atm({imnr}));

            // Find name of gas associated with minor species identifier (e.g. h2o)
            idx_minor_atm_out({imnr}) = find_index(gas_names, gas_minor({idx_mnr}));
        }

        idx_minor_atm = idx_minor_atm_out;
    }

    void create_idx_minor_scaling(
            const Array<std::string,1>& gas_names,
            const Array<std::string,1>& scaling_gas_atm,
            Array<int,1>& idx_minor_scaling_atm)
    {
        Array<int,1> idx_minor_scaling_atm_out({scaling_gas_atm.dim(1)});

        for (int imnr=1; imnr<=scaling_gas_atm.dim(1); ++imnr)
            idx_minor_scaling_atm_out({imnr}) = find_index(gas_names, scaling_gas_atm({imnr}));

        idx_minor_scaling_atm = idx_minor_scaling_atm_out;
    }

    void create_key_species_reduce(
            const Array<std::string,1>& gas_names,
            const Array<std::string,1>& gas_names_red,
            const Array<int,3>& key_species,
            Array<int,3>& key_species_red,
            Array<int,1>& key_species_present_init)
    {
        const int np = key_species.dim(1);
        const int na = key_species.dim(2);
        const int nt = key_species.dim(3);

        key_species_red.set_dims({key_species.dim(1), key_species.dim(2), key_species.dim(3)});
        key_species_present_init.set_dims({gas_names.dim(1)});

        for (int i=1; i<=key_species_present_init.dim(1); ++i)
            key_species_present_init({i}) = 1;

        for (int ip=1; ip<=np; ++ip)
            for (int ia=1; ia<=na; ++ia)
                for (int it=1; it<=nt; ++it)
                {
                    const int ks = key_species({ip,ia,it});
                    if (ks != 0)
                    {
                        const int ksr = find_index(gas_names_red, gas_names({ks}));
                        key_species_red({ip,ia,it}) = ksr;
                        if (ksr == -1)
                            key_species_present_init({ks}) = 0;
                    }
                    else
                        key_species_red({ip,ia,it}) = ks;
                }
    }

    void check_key_species_present_init(
            const Array<std::string,1>& gas_names,
            const Array<int,1>& key_species_present_init
            )
    {
        for (int i=1; i<=key_species_present_init.dim(1); ++i)
        {
            if (key_species_present_init({i}) == 0)
            {
                std::string error_message = "Gas optics: required gas " + gas_names({i}) + " is missing";
                throw std::runtime_error(error_message);
            }
        }
    }

    void create_flavor(
            const Array<int,3>& key_species,
            Array<int,2>& flavor)
    {
        Array<int,2> key_species_list({2, key_species.dim(3)*2});

        // Prepare list of key species.
        int i = 1;
        for (int ibnd=1; ibnd<=key_species.dim(3); ++ibnd)
            for (int iatm=1; iatm<=key_species.dim(1); ++iatm)
            {
                key_species_list({1,i}) = key_species({1,iatm,ibnd});
                key_species_list({2,i}) = key_species({2,iatm,ibnd});
                ++i;
            }

        // Rewrite single key_species pairs.
        for (int i=1; i<=key_species_list.dim(2); ++i)
        {
            if ( key_species_list({1,i}) == 0 && key_species_list({2,i}) == 0 )
            {
                key_species_list({1,i}) = 2;
                key_species_list({2,i}) = 2;
            }
        }

        // Count unique key species pairs.
        int iflavor = 0;
        for (int i=1; i<=key_species_list.dim(2); ++i)
        {
            bool pair_exists = false;
            for (int ii=1; ii<=i-1; ++ii)
            {
                if ( (key_species_list({1,i}) == key_species_list({1,ii})) &&
                     (key_species_list({2,i}) == key_species_list({2,ii})) )
                {
                    pair_exists = true;
                    break;
                }
            }
            if (!pair_exists)
                ++iflavor;
        }

        // Fill flavors.
        flavor.set_dims({2,iflavor});
        iflavor = 0;
        for (int i=1; i<=key_species_list.dim(2); ++i)
        {
            bool pair_exists = false;
            for (int ii=1; ii<=i-1; ++ii)
            {
                if ( (key_species_list({1,i}) == key_species_list({1,ii})) &&
                     (key_species_list({2,i}) == key_species_list({2,ii})) )
                {
                    pair_exists = true;
                    break;
                }
            }
            if (!pair_exists)
            {
                ++iflavor;
                flavor({1,iflavor}) = key_species_list({1,i});
                flavor({2,iflavor}) = key_species_list({2,i});
            }
        }
    }

    int key_species_pair2flavor(
            const Array<int,2>& flavor,
            const Array<int,1>& key_species_pair)
    {
        // Search for match.
        for (int iflav=1; iflav<=flavor.dim(2); ++iflav)
        {
            if ( key_species_pair({1}) == flavor({1, iflav}) &&
                 key_species_pair({2}) == flavor({2, iflav}) )
                return iflav;
        }

        // No match found.
        return -1;
    }

    void create_gpoint_flavor(
            const Array<int,3>& key_species,
            const Array<int,1>& gpt2band,
            const Array<int,2>& flavor,
            Array<int,2>& gpoint_flavor)
    {
        const int ngpt = gpt2band.dim(1);
        gpoint_flavor.set_dims({2,ngpt});

        for (int igpt=1; igpt<=ngpt; ++igpt)
            for (int iatm=1; iatm<=2; ++iatm)
            {
                int pair_1 = key_species( {1, iatm, gpt2band({igpt})} );
                int pair_2 = key_species( {2, iatm, gpt2band({igpt})} );

                // Rewrite species pair.
                Array<int,1> rewritten_pair({2});
                if (pair_1 == 0 && pair_2 == 0)
                {
                    rewritten_pair({1}) = 2;
                    rewritten_pair({2}) = 2;
                }
                else
                {
                    rewritten_pair({1}) = pair_1;
                    rewritten_pair({2}) = pair_2;
                }

                // Write the output.
                gpoint_flavor({iatm,igpt}) = key_species_pair2flavor(
                        flavor, rewritten_pair);
            }
    }

    /*
    template<typename TF>
    inline void reorder123x321_test(
            TF* restrict out, const TF* restrict in,
            const int d1, const int d2, const int d3)
    {
        const int jj_in = d1;
        const int kk_in = d1*d2;

        const int ii_out = d3*d2;
        const int jj_out = d3;

        for (int i=0; i<d1; ++i)
            for (int j=0; j<d2; ++j)
                #pragma GCC ivdep
                for (int k=0; k<d3; ++k)
                {
                    const int ijk_in  = i + j*jj_in  + k*kk_in ;
                    const int ijk_out = k + j*jj_out + i*ii_out;
                    out[ijk_out] = in[ijk_in];
                }
    }

    template<typename TF>
    inline void reorder123x321_test(
            TF* out, const TF* in,
            const size_t d1, const size_t d2, const size_t d3)
    {
        const size_t size = d1*d2*d3;
        const std::array<size_t, 3> in_shape = { d1, d2, d3 };
        const std::array<size_t, 3> out_shape = { d3, d2, d1 };

        const auto a_in = xt::adapt<xt::layout_type::column_major>(in, size, xt::no_ownership(), in_shape);
        auto a_out = xt::adapt<xt::layout_type::column_major>(out, size, xt::no_ownership(), out_shape);

        xt::noalias(a_out) = xt::transpose(a_out);
    }
    */
}

// IMPLEMENTATION OF CLASS FUNCTIONS.
// Constructor of longwave variant.
template<typename TF>
Gas_optics<TF>::Gas_optics(
        const Gas_concs<TF>& available_gases,
        const Array<std::string,1>& gas_names,
        const Array<int,3>& key_species,
        const Array<int,2>& band2gpt,
        const Array<TF,2>& band_lims_wavenum,
        const Array<TF,1>& press_ref,
        const TF press_ref_trop,
        const Array<TF,1>& temp_ref,
        const TF temp_ref_p,
        const TF temp_ref_t,
        const Array<TF,3>& vmr_ref,
        const Array<TF,4>& kmajor,
        const Array<TF,3>& kminor_lower,
        const Array<TF,3>& kminor_upper,
        const Array<std::string,1>& gas_minor,
        const Array<std::string,1>& identifier_minor,
        const Array<std::string,1>& minor_gases_lower,
        const Array<std::string,1>& minor_gases_upper,
        const Array<int,2>& minor_limits_gpt_lower,
        const Array<int,2>& minor_limits_gpt_upper,
        const Array<int,1>& minor_scales_with_density_lower,
        const Array<int,1>& minor_scales_with_density_upper,
        const Array<std::string,1>& scaling_gas_lower,
        const Array<std::string,1>& scaling_gas_upper,
        const Array<int,1>& scale_by_complement_lower,
        const Array<int,1>& scale_by_complement_upper,
        const Array<int,1>& kminor_start_lower,
        const Array<int,1>& kminor_start_upper,
        const Array<TF,2>& totplnk,
        const Array<TF,4>& planck_frac,
        const Array<TF,3>& rayl_lower,
        const Array<TF,3>& rayl_upper) :
            Optical_props<TF>(band_lims_wavenum, band2gpt),
            totplnk(totplnk),
            planck_frac(planck_frac)
{
    // Initialize the absorption coefficient array, including Rayleigh scattering
    // tables if provided.
    init_abs_coeffs(
            available_gases,
            gas_names, key_species,
            band2gpt, band_lims_wavenum,
            press_ref, temp_ref,
            press_ref_trop, temp_ref_p, temp_ref_t,
            vmr_ref,
            kmajor, kminor_lower, kminor_upper,
            gas_minor,identifier_minor,
            minor_gases_lower, minor_gases_upper,
            minor_limits_gpt_lower,
            minor_limits_gpt_upper,
            minor_scales_with_density_lower,
            minor_scales_with_density_upper,
            scaling_gas_lower, scaling_gas_upper,
            scale_by_complement_lower,
            scale_by_complement_upper,
            kminor_start_lower,
            kminor_start_upper,
            rayl_lower, rayl_upper);

    // Temperature steps for Planck function interpolation.
    // Assumes that temperature minimum and max are the same for the absorption coefficient grid and the
    // Planck grid and the Planck grid is equally spaced.
    totplnk_delta = (temp_ref_max - temp_ref_min) / (totplnk.dim(1)-1);
}

// Constructor of the shortwave variant.
template<typename TF>
Gas_optics<TF>::Gas_optics(
        const Gas_concs<TF>& available_gases,
        const Array<std::string,1>& gas_names,
        const Array<int,3>& key_species,
        const Array<int,2>& band2gpt,
        const Array<TF,2>& band_lims_wavenum,
        const Array<TF,1>& press_ref,
        const TF press_ref_trop,
        const Array<TF,1>& temp_ref,
        const TF temp_ref_p,
        const TF temp_ref_t,
        const Array<TF,3>& vmr_ref,
        const Array<TF,4>& kmajor,
        const Array<TF,3>& kminor_lower,
        const Array<TF,3>& kminor_upper,
        const Array<std::string,1>& gas_minor,
        const Array<std::string,1>& identifier_minor,
        const Array<std::string,1>& minor_gases_lower,
        const Array<std::string,1>& minor_gases_upper,
        const Array<int,2>& minor_limits_gpt_lower,
        const Array<int,2>& minor_limits_gpt_upper,
        const Array<int,1>& minor_scales_with_density_lower,
        const Array<int,1>& minor_scales_with_density_upper,
        const Array<std::string,1>& scaling_gas_lower,
        const Array<std::string,1>& scaling_gas_upper,
        const Array<int,1>& scale_by_complement_lower,
        const Array<int,1>& scale_by_complement_upper,
        const Array<int,1>& kminor_start_lower,
        const Array<int,1>& kminor_start_upper,
        const Array<TF,1>& solar_source_quiet,
        const Array<TF,1>& solar_source_facular,
        const Array<TF,1>& solar_source_sunspot,
        const TF tsi_default,
        const TF mg_default,
        const TF sb_default,
        const Array<TF,3>& rayl_lower,
        const Array<TF,3>& rayl_upper) :
            Optical_props<TF>(band_lims_wavenum, band2gpt)
{
    // Initialize the absorption coefficient array, including Rayleigh scattering
    // tables if provided.
    init_abs_coeffs(
            available_gases,
            gas_names, key_species,
            band2gpt, band_lims_wavenum,
            press_ref, temp_ref,
            press_ref_trop, temp_ref_p, temp_ref_t,
            vmr_ref,
            kmajor, kminor_lower, kminor_upper,
            gas_minor,identifier_minor,
            minor_gases_lower, minor_gases_upper,
            minor_limits_gpt_lower,
            minor_limits_gpt_upper,
            minor_scales_with_density_lower,
            minor_scales_with_density_upper,
            scaling_gas_lower, scaling_gas_upper,
            scale_by_complement_lower,
            scale_by_complement_upper,
            kminor_start_lower,
            kminor_start_upper,
            rayl_lower, rayl_upper);

    // Compute the solar source.
    this->solar_source_quiet = solar_source_quiet;
    this->solar_source_facular = solar_source_facular;
    this->solar_source_sunspot = solar_source_sunspot;

    this->solar_source.set_dims(solar_source_quiet.get_dims());

    set_solar_variability(mg_default, sb_default);
}

template<typename TF>
void Gas_optics<TF>::init_abs_coeffs(
        const Gas_concs<TF>& available_gases,
        const Array<std::string,1>& gas_names,
        const Array<int,3>& key_species,
        const Array<int,2>& band2gpt,
        const Array<TF,2>& band_lims_wavenum,
        const Array<TF,1>& press_ref,
        const Array<TF,1>& temp_ref,
        const TF press_ref_trop,
        const TF temp_ref_p,
        const TF temp_ref_t,
        const Array<TF,3>& vmr_ref,
        const Array<TF,4>& kmajor,
        const Array<TF,3>& kminor_lower,
        const Array<TF,3>& kminor_upper,
        const Array<std::string,1>& gas_minor,
        const Array<std::string,1>& identifier_minor,
        const Array<std::string,1>& minor_gases_lower,
        const Array<std::string,1>& minor_gases_upper,
        const Array<int,2>& minor_limits_gpt_lower,
        const Array<int,2>& minor_limits_gpt_upper,
        const Array<int,1>& minor_scales_with_density_lower,
        const Array<int,1>& minor_scales_with_density_upper,
        const Array<std::string,1>& scaling_gas_lower,
        const Array<std::string,1>& scaling_gas_upper,
        const Array<int,1>& scale_by_complement_lower,
        const Array<int,1>& scale_by_complement_upper,
        const Array<int,1>& kminor_start_lower,
        const Array<int,1>& kminor_start_upper,
        const Array<TF,3>& rayl_lower,
        const Array<TF,3>& rayl_upper)
{
    // Which gases known to the gas optics are present in the host model (available_gases)?
    std::vector<std::string> gas_names_to_use;

    for (const std::string &s : gas_names.v())
    {
        if (available_gases.exists(s))
            gas_names_to_use.push_back(s);
    }

    // Now the number of gases is the union of those known to the k-distribution and provided
    // by the host model.
    const int n_gas = gas_names_to_use.size();
    Array<std::string,1> gas_names_this(std::move(gas_names_to_use), {n_gas});
    this->gas_names = gas_names_this;

    // Initialize the gas optics object, keeping only those gases known to the
    // gas optics and also present in the host model.
    // Add an offset to the indexing to interface the negative ranging of fortran.
    Array<TF,3> vmr_ref_red({vmr_ref.dim(1), n_gas + 1, vmr_ref.dim(3)});
    vmr_ref_red.set_offsets({0, -1, 0});

    // Gas 0 is used in single-key species method, set to 1.0 (col_dry)
    for (int i1=1; i1<=vmr_ref_red.dim(1); ++i1)
        for (int i3=1; i3<=vmr_ref_red.dim(3); ++i3)
            vmr_ref_red({i1, 0, i3}) = vmr_ref({i1, 1, i3});

    for (int i=1; i<=n_gas; ++i)
    {
        int idx = find_index(gas_names, this->gas_names({i}));
        for (int i1=1; i1<=vmr_ref_red.dim(1); ++i1)
            for (int i3=1; i3<=vmr_ref_red.dim(3); ++i3)
                vmr_ref_red({i1, i, i3}) = vmr_ref({i1, idx+1, i3}); // CvH: why +1?
    }

    this->vmr_ref = std::move(vmr_ref_red);

    // Reduce minor arrays so variables only contain minor gases that are available.
    // Reduce size of minor Arrays.
    Array<std::string, 1> minor_gases_lower_red;
    Array<std::string, 1> scaling_gas_lower_red;
    Array<std::string, 1> minor_gases_upper_red;
    Array<std::string, 1> scaling_gas_upper_red;

    reduce_minor_arrays(
            available_gases,
            gas_names,
            gas_minor, identifier_minor,
            kminor_lower,
            minor_gases_lower,
            minor_limits_gpt_lower,
            minor_scales_with_density_lower,
            scaling_gas_lower,
            scale_by_complement_lower,
            kminor_start_lower,
            this->kminor_lower,
            minor_gases_lower_red,
            this->minor_limits_gpt_lower,
            this->minor_scales_with_density_lower,
            scaling_gas_lower_red,
            this->scale_by_complement_lower,
            this->kminor_start_lower);

    reduce_minor_arrays(
            available_gases,
            gas_names,
            gas_minor,
            identifier_minor,
            kminor_upper,
            minor_gases_upper,
            minor_limits_gpt_upper,
            minor_scales_with_density_upper,
            scaling_gas_upper,
            scale_by_complement_upper,
            kminor_start_upper,
            this->kminor_upper,
            minor_gases_upper_red,
            this->minor_limits_gpt_upper,
            this->minor_scales_with_density_upper,
            scaling_gas_upper_red,
            this->scale_by_complement_upper,
            this->kminor_start_upper);

    // Arrays not reduced by the presence, or lack thereof, of a gas
    this->press_ref = press_ref;
    this->temp_ref = temp_ref;
    this->kmajor = kmajor;

    // Create a new vector that consists of rayl_lower and rayl_upper stored in one variable.
    if (rayl_lower.size() > 0)
    {
        this->krayl.set_dims({rayl_lower.dim(1), rayl_lower.dim(2), rayl_lower.dim(3), 2});
        for (int i=0; i<rayl_lower.size(); ++i)
        {
            this->krayl.v()[i                    ] = rayl_lower.v()[i];
            this->krayl.v()[i + rayl_lower.size()] = rayl_upper.v()[i];
        }
    }

    // ---- post processing ----
    //  creates log reference pressure
    this->press_ref_log = this->press_ref;
    for (int i1=1; i1<=this->press_ref_log.dim(1); ++i1)
        this->press_ref_log({i1}) = std::log(this->press_ref_log({i1}));

    // log scale of reference pressure
    this->press_ref_trop_log = std::log(press_ref_trop);

    // Get index of gas (if present) for determining col_gas
    create_idx_minor(
            this->gas_names, gas_minor, identifier_minor, minor_gases_lower_red, this->idx_minor_lower);
    create_idx_minor(
            this->gas_names, gas_minor, identifier_minor, minor_gases_upper_red, this->idx_minor_upper);

    // Get index of gas (if present) that has special treatment in density scaling
    create_idx_minor_scaling(
            this->gas_names, scaling_gas_lower_red, this->idx_minor_scaling_lower);
    create_idx_minor_scaling(
            this->gas_names, scaling_gas_upper_red, this->idx_minor_scaling_upper);

    // Create flavor list.
    // Reduce (remap) key_species list; checks that all key gases are present in incoming
    Array<int, 3> key_species_red;
    Array<int, 1> key_species_present_init; // CvH bool or int?

    create_key_species_reduce(
            gas_names, this->gas_names, key_species, key_species_red, key_species_present_init);

    check_key_species_present_init(gas_names, key_species_present_init);

    // create flavor list
    create_flavor(key_species_red, this->flavor);

    // create gpoint flavor list
    create_gpoint_flavor(
            key_species_red, this->get_gpoint_bands(), this->flavor, this->gpoint_flavor);

    // minimum, maximum reference temperature, pressure -- assumes low-to-high ordering
    // for T, high-to-low ordering for p
    this->temp_ref_min = this->temp_ref({1});
    this->temp_ref_max = this->temp_ref({temp_ref.dim(1)});
    this->press_ref_min = this->press_ref({press_ref.dim(1)});
    this->press_ref_max = this->press_ref({1});

    // creates press_ref_log, temp_ref_delta
    this->press_ref_log_delta =
            (std::log(this->press_ref_min) - std::log(this->press_ref_max)) / (this->press_ref.dim(1) - 1);
    this->temp_ref_delta = (this->temp_ref_max - this->temp_ref_min) / (this->temp_ref.dim(1) - 1);

    // Which species are key in one or more bands?
    // this->flavor is an index into this->gas_names
    // if (allocated(this%is_key)) deallocate(this%is_key) ! Shouldn't ever happen...
    Array<int,1> is_key({get_ngas()}); // CvH bool, defaults to 0.?

    for (int j=1; j<=this->flavor.dim(2); ++j)
        for (int i=1; i<=this->flavor.dim(1); ++i)
        {
            if (this->flavor({i, j}) != 0)
                is_key({this->flavor({i, j})}) = true;
        }

    this->is_key = is_key;
}

template<typename TF>
void Gas_optics<TF>::set_solar_variability(
        const TF mg_index, const TF sb_index)
{
    constexpr TF a_offset = TF(0.1495954);
    constexpr TF b_offset = TF(0.00066696);

    for (int igpt=1; igpt<=this->solar_source_quiet.dim(1); ++igpt)
    {
        this->solar_source({igpt}) = this->solar_source_quiet({igpt})
                + (mg_index - a_offset) * this->solar_source_facular({igpt})
                + (sb_index - b_offset) * this->solar_source_sunspot({igpt});
    }

    // Scale solar source to input TSI value
    // if (present(tsi)) error_msg = this%set_tsi(tsi)
}

// Calculate the molecules of dry air.
template<typename TF>
void Gas_optics<TF>::get_col_dry(
        Array<TF,2>& col_dry, const Array<TF,2>& vmr_h2o,
        const Array<TF,2>& plev)
{
    // CvH: RRTMGP uses more accurate method based on latitude.
    constexpr TF g0 = 9.80665;

    constexpr TF avogad = 6.02214076e23;
    constexpr TF m_dry = 0.028964;
    constexpr TF m_h2o = 0.018016;

    Array<double,2> delta_plev({col_dry.dim(1), col_dry.dim(2)});
    Array<double,2> m_air     ({col_dry.dim(1), col_dry.dim(2)});

    for (int ilay=1; ilay<=col_dry.dim(2); ++ilay)
        for (int icol=1; icol<=col_dry.dim(1); ++icol)
            delta_plev({icol, ilay}) = std::abs(plev({icol, ilay}) - plev({icol, ilay+1}));

    for (int ilay=1; ilay<=col_dry.dim(2); ++ilay)
        for (int icol=1; icol<=col_dry.dim(1); ++icol)
            m_air({icol, ilay}) = (m_dry + m_h2o * vmr_h2o({icol, ilay})) / (1. + vmr_h2o({icol, ilay}));

    for (int ilay=1; ilay<=col_dry.dim(2); ++ilay)
        for (int icol=1; icol<=col_dry.dim(1); ++icol)
        {
            col_dry({icol, ilay}) = TF(10.) * delta_plev({icol, ilay}) * avogad / (TF(1000.)*m_air({icol, ilay})*TF(100.)*g0);
            col_dry({icol, ilay}) /= (TF(1.) + vmr_h2o({icol, ilay}));
        }
}

// Gas optics solver longwave variant.
template<typename TF>
void Gas_optics<TF>::gas_optics(
        const Array<TF,2>& play,
        const Array<TF,2>& plev,
        const Array<TF,2>& tlay,
        const Array<TF,1>& tsfc,
        const Gas_concs<TF>& gas_desc,
        std::unique_ptr<Optical_props_arry<TF>>& optical_props,
        Source_func_lw<TF>& sources,
        const Array<TF,2>& col_dry,
        const Array<TF,2>& tlev) const
{
    const int ncol = play.dim(1);
    const int nlay = play.dim(2);
    const int ngpt = this->get_ngpt();
    const int nband = this->get_nband();

    Array<int,2> jtemp({play.dim(1), play.dim(2)});
    Array<int,2> jpress({play.dim(1), play.dim(2)});
    Array<int,2> tropo({play.dim(1), play.dim(2)});
    Array<TF,6> fmajor({2, 2, 2, this->get_nflav(), play.dim(1), play.dim(2)});
    Array<int,4> jeta({2, this->get_nflav(), play.dim(1), play.dim(2)});

    // Gas optics.
    compute_gas_taus(
            ncol, nlay, ngpt, nband,
            play, plev, tlay, gas_desc,
            optical_props,
            jtemp, jpress, jeta, tropo, fmajor,
            col_dry);

    // External sources.
    source(
            ncol, nlay, nband, ngpt,
            play, plev, tlay, tsfc,
            jtemp, jpress, jeta, tropo, fmajor,
            sources, tlev);
}

// Gas optics solver shortwave variant.
template<typename TF>
void Gas_optics<TF>::gas_optics(
        const Array<TF,2>& play,
        const Array<TF,2>& plev,
        const Array<TF,2>& tlay,
        const Gas_concs<TF>& gas_desc,
        std::unique_ptr<Optical_props_arry<TF>>& optical_props,
        Array<TF,2>& toa_src,
        const Array<TF,2>& col_dry) const
{
    const int ncol = play.dim(1);
    const int nlay = play.dim(2);
    const int ngpt = this->get_ngpt();
    const int nband = this->get_nband();

    Array<int,2> jtemp({play.dim(1), play.dim(2)});
    Array<int,2> jpress({play.dim(1), play.dim(2)});
    Array<int,2> tropo({play.dim(1), play.dim(2)});
    Array<TF,6> fmajor({2, 2, 2, this->get_nflav(), play.dim(1), play.dim(2)});
    Array<int,4> jeta({2, this->get_nflav(), play.dim(1), play.dim(2)});

    // Gas optics.
    compute_gas_taus(
            ncol, nlay, ngpt, nband,
            play, plev, tlay, gas_desc,
            optical_props,
            jtemp, jpress, jeta, tropo, fmajor,
            col_dry);

    // External source function is constant.
    for (int igpt=1; igpt<=ngpt; ++igpt)
        for (int icol=1; icol<=ncol; ++icol)
            toa_src({icol, igpt}) = this->solar_source({igpt});
}

namespace rrtmgp_kernel_launcher
{
    template<typename TF> void zero_array(
            int ni, int nj, int nk, Array<TF,3>& array)
    {
        rrtmgp_kernels::zero_array_3D(&ni, &nj, &nk, array.ptr());
    }

    template<typename TF> void zero_array(
            int ni, int nj, int nk, int nl, Array<TF,4>& array)
    {
        rrtmgp_kernels::zero_array_4D(&ni, &nj, &nk, &nl, array.ptr());
    }

    template<typename TF>
    void interpolation(
            int ncol, int nlay,
            int ngas, int nflav, int neta, int npres, int ntemp,
            const Array<int,2>& flavor,
            const Array<TF,1>& press_ref_log,
            const Array<TF,1>& temp_ref,
            TF press_ref_log_delta,
            TF temp_ref_min,
            TF temp_ref_delta,
            TF press_ref_trop_log,
            const Array<TF,3>& vmr_ref,
            const Array<TF,2>& play,
            const Array<TF,2>& tlay,
            Array<TF,3>& col_gas,
            Array<int,2>& jtemp,
            Array<TF,6>& fmajor, Array<TF,5>& fminor,
            Array<TF,4>& col_mix,
            Array<int,2>& tropo,
            Array<int,4>& jeta,
            Array<int,2>& jpress)
    {
        rrtmgp_kernels::interpolation(
                &ncol, &nlay,
                &ngas, &nflav, &neta, &npres, &ntemp,
                const_cast<int*>(flavor.ptr()),
                const_cast<TF*>(press_ref_log.ptr()),
                const_cast<TF*>(temp_ref.ptr()),
                &press_ref_log_delta,
                &temp_ref_min,
                &temp_ref_delta,
                &press_ref_trop_log,
                const_cast<TF*>(vmr_ref.ptr()),
                const_cast<TF*>(play.ptr()),
                const_cast<TF*>(tlay.ptr()),
                col_gas.ptr(),
                jtemp.ptr(),
                fmajor.ptr(), fminor.ptr(),
                col_mix.ptr(),
                tropo.ptr(),
                jeta.ptr(),
                jpress.ptr());
    }

    template<typename TF>
    void compute_tau_absorption(
            int ncol, int nlay, int nband, int ngpt,
            int ngas, int nflav, int neta, int npres, int ntemp,
            int nminorlower, int nminorklower,
            int nminorupper, int nminorkupper,
            int idx_h2o,
            const Array<int,2>& gpoint_flavor,
            const Array<int,2>& band_lims_gpt,
            const Array<TF,4>& kmajor,
            const Array<TF,3>& kminor_lower,
            const Array<TF,3>& kminor_upper,
            const Array<int,2>& minor_limits_gpt_lower,
            const Array<int,2>& minor_limits_gpt_upper,
            const Array<int,1>& minor_scales_with_density_lower,
            const Array<int,1>& minor_scales_with_density_upper,
            const Array<int,1>& scale_by_complement_lower,
            const Array<int,1>& scale_by_complement_upper,
            const Array<int,1>& idx_minor_lower,
            const Array<int,1>& idx_minor_upper,
            const Array<int,1>& idx_minor_scaling_lower,
            const Array<int,1>& idx_minor_scaling_upper,
            const Array<int,1>& kminor_start_lower,
            const Array<int,1>& kminor_start_upper,
            Array<int,2>& tropo,
            Array<TF,4>& col_mix, Array<TF,6>& fmajor, Array<TF,5>& fminor,
            const Array<TF,2>& play, const Array<TF,2>& tlay, Array<TF,3>& col_gas,
            Array<int,4>& jeta, Array<int,2>& jtemp, Array<int,2>& jpress,
            Array<TF,3>& tau)
    {
        rrtmgp_kernels::compute_tau_absorption(
            &ncol, &nlay, &nband, &ngpt,
            &ngas, &nflav, &neta, &npres, &ntemp,
            &nminorlower, &nminorklower,
            &nminorupper, &nminorkupper,
            &idx_h2o,
            const_cast<int*>(gpoint_flavor.ptr()),
            const_cast<int*>(band_lims_gpt.ptr()),
            const_cast<TF*>(kmajor.ptr()),
            const_cast<TF*>(kminor_lower.ptr()),
            const_cast<TF*>(kminor_upper.ptr()),
            const_cast<int*>(minor_limits_gpt_lower.ptr()),
            const_cast<int*>(minor_limits_gpt_upper.ptr()),
            const_cast<int*>(minor_scales_with_density_lower.ptr()),
            const_cast<int*>(minor_scales_with_density_upper.ptr()),
            const_cast<int*>(scale_by_complement_lower.ptr()),
            const_cast<int*>(scale_by_complement_upper.ptr()),
            const_cast<int*>(idx_minor_lower.ptr()),
            const_cast<int*>(idx_minor_upper.ptr()),
            const_cast<int*>(idx_minor_scaling_lower.ptr()),
            const_cast<int*>(idx_minor_scaling_upper.ptr()),
            const_cast<int*>(kminor_start_lower.ptr()),
            const_cast<int*>(kminor_start_upper.ptr()),
            tropo.ptr(),
            col_mix.ptr(), fmajor.ptr(), fminor.ptr(),
            const_cast<TF*>(play.ptr()), const_cast<TF*>(tlay.ptr()), col_gas.ptr(),
            jeta.ptr(), jtemp.ptr(), jpress.ptr(),
            tau.ptr());
    }

    template<typename TF>
    void compute_tau_rayleigh(
            int ncol, int nlay, int nband, int ngpt,
            int ngas, int nflav, int neta, int npres, int ntemp,
            const Array<int,2>& gpoint_flavor,
            const Array<int,2>& band_lims_gpt,
            const Array<TF,4>& krayl,
            int idx_h2o, const Array<TF,2>& col_dry, const Array<TF,3>& col_gas,
            const Array<TF,5>& fminor, const Array<int,4>& jeta,
            const Array<int,2>& tropo, const Array<int,2>& jtemp,
            Array<TF,3>& tau_rayleigh)
    {
        rrtmgp_kernels::compute_tau_rayleigh(
                &ncol, &nlay, &nband, &ngpt,
                &ngas, &nflav, &neta, &npres, &ntemp,
                const_cast<int*>(gpoint_flavor.ptr()),
                const_cast<int*>(band_lims_gpt.ptr()),
                const_cast<TF*>(krayl.ptr()),
                &idx_h2o,
                const_cast<TF*>(col_dry.ptr()), const_cast<TF*>(col_gas.ptr()),
                const_cast<TF*>(fminor.ptr()), const_cast<int*>(jeta.ptr()),
                const_cast<int*>(tropo.ptr()), const_cast<int*>(jtemp.ptr()),
                tau_rayleigh.ptr());
    }

    template<typename TF>
    void reorder123x321(
            const Array<TF,3>& data,
            Array<TF,3>& data_out)
    {
        int dim1 = data.dim(1);
        int dim2 = data.dim(2);
        int dim3 = data.dim(3);
        rrtmgp_kernels::reorder_123x321_kernel(
                &dim1, &dim2, &dim3,
                const_cast<TF*>(data.ptr()),
                data_out.ptr());
    }

    template<typename TF>
    void combine_and_reorder_2str(
            int ncol, int nlay, int ngpt,
            const Array<TF,3>& tau_local, const Array<TF,3>& tau_rayleigh,
            Array<TF,3>& tau, Array<TF,3>& ssa, Array<TF,3>& g)
    {
        rrtmgp_kernels::combine_and_reorder_2str(
                &ncol, &nlay, &ngpt,
                const_cast<TF*>(tau_local.ptr()), const_cast<TF*>(tau_rayleigh.ptr()),
                tau.ptr(), ssa.ptr(), g.ptr());
    }

    template<typename TF>
    void compute_Planck_source(
            int ncol, int nlay, int nbnd, int ngpt,
            int nflav, int neta, int npres, int ntemp, int nPlanckTemp,
            const Array<TF,2>& tlay, const Array<TF,2>& tlev, const Array<TF,1>& tsfc, int sfc_lay,
            const Array<TF,6>& fmajor, const Array<int,4>& jeta, const Array<int,2>& tropo, const Array<int,2>& jtemp, const Array<int,2>& jpress,
            const Array<int,1>& gpoint_bands, const Array<int,2>& band_lims_gpt, const Array<TF,4>& pfracin, TF temp_ref_min,
            TF totplnk_delta, const Array<TF,2>& totplnk, const Array<int,2>& gpoint_flavor,
            Array<TF,2>& sfc_src, Array<TF,3>& lay_src, Array<TF,3>& lev_src_inc, Array<TF,3>& lev_src_dec,
            Array<TF,2>& sfc_src_jac)
    {
        rrtmgp_kernels::compute_Planck_source(
                &ncol, &nlay, &nbnd, &ngpt,
                &nflav, &neta, &npres, &ntemp, &nPlanckTemp,
                const_cast<TF*>(tlay.ptr()),
                const_cast<TF*>(tlev.ptr()),
                const_cast<TF*>(tsfc.ptr()),
                &sfc_lay,
                const_cast<TF*>(fmajor.ptr()),
                const_cast<int*>(jeta.ptr()),
                const_cast<int*>(tropo.ptr()),
                const_cast<int*>(jtemp.ptr()),
                const_cast<int*>(jpress.ptr()),
                const_cast<int*>(gpoint_bands.ptr()), const_cast<int*>(band_lims_gpt.ptr()), const_cast<TF*>(pfracin.ptr()), &temp_ref_min,
                &totplnk_delta, const_cast<TF*>(totplnk.ptr()), const_cast<int*>(gpoint_flavor.ptr()),
                sfc_src.ptr(), lay_src.ptr(), lev_src_inc.ptr(), lev_src_dec.ptr(),
                sfc_src_jac.ptr());
    }
}

template<typename TF>
void Gas_optics<TF>::compute_gas_taus(
        const int ncol, const int nlay, const int ngpt, const int nband,
        const Array<TF,2>& play,
        const Array<TF,2>& plev,
        const Array<TF,2>& tlay,
        const Gas_concs<TF>& gas_desc,
        std::unique_ptr<Optical_props_arry<TF>>& optical_props,
        Array<int,2>& jtemp, Array<int,2>& jpress,
        Array<int,4>& jeta,
        Array<int,2>& tropo,
        Array<TF,6>& fmajor,
        const Array<TF,2>& col_dry) const
{
    Array<TF,3> tau({ngpt, nlay, ncol});
    Array<TF,3> tau_rayleigh({ngpt, nlay, ncol});
    Array<TF,3> vmr({ncol, nlay, this->get_ngas()});
    Array<TF,3> col_gas({ncol, nlay, this->get_ngas()+1});
    col_gas.set_offsets({0, 0, -1});
    Array<TF,4> col_mix({2, this->get_nflav(), ncol, nlay});
    Array<TF,5> fminor({2, 2, this->get_nflav(), ncol, nlay});

    // CvH add all the checking...
    const int ngas = this->get_ngas();
    const int nflav = this->get_nflav();
    const int neta = this->get_neta();
    const int npres = this->get_npres();
    const int ntemp = this->get_ntemp();

    const int nminorlower = this->minor_scales_with_density_lower.dim(1);
    const int nminorklower = this->kminor_lower.dim(1);
    const int nminorupper = this->minor_scales_with_density_upper.dim(1);
    const int nminorkupper = this->kminor_upper.dim(1);

    for (int igas=1; igas<=ngas; ++igas)
    {
        const Array<TF,2>& vmr_2d = gas_desc.get_vmr(this->gas_names({igas}));

        // Fill array with constant value.
        if (vmr_2d.dim(1) == 1 && vmr_2d.dim(2) == 1)
        {
            const TF vmr_c = vmr_2d({1, 1});
            for (int ilay=1; ilay<=nlay; ++ilay)
                for (int icol=1; icol<=ncol; ++icol)
                    vmr({icol, ilay, igas}) = vmr_c;
        }
        // Fill array with constant profile.
        else if (vmr_2d.dim(1) == 1)
        {
            for (int ilay=1; ilay<=nlay; ++ilay)
            {
                const TF vmr_lay = vmr_2d({1, ilay});
                for (int icol=1; icol<=ncol; ++icol)
                    vmr({icol, ilay, igas}) = vmr_lay;
            }
        }
        // Fill array with full 2d data.
        else
        {
            for (int ilay=1; ilay<=nlay; ++ilay)
                for (int icol=1; icol<=ncol; ++icol)
                    vmr({icol, ilay, igas}) = vmr_2d({icol, ilay});
        }
    }

    // CvH: Assume that col_dry is provided.
    for (int ilay=1; ilay<=nlay; ++ilay)
        for (int icol=1; icol<=ncol; ++icol)
            col_gas({icol, ilay, 0}) = col_dry({icol, ilay});

    for (int igas=1; igas<=ngas; ++igas)
        for (int ilay=1; ilay<=nlay; ++ilay)
            for (int icol=1; icol<=ncol; ++icol)
                col_gas({icol, ilay, igas}) = vmr({icol, ilay, igas}) * col_dry({icol, ilay});

    // Call the fortran kernels
    rrtmgp_kernel_launcher::zero_array(ngpt, nlay, ncol, tau);

    rrtmgp_kernel_launcher::interpolation(
            ncol, nlay,
            ngas, nflav, neta, npres, ntemp,
            this->flavor,
            this->press_ref_log,
            this->temp_ref,
            this->press_ref_log_delta,
            this->temp_ref_min,
            this->temp_ref_delta,
            this->press_ref_trop_log,
            this->vmr_ref,
            play,
            tlay,
            col_gas,
            jtemp,
            fmajor, fminor,
            col_mix,
            tropo,
            jeta, jpress);

    int idx_h2o = -1;
    for (int i=1; i<=this->gas_names.dim(1); ++i)
        if (gas_names({i}) == "h2o")
        {
            idx_h2o = i;
            break;
        }

    if (idx_h2o == -1)
        throw std::runtime_error("idx_h2o cannot be found");

    rrtmgp_kernel_launcher::compute_tau_absorption(
            ncol, nlay, nband, ngpt,
            ngas, nflav, neta, npres, ntemp,
            nminorlower, nminorklower,
            nminorupper, nminorkupper,
            idx_h2o,
            this->gpoint_flavor,
            this->get_band_lims_gpoint(),
            this->kmajor,
            this->kminor_lower,
            this->kminor_upper,
            this->minor_limits_gpt_lower,
            this->minor_limits_gpt_upper,
            this->minor_scales_with_density_lower,
            this->minor_scales_with_density_upper,
            this->scale_by_complement_lower,
            this->scale_by_complement_upper,
            this->idx_minor_lower,
            this->idx_minor_upper,
            this->idx_minor_scaling_lower,
            this->idx_minor_scaling_upper,
            this->kminor_start_lower,
            this->kminor_start_upper,
            tropo,
            col_mix, fmajor, fminor,
            play, tlay, col_gas,
            jeta, jtemp, jpress,
            tau);

    bool has_rayleigh = (this->krayl.size() > 0);

    if (has_rayleigh)
    {
        rrtmgp_kernel_launcher::compute_tau_rayleigh(
                ncol, nlay, nband, ngpt,
                ngas, nflav, neta, npres, ntemp,
                this->gpoint_flavor,
                this->get_band_lims_gpoint(),
                this->krayl,
                idx_h2o, col_dry, col_gas,
                fminor, jeta, tropo, jtemp,
                tau_rayleigh);
    }

    combine_and_reorder(tau, tau_rayleigh, has_rayleigh, optical_props);
}

template<typename TF>
void Gas_optics<TF>::combine_and_reorder(
        const Array<TF,3>& tau,
        const Array<TF,3>& tau_rayleigh,
        const bool has_rayleigh,
        std::unique_ptr<Optical_props_arry<TF>>& optical_props) const
{
    int ncol = tau.dim(3);
    int nlay = tau.dim(2);
    int ngpt = tau.dim(1);

    if (!has_rayleigh)
    {
        // CvH for 2 stream and n-stream zero the g and ssa
        rrtmgp_kernel_launcher::reorder123x321(tau, optical_props->get_tau());
        // reorder123x321_test(optical_props->get_tau().ptr(), tau.ptr(), ngpt, nlay, ncol);

        // rrtmgp_kernel_launcher::zero_array(ngpt, nlay, ncol, optical_props->get_ssa());
        // rrtmgp_kernel_launcher::zero_array(ngpt, nlay, ncol, optical_props->get_g  ());
    }
    else
    {
        // In case of 1scl type
        // rrtmgp_kernel_launcher::reorder123x321(tau, optical_props->get_tau());

        // In case of 2str type
        rrtmgp_kernel_launcher::combine_and_reorder_2str(
                ncol, nlay, ngpt,
                tau, tau_rayleigh,
                optical_props->get_tau(), optical_props->get_ssa(), optical_props->get_g());
    }
}

template<typename TF>
void Gas_optics<TF>::source(
        const int ncol, const int nlay, const int nbnd, const int ngpt,
        const Array<TF,2>& play, const Array<TF,2>& plev,
        const Array<TF,2>& tlay, const Array<TF,1>& tsfc,
        const Array<int,2>& jtemp, const Array<int,2>& jpress,
        const Array<int,4>& jeta, const Array<int,2>& tropo,
        const Array<TF,6>& fmajor,
        Source_func_lw<TF>& sources,
        const Array<TF,2>& tlev) const
{
    // CvH Assume tlev is available.
    // Compute internal (Planck) source functions at layers and levels,
    // which depend on mapping from spectral space that creates k-distribution.
    const int nflav = this->get_nflav();
    const int neta = this->get_neta();
    const int npres = this->get_npres();
    const int ntemp = this->get_ntemp();
    const int nPlanckTemp = this->get_nPlanckTemp();
    auto gpoint_bands = this->get_gpoint_bands();
    auto band_lims_gpoint = this->get_band_lims_gpoint();

    Array<TF,3> lay_source_t({ngpt, nlay, ncol});
    Array<TF,3> lev_source_inc_t({ngpt, nlay, ncol});
    Array<TF,3> lev_source_dec_t({ngpt, nlay, ncol});
    Array<TF,2> sfc_source_t({ngpt, ncol});
    Array<TF,2> sfc_source_jac({ngpt, ncol});

    int sfc_lay = play({1, 1}) > play({1, nlay}) ? 1 : nlay;
    rrtmgp_kernel_launcher::compute_Planck_source(
            ncol, nlay, nbnd, ngpt,
            nflav, neta, npres, ntemp, nPlanckTemp,
            tlay, tlev, tsfc, sfc_lay,
            fmajor, jeta, tropo, jtemp, jpress,
            gpoint_bands, band_lims_gpoint, this->planck_frac, this->temp_ref_min,
            this->totplnk_delta, this->totplnk, this->gpoint_flavor,
            sfc_source_t, lay_source_t, lev_source_inc_t, lev_source_dec_t,
            sfc_source_jac);

    // CvH this transpose is super slow.
    for (int j=1; j<=sfc_source_t.dim(2); ++j)
        for (int i=1; i<=sfc_source_t.dim(1); ++i)
        {
            sources.get_sfc_source    ()({j, i}) = sfc_source_t  ({i, j});
            sources.get_sfc_source_jac()({j, i}) = sfc_source_jac({i, j});
        }

    rrtmgp_kernel_launcher::reorder123x321(lay_source_t, sources.get_lay_source());
    rrtmgp_kernel_launcher::reorder123x321(lev_source_inc_t, sources.get_lev_source_inc());
    rrtmgp_kernel_launcher::reorder123x321(lev_source_dec_t, sources.get_lev_source_dec());
    // reorder123x321_test(sources.get_lay_source    ().ptr(), lay_source_t    .ptr(), ngpt, nlay, ncol);
    // reorder123x321_test(sources.get_lev_source_inc().ptr(), lev_source_inc_t.ptr(), ngpt, nlay, ncol);
    // reorder123x321_test(sources.get_lev_source_dec().ptr(), lev_source_dec_t.ptr(), ngpt, nlay, ncol);
}

#ifdef FLOAT_SINGLE_RRTMGP
template class Gas_optics<float>;
#else
template class Gas_optics<double>;
#endif