#include <prc/range.hpp>

#include <chrono>

#include <algorithm>
#include <iostream>
#include <map>
#include <ostream>
#include <sstream>
#include <utility>

#include <boost/algorithm/string/join.hpp>

namespace prc
{
namespace
{
constexpr auto nb_total_combos = 1326;
constexpr auto minimum_weight = 0.001;

std::vector<range::weighted_elems> weights_to_weighted_elems(
    std::vector<double> const& weights)
{
  std::map<double, std::vector<combo>> m;
  auto const& any_two = any_two_combos();

  for (auto i = 0; i < nb_total_combos; ++i)
  {
    // do not include folded combos ever
    // it can be reconstituted by expanding combos of all subranges, then
    // set_difference between that and the parent combos
    if (weights[i] > minimum_weight)
      m[weights[i] * 100.0].push_back(any_two[i]);
  }

  std::vector<range::weighted_elems> ret;
  for (auto const& [weight, combos] : m)
    ret.push_back({weight, reduce_combos(combos)});
  return ret;
}

std::vector<range::weighted_elems> weights_to_weighted_elems(
    std::vector<double> const& weights,
    std::vector<double> const& parent_weights)
{
  std::vector<double> adjusted_weights;

  // pio weights are absolute, e.g. parent has 0.8, child has 0.7, it means 70%
  // of 100%, not 70% of 80%
  // equilab stores relative values and transform them in absolute ones when the
  // user selects a range to bring to the main window
  //
  // I do like equilab to allow easy replacing of parent ranges later on
  std::transform(weights.begin(),
                 weights.end(),
                 parent_weights.begin(),
                 std::back_inserter(adjusted_weights),
                 [](auto a, auto b) {
                   auto const margin = 1.0 - b;
                   return a + margin;
                 });
  return weights_to_weighted_elems(adjusted_weights);
}

using weight_by_combo_t = std::map<prc::combo, double>;

weight_by_combo_t weight_by_combos(
    std::vector<range::weighted_elems> const& elems)
{
  weight_by_combo_t ret;
  for (auto const& [weight, e] : elems)
  {
    for (auto const& combo : expand_combos(e))
    {
      auto& w = ret[combo];
      w += weight;
      if (w + minimum_weight >= 100.0)
        w = 100.0;
    }
  }
  return ret;
}

std::vector<range::weighted_elems> unassigned_combos_to_weighted_elems(
    weight_by_combo_t const& combos)
{
  std::map<double, std::vector<prc::combo>> m;
  std::vector<range::weighted_elems> ret;

  for (auto const& [combo, weight] : combos)
  {
    if (weight > minimum_weight)
      m[weight].push_back(combo);
  }
  for (auto const& [weight, combos] : m)
    ret.push_back(range::weighted_elems{weight, reduce_combos(combos)});
  return ret;
}

std::vector<range::weighted_elems> equilab_weighted_hands_to_weighted_elems(
    std::vector<equilab::parser::ast::weighted_hands> const& weighted_hands)
{
  std::vector<range::weighted_elems> ret;
  for (auto const& [weight, hands] : weighted_hands)
  {
    if (weight <= minimum_weight)
      continue;
    range::weighted_elems e{weight, {}};
    std::transform(
        hands.begin(), hands.end(), std::back_inserter(e.elems), [](auto& e) {
          return prc::range_elem{e};
        });
    std::sort(e.elems.begin(), e.elems.end());
    ret.push_back(std::move(e));
  }
  return ret;
}
template <typename Iterator>
Iterator order_equilab_groups(Iterator begin, Iterator end)
{
  std::sort(begin, end, [](auto& lhs, auto& rhs) {
    if (lhs.info.value().nesting_index != rhs.info.value().nesting_index)
      return lhs.info.value().nesting_index < rhs.info.value().nesting_index;
    return lhs.info.value().index < rhs.info.value().index;
  });
  std::stable_sort(begin, end, [](auto& lhs, auto& rhs) {
    return lhs.info.value().nesting_index > rhs.info.value().nesting_index;
  });
  return std::find_if(
      begin, end, [](auto& g) { return g.info.value().nesting_index == 0; });
}

template <typename Iterator, typename Sentinel>
std::vector<range> nest_equilab_ranges(Iterator begin, Sentinel end)
{
  // far from being optimized nor pretty, but heh
  auto const first_non_nested = order_equilab_groups(begin, end);

  std::map<int, int> index_to_pos;
  for (auto it = begin; it != end; ++it)
    index_to_pos[it->info->index] = std::distance(begin, it);

  std::vector<range> subranges;
  std::transform(begin, end, std::back_inserter(subranges), [](auto& g) {
    auto elems = equilab_weighted_hands_to_weighted_elems(g.weighted_hands);
    return range{g.info->name, std::move(elems), g.info->rgb};
  });
  for (auto it = begin; it != end; ++it)
  {
    if (it->info->nesting_index > 0)
    {
      auto const parent_pos = index_to_pos[it->info->parent_index];
      // copy, to be able to erase remove after
      subranges[parent_pos].add_subrange(
          subranges[index_to_pos[it->info->index]]);
    }
  }
  auto const first_non_nested_pos = std::distance(begin, first_non_nested);
  return std::vector<range>(
      std::make_move_iterator(subranges.begin() + first_non_nested_pos),
      std::make_move_iterator(subranges.end()));
}
}

range::range(std::string name,
             std::vector<weighted_elems> elems,
             int rgb,
             std::vector<range> subranges)
  : _name(std::move(name)),
    _elems(std::move(elems)),
    _rgb(rgb),
    _subranges(std::move(subranges))
{
}

range::range(equilab::parser::ast::range const& r) : _name(r.name), _rgb(0)
{
  if (r.groups.empty())
    throw std::runtime_error{"there must be at least one group in range"};
  auto& base_range = r.groups.front();
  _elems = equilab_weighted_hands_to_weighted_elems(base_range.weighted_hands);
  if (r.groups.size() > 1)
  {
    std::vector groups(r.groups.begin() + 1, r.groups.end());
    _subranges = nest_equilab_ranges(groups.begin(), groups.end());
  }
}

range::range(pio::parser::ast::range const& r) : _rgb(0)
{
  if (r.base_range.weights.size() != nb_total_combos)
    throw std::runtime_error{"base_range does not have 1326 weights"};
  _elems = weights_to_weighted_elems(r.base_range.weights);
  for (auto const& s : r.subranges)
  {
    if (s.weights.size() != nb_total_combos)
      throw std::runtime_error{"subrange does not have 1326 weights"};
    auto sub_elems = weights_to_weighted_elems(s.weights, r.base_range.weights);
    if (!sub_elems.empty())
      add_subrange({s.name, std::move(sub_elems), s.rgb});
  }
}

void range::add_subrange(range const& r)
{
  _subranges.push_back(r);
}

void range::add_subrange(range&& r)
{
  _subranges.push_back(std::move(r));
}

range* range::find_subrange(std::string const& name)
{
  auto it = std::find_if(_subranges.begin(), _subranges.end(), [&](auto& s) {
    return s.name() == name;
  });
  if (it == _subranges.end())
    return nullptr;
  return std::addressof(*it);
}

range const* range::find_subrange(std::string const& name) const
{
  return const_cast<range const*>(
      (*const_cast<range const*>(this)).find_subrange(name));
}

void range::set_rgb(int rgb)
{
  _rgb = rgb;
}

void range::set_name(std::string name)
{
  _name = std::move(name);
}

void range::set_elems(std::vector<weighted_elems> elems)
{
  _elems = std::move(elems);
}

std::string const& range::name() const
{
  return _name;
}

auto range::elems() const -> std::vector<weighted_elems> const&
{
  return _elems;
}

int range::rgb() const
{
  return _rgb;
}

std::vector<range> const& range::subranges() const
{
  return _subranges;
}

std::vector<range>& range::subranges()
{
  return _subranges;
}

bool operator==(range const& lhs, range const& rhs)
{
  return lhs.rgb() == rhs.rgb() &&
         std::tie(lhs.name(), lhs.elems(), lhs.subranges()) ==
             std::tie(rhs.name(), rhs.elems(), rhs.subranges());
}

bool operator!=(range const& lhs, range const& rhs)
{
  return !(lhs == rhs);
}

bool operator==(range::weighted_elems const& lhs,
                range::weighted_elems const& rhs)
{
  return std::tie(lhs.weight, lhs.elems) == std::tie(rhs.weight, rhs.elems);
}

bool operator!=(range::weighted_elems const& lhs,
                range::weighted_elems const& rhs)
{
  return !(lhs == rhs);
}

std::vector<range::weighted_elems> unassigned_elems(range const& r)
{
  if (r.subranges().empty())
    return {};
  auto current = weight_by_combos(r.elems());
  for (auto const& sub : r.subranges())
  {
    for (auto const& [combo, weight] : weight_by_combos(sub.elems()))
    {
      auto& w = current[combo];
      w -= weight;
      if (w <= minimum_weight)
        w = 0;
    }
  }

  return unassigned_combos_to_weighted_elems(current);
}

std::vector<range::weighted_elems> adjust_weights(
    std::vector<range::weighted_elems> const& base_range_elems, range const& r)
{
  auto const base_range_combos = weight_by_combos(base_range_elems);
  auto const range_combos = weight_by_combos(r.elems());

  std::map<double, std::vector<prc::combo>> tmp;
  for (auto const& [combo, weight] : range_combos)
  {
    if (auto it = base_range_combos.find(combo); it != base_range_combos.end())
      tmp[(it->second * weight) / 100.0].push_back(combo);
  }
  std::vector<range::weighted_elems> ret;
  for (auto const& [weight, combos] : tmp)
    ret.push_back(range::weighted_elems{weight, reduce_combos(combos)});
  return ret;
}

std::ostream& operator<<(std::ostream& os, range::weighted_elems const& elems)
{
  std::vector<std::string> strs;
  for (auto const& elem : elems.elems)
  {
    std::stringstream ss;
    ss << elem;
    strs.push_back(ss.str());
  }
  os << elems.weight << ":";
  os << boost::algorithm::join(strs, ",");
  return os;
}
}


#include <boost/hana/ext/boost/mpl/list.hpp>
#include <boost/hana/type.hpp>

<% list = Benchcc::MPL::List.new((0...x).map { |i| "x<#{i}>" }) %>
<%= list.includes %>

struct f {
    template <typename>
    struct apply { struct type; };
};

template <int> struct x;

int main() {
    auto go = boost::hana::fmap(
        boost::hana::metafunction_class<f>,
        <%= list %>{}
    );
}


/*******************************************************************************
*
*  Filename    : MakeErrHist.cc
*  Description : Implementatin of functions defined in include/MakeErrHist.hpp
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/CompareDataMC/interface/MakeErrHist.hpp"
#include "TstarAnalysis/CompareDataMC/interface/SampleErrHistMgr.hpp"

#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "TstarAnalysis/Common/interface/NameParse.hpp"
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/CompareDataMC/interface/Compare_Common.hpp"
#include "TstarAnalysis/CompareDataMC/interface/MakeHist.hpp"
#include "TstarAnalysis/CompareDataMC/interface/SampleHistMgr.hpp"

#include <boost/format.hpp>
#include <boost/range/adaptor/reversed.hpp>
using namespace std;
using namespace mgr;

void
MakeFullComparePlot(
  SampleErrHistMgr*          datamgr,
  vector<SampleErrHistMgr*>& background,
  SampleErrHistMgr*          signalmgr,
  const string&              label
  )
{
  SetBkgColor( background );

  for( const auto& histname : histnamelist ){

    // Declaring Histograms, for construction look below.
    THStack* stack = MakeBkgStack( background, histname );
    TH1D* bkgerror = MakeBkgError( background, histname );
    TH1D* datahist = (TH1D*)datamgr->Hist( histname )->Clone();
    TH1D* bkgrel   = MakeBkgRelHist( bkgerror );
    TH1D* datarel  = MakeDataRelHist( datahist, bkgerror );
    TH1D* sighist  = (TH1D*)signalmgr->Hist( histname )->Clone();

    cout << bkgerror->Integral() << " " << datahist->Integral() << endl;

    // Scaling signal plot for clarity
    const unsigned datanum = datamgr->ExpectedYield();
    const double signum    = signalmgr->ExpectedYield();
    const double sigscale  = datanum / signum / 2.;
    if( sighist->Integral() < datahist->Integral() /4.0 ){
      sighist->Scale( sigscale );
    }

    // Legend settings
    TLegend* l = mgr::NewLegend( 0.6, 0.5 );
    l->AddEntry( datahist, datamgr->RootName().c_str(), "pe" );

    for( const auto& entry : background ){
      l->AddEntry( entry->Hist( histname ), entry->RootName().c_str(), "f" );
    }

    l->AddEntry( bkgerror, "Unc. (stat #oplus sys)",                                                                "fl" );
    l->AddEntry( sighist,  boost::str( boost::format( "%s(#times%.0lf)" )%signalmgr->RootName()%sigscale ).c_str(), "fl" );

    MakePlot(
      stack,
      bkgerror,
      datahist,
      sighist,
      bkgrel,
      datarel,
      l,
      "fullcomp",
      {histname, label}
      );
    delete l;
    delete stack;
    delete bkgerror;
    delete datahist;
    delete bkgrel;
    delete datarel;
    delete sighist;
  }

}

/******************************************************************************/

extern void
Normalize(
  SampleErrHistMgr*               data,
  std::vector<SampleErrHistMgr*>& bg
  )
{
  const double datayield = data->ExpectedYield();
  double bgyield         = 0;

  for( const auto& sample : bg ){
    bgyield += sample->ExpectedYield();
  }

  const double scale = datayield / bgyield;

  for( auto& sample : bg ){
    sample->Scale( scale );
  }
}

/******************************************************************************/

TH1D*
MakeSumHistogram(
  const vector<SampleErrHistMgr*>& samplelist,
  const string&                    histname
  )
{
  const TH1D* temphist = samplelist.front()->Hist( histname );
  const unsigned bins  = temphist->GetXaxis()->GetNbins();
  const double xmin    = temphist->GetXaxis()->GetXmin();
  const double xmax    = temphist->GetXaxis()->GetXmax();

  const string newname = samplelist.front()->Name() + histname + "sum";
  const string title   = ";" + string( temphist->GetXaxis()->GetTitle() ) + ";" + string( temphist->GetYaxis()->GetTitle() );
  TH1D* sumhist        = new TH1D( newname.c_str(), title.c_str(), bins, xmin, xmax );

  for( const auto& sample : samplelist ){
    sumhist->Add( sample->Hist( histname ) );
  }

  sumhist->SetStats( 0 );
  return sumhist;
}

/******************************************************************************/

Parameter
ExpectedYield( const vector<SampleErrHistMgr*>& samplelist )
{
  Parameter ans;

  for( const auto& group : samplelist ){
    for( const auto& sample : group->SampleList() ){
      ans += sample.CrossSection()
             * sample.SelectionEfficiency()
             * sample.PDFUncertainty()
             * sample.QCDScaleUncertainty();
    }
  }

  return ans;
}

/******************************************************************************/

void
SetBkgColor( vector<SampleErrHistMgr*>& bkg )
{
  bkg[0]->SetColor( TColor::GetColor( "#FFCC88" ) );
  bkg[1]->SetColor( TColor::GetColor( "#996600" ) );
  bkg[2]->SetColor( TColor::GetColor( "#FF3333" ) );
  bkg[3]->SetColor( TColor::GetColor( "#33EEEE" ) );
  // bkg[4]->SetColor( TColor::GetColor( "#0066EE" ) );
}

/******************************************************************************/

THStack*
MakeBkgStack(
  const vector<SampleErrHistMgr*>& samplelist,
  const string&                    histname
  )
{
  THStack* stack = new THStack( ( histname+"bkgstack" ).c_str(), "" );

  for( const auto& sample : boost::adaptors::reverse( samplelist ) ){
    stack->Add( sample->Hist( histname ), "HIST" );
  }

  return stack;
}

/******************************************************************************/

TH1D*
MakeBkgError(
  const vector<SampleErrHistMgr*>& samplelist,
  const string&                    histname
  )
{
  TH1D* central = MakeSumHistogram( samplelist, histname );
  vector<pair<TH1D*, TH1D*> > errorhistlist;

  for( const auto& err : histerrlist ){

    // rescaling object for PDF and QCD scale errors
    if( err.tag == "pdf" || err.tag == "scale " ){
      for( const auto sample : samplelist ){
        TH1D* central  = sample->Hist( histname );
        TH1D* uphist   = sample->Hist( histname + err.tag + "up" );
        TH1D* downhist = sample->Hist( histname + err.tag + "down" );
        uphist->Scale( central->Integral()/uphist->Integral() );
        downhist->Scale( central->Integral()/downhist->Integral() );
      }
    }

    // Creating the summed results.
    errorhistlist.emplace_back(
      MakeSumHistogram( samplelist, histname + err.tag + "up" ),
      MakeSumHistogram( samplelist, histname + err.tag + "down" )
      );
  }

  const Parameter expyield = ExpectedYield( samplelist );
  const Parameter normerr  = Parameter(
    1,
    expyield.RelUpperError(),
    expyield.RelLowerError()
    );

  for( int i = 1; i <= central->GetSize(); ++i ){
    const double bincont = central->GetBinContent( i );
    const double binerr  = central->GetBinError( i );

    Parameter errtot( 1, binerr/bincont, binerr/bincont );

    for( const auto pair : errorhistlist ){
      const double upbincont   = pair.first->GetBinContent( i );
      const double downbincont = pair.second->GetBinContent( i );
      const double upabserr    = std::max( 0.0, upbincont-bincont );
      const double downabserr  = std::max( 0.0, bincont-downbincont );
      errtot *= Parameter( 1, upabserr/bincont, downabserr/bincont );
    }

    // Additional errors
    errtot *= ( normerr );// cross section and selection eff
    errtot *= compnamer.MasterConfig().GetStaticParameter( "Lumi Error" );// lumierror
    errtot *= compnamer.GetChannel().find( "Electron" ) == string::npos ?  // leptonic systematic error
              compnamer.MasterConfig().GetStaticParameter( "Muon Systematic" ) :
              compnamer.MasterConfig().GetStaticParameter( "Electron Systematic" );

    if( bincont == 0 ){
      central->SetBinError( i, 0 );
    } else if( errtot.RelAvgError() >= 0.6 ){
      central->SetBinError( i, bincont * 0.6 );
    } else {
      central->SetBinError( i, bincont * errtot.RelAvgError() );
    }

  }

  for( auto& pair : errorhistlist ){
    delete pair.first;
    delete pair.second;
  }

  return central;
}

/******************************************************************************/

void
PlotErrCompare(
  const vector<SampleErrHistMgr*>& samplelist,
  const string&                    histname,
  const ErrorSource&               err
  )
{
  TH1D* central = MakeSumHistogram( samplelist, histname );
  TH1D* errup   = MakeSumHistogram( samplelist, histname + err.tag + "up" );
  TH1D* errdown = MakeSumHistogram( samplelist, histname + err.tag + "down" );

  // Normailizing plots if err is the pdf error or scale errors
  if( err.tag == "scale" || err.tag == "pdf" ){
    errup->Scale( central->Integral() / errup->Integral() );
    errdown->Scale( central->Integral() / errdown->Integral() );
  }
  // Making duplicate objects
  TH1* uprel   = mgr::DivideHist( errup, central );
  TH1* downrel = mgr::DivideHist( errdown, central );

  const double xmin = central->GetXaxis()->GetXmin();
  const double xmax = central->GetXaxis()->GetXmax();
  // Setting plot range
  const double ymax     = mgr::GetYmax( central, errup, errdown );
  const double relymax  = std::min( 2.0, std::max( uprel->GetMaximum(), downrel->GetMaximum() ) );
  const double bpaddist = std::ceil( ( relymax-1 )*10 )/10.;

  // Plotting
  TCanvas* c = mgr::NewCanvas();

  // TOPPAD
  TPad* pad1 = mgr::NewTopPad();
  pad1->Draw();
  pad1->cd();
  central->Draw( PS_AXIS );
  errup->Draw( PS_HIST PS_SAME );
  errdown->Draw( PS_HIST PS_SAME );
  central->Draw( PS_HIST PS_SAME );
  c->cd();

  // Bottom pad
  TPad* pad2      = mgr::NewBottomPad();
  TLine* line     = new TLine( xmin, 1, xmax, 1 );
  TLine* line_top = new TLine( xmin, 1+bpaddist, xmax, 1+bpaddist );
  TLine* line_bot = new TLine( xmin, 1-bpaddist, xmax, 1-bpaddist );
  pad2->Draw();
  pad2->cd();

  uprel->Draw( PS_AXIS );
  uprel->Draw( PS_HIST PS_SAME );
  downrel->Draw( PS_HIST PS_SAME );
  line->Draw( PS_SAME );
  line_top->Draw( PS_SAME );
  line_bot->Draw( PS_SAME );
  c->cd();

  // Drawing legend
  const double legxmin = 0.5;
  const double legymin = 0.7;
  TLegend* tl          = mgr::NewLegend( legxmin, legymin );
  const string label   = ( err.label != "" ) ? err.label : "1#sigma";
  tl->AddEntry( central, "Central Value",                               "l" );
  tl->AddEntry( errup,   ( err.rootname + "(+" + label + ")" ).c_str(), "l" );
  tl->AddEntry( errdown, ( err.rootname + "(-" + label + ")" ).c_str(), "l" );

  tl->Draw( PS_SAME );

  // Common styling
  central->SetLineColor( kBlack );
  errup->SetLineColor( KRED );
  errdown->SetLineColor( KBLUE );
  uprel->SetLineColor( KRED );
  downrel->SetLineColor( KBLUE );

  // accessory styling
  line->SetLineColor( kBlack );
  line->SetLineStyle( 1 );
  line->SetLineWidth( 2 );
  line_top->SetLineColor( kBlack );
  line_bot->SetLineColor( kBlack );
  line_top->SetLineStyle( 3 );
  line_bot->SetLineStyle( 3 );

  mgr::SetTopPlotAxis( central );
  mgr::SetBottomPlotAxis( uprel );

  // Setting plot range
  central->SetMaximum( ymax * 1.5 );
  uprel->SetMaximum( 1+( bpaddist+0.05 ) );
  uprel->SetMinimum( 1-( bpaddist+0.05 ) );
  uprel->GetYaxis()->SetTitle( "#frac{up,down}{Norm}" );

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + ( TEXT_MARGIN/2 ), BOTTOM_LEFT )
  .WriteLine( compnamer.GetChannelEXT( "Root Name" ) );

  latex.SetOrigin( PLOT_X_TEXT_MAX, legymin - 0.02, TOP_RIGHT );
  if( compnamer.GetInput<string>( "group" ).find( "Tstar" ) != string::npos ){
    latex.WriteLine( boost::str( boost::format( "M_{t*}=%dGeV/c^{2}" )%GetInt( compnamer.GetInput<string>( "group" ) ) ) );
  } else {
    latex.WriteLine( compnamer.GetInput<string>( "group" ) );
  }

  // cleaning up
  mgr::DrawCMSLabel( SIMULATION );
  mgr::DrawLuminosity( mgr::SampleMgr::TotalLuminosity() );

  mgr::SaveToPDF( c, compnamer.PlotFileName( "errcomp", histname, err.tag ) );
  mgr::SaveToROOT(
    c,
    compnamer.PlotRootFile(),
    compnamer.OptFileName( "", "errcomp", histname, err.tag )
    );

  delete tl;
  delete uprel;
  delete downrel;
  delete central;
  delete errup;
  delete errdown;
  delete c;
}


// Velodyne HDL Packet Bundle Decoder
// Nick Rypkema (rypkema@mit.edu), MIT 2017
// shared library to decode a bundle of velodyne packets

#include <cmath>
#include <stdint.h>
#include <iostream>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>

#include "PacketBundleDecoder.h"

PacketBundleDecoder::PacketBundleDecoder()
{
  _max_num_of_frames = 10;
  UnloadData();
  InitTables();
  LoadHDL32Corrections();
}

PacketBundleDecoder::~PacketBundleDecoder()
{

}

void PacketBundleDecoder::SetMaxNumberOfFrames(unsigned int max_num_of_frames)
{

  if (max_num_of_frames <= 0) {
    return;
  } else {
    _max_num_of_frames = max_num_of_frames;
  }
  while (_frames.size() >= _max_num_of_frames) {
    _frames.pop_front();
  }
}

void PacketBundleDecoder::DecodeBundle(std::string* bundle, unsigned int* bundle_length)
{
  unsigned int num_packets = *bundle_length/1206;

  for (int i = 0; i < num_packets; i++) {
    std::string data = bundle->substr(i*1206, 1206);
    const unsigned char* data_char = reinterpret_cast<const unsigned char*>(data.c_str());
    ProcessHDLPacket(const_cast<unsigned char*>(data_char), 1206);
  }

  if (_frames.size() == _max_num_of_frames-1) {
    _frames.pop_front();
  }
  _frames.push_back(*_frame);
  delete _frame;
  _frame = new HDLFrame();
}

void PacketBundleDecoder::ProcessHDLPacket(unsigned char *data, unsigned int data_length)
{
  if (data_length != 1206) {
    std::cout << "PacketDecoder: Warning, data packet is not 1206 bytes" << std::endl;
    return;
  }

  HDLDataPacket* dataPacket = reinterpret_cast<HDLDataPacket *>(data);

  for (int i = 0; i < HDL_FIRING_PER_PKT; ++i) {
    HDLFiringData firingData = dataPacket->firingData[i];
    int offset = (firingData.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32;

    for (int j = 0; j < HDL_LASER_PER_FIRING; j++) {
      unsigned char laserId = static_cast<unsigned char>(j + offset);
      if (firingData.laserReturns[j].distance != 0.0) {
        PushFiringData(laserId, firingData.rotationalPosition, dataPacket->gpsTimestamp, firingData.laserReturns[j], laser_corrections_[j + offset]);
      }
    }
  }
}

void PacketBundleDecoder::PushFiringData(unsigned char laserId, unsigned short azimuth, unsigned int timestamp, HDLLaserReturn laserReturn, HDLLaserCorrection correction)
{
  double cosAzimuth, sinAzimuth;
  if (correction.azimuthCorrection == 0) {
    cosAzimuth = cos_lookup_table_[azimuth];
    sinAzimuth = sin_lookup_table_[azimuth];
  } else {
    double azimuthInRadians = HDL_Grabber_toRadians((static_cast<double> (azimuth) / 100.0) - correction.azimuthCorrection);
    cosAzimuth = std::cos(azimuthInRadians);
    sinAzimuth = std::sin(azimuthInRadians);
  }

  double distanceM = laserReturn.distance * 0.002 + correction.distanceCorrection;
  double xyDistance = distanceM * correction.cosVertCorrection - correction.sinVertOffsetCorrection;

  double x = (xyDistance * sinAzimuth - correction.horizontalOffsetCorrection * cosAzimuth);
  double y = (xyDistance * cosAzimuth + correction.horizontalOffsetCorrection * sinAzimuth);
  double z = (distanceM * correction.sinVertCorrection + correction.cosVertOffsetCorrection);
  unsigned char intensity = laserReturn.intensity;

  _frame->x.push_back(x);
  _frame->y.push_back(y);
  _frame->z.push_back(z);
  _frame->intensity.push_back(intensity);
  _frame->laser_id.push_back(laserId);
  _frame->azimuth.push_back(azimuth);
  _frame->distance.push_back(distanceM);
  _frame->ms_from_top_of_hour.push_back(timestamp);
}

void PacketBundleDecoder::SetCorrectionsFile(const std::string& corrections_file)
{
  if (corrections_file == _corrections_file) {
    return;
  }

  if (corrections_file.length()) {
    LoadCorrectionsFile(corrections_file);
  } else {
    LoadHDL32Corrections();
  }

  _corrections_file = corrections_file;
  UnloadData();
}

void PacketBundleDecoder::UnloadData()
{
  _frame = new HDLFrame();
  _frames.clear();
}

void PacketBundleDecoder::InitTables()
{
  if (cos_lookup_table_ == NULL && sin_lookup_table_ == NULL) {
    cos_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*cos_lookup_table_)));
    sin_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*sin_lookup_table_)));
    for (unsigned int i = 0; i < HDL_NUM_ROT_ANGLES; i++) {
      double rad = HDL_Grabber_toRadians(i / 100.0);
      cos_lookup_table_[i] = std::cos(rad);
      sin_lookup_table_[i] = std::sin(rad);
    }
  }
}

void PacketBundleDecoder::LoadCorrectionsFile(const std::string& correctionsFile)
{

  boost::property_tree::ptree pt;
  try {
    read_xml(correctionsFile, pt, boost::property_tree::xml_parser::trim_whitespace);
  } catch (boost::exception const&) {
    std::cout << "PacketDecoder: Error reading calibration file - " << correctionsFile << std::endl;
    return;
  }

  BOOST_FOREACH (boost::property_tree::ptree::value_type &v, pt.get_child("boost_serialization.DB.points_")) {
    if (v.first == "item") {
      boost::property_tree::ptree points = v.second;
      BOOST_FOREACH (boost::property_tree::ptree::value_type &px, points) {
        if (px.first == "px") {
          boost::property_tree::ptree calibrationData = px.second;
          int index = -1;
          double azimuth = 0;
          double vertCorrection = 0;
          double distCorrection = 0;
          double vertOffsetCorrection = 0;
          double horizOffsetCorrection = 0;

          BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibrationData) {
            if (item.first == "id_")
              index = atoi(item.second.data().c_str());
            if (item.first == "rotCorrection_")
              azimuth = atof(item.second.data().c_str());
            if (item.first == "vertCorrection_")
              vertCorrection = atof(item.second.data().c_str());
            if (item.first == "distCorrection_")
              distCorrection = atof(item.second.data().c_str());
            if (item.first == "vertOffsetCorrection_")
              vertOffsetCorrection = atof(item.second.data().c_str());
            if (item.first == "horizOffsetCorrection_")
              horizOffsetCorrection = atof(item.second.data().c_str());
          }
          if (index != -1) {
            laser_corrections_[index].azimuthCorrection = azimuth;
            laser_corrections_[index].verticalCorrection = vertCorrection;
            laser_corrections_[index].distanceCorrection = distCorrection / 100.0;
            laser_corrections_[index].verticalOffsetCorrection = vertOffsetCorrection / 100.0;
            laser_corrections_[index].horizontalOffsetCorrection = horizOffsetCorrection / 100.0;

            laser_corrections_[index].cosVertCorrection = std::cos (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
            laser_corrections_[index].sinVertCorrection = std::sin (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
          }
        }
      }
    }
  }

  SetCorrectionsCommon();
}

void PacketBundleDecoder::LoadHDL32Corrections()
{
  double hdl32VerticalCorrections[] = {
    -30.67, -9.3299999, -29.33, -8, -28,
    -6.6700001, -26.67, -5.3299999, -25.33, -4, -24, -2.6700001, -22.67,
    -1.33, -21.33, 0, -20, 1.33, -18.67, 2.6700001, -17.33, 4, -16, 5.3299999,
    -14.67, 6.6700001, -13.33, 8, -12, 9.3299999, -10.67, 10.67 };

  for (int i = 0; i < HDL_LASER_PER_FIRING; i++) {
    laser_corrections_[i].azimuthCorrection = 0.0;
    laser_corrections_[i].distanceCorrection = 0.0;
    laser_corrections_[i].horizontalOffsetCorrection = 0.0;
    laser_corrections_[i].verticalOffsetCorrection = 0.0;
    laser_corrections_[i].verticalCorrection = hdl32VerticalCorrections[i];
    laser_corrections_[i].sinVertCorrection = std::sin(HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
    laser_corrections_[i].cosVertCorrection = std::cos(HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
  }

  for (int i = HDL_LASER_PER_FIRING; i < HDL_MAX_NUM_LASERS; i++) {
    laser_corrections_[i].azimuthCorrection = 0.0;
    laser_corrections_[i].distanceCorrection = 0.0;
    laser_corrections_[i].horizontalOffsetCorrection = 0.0;
    laser_corrections_[i].verticalOffsetCorrection = 0.0;
    laser_corrections_[i].verticalCorrection = 0.0;
    laser_corrections_[i].sinVertCorrection = 0.0;
    laser_corrections_[i].cosVertCorrection = 1.0;
  }

  SetCorrectionsCommon();
}

void PacketBundleDecoder::SetCorrectionsCommon()
{
  for (int i = 0; i < HDL_MAX_NUM_LASERS; i++) {
    HDLLaserCorrection correction = laser_corrections_[i];
    laser_corrections_[i].sinVertOffsetCorrection = correction.verticalOffsetCorrection
                                       * correction.sinVertCorrection;
    laser_corrections_[i].cosVertOffsetCorrection = correction.verticalOffsetCorrection
                                       * correction.cosVertCorrection;
  }
}

std::deque<PacketBundleDecoder::HDLFrame> PacketBundleDecoder::GetFrames()
{
  return _frames;
}

void PacketBundleDecoder::ClearFrames()
{
  _frames.clear();
}

bool PacketBundleDecoder::GetLatestFrame(PacketBundleDecoder::HDLFrame* frame)
{
  if (_frames.size()) {
    *frame = _frames.back();
    _frames.clear();
    return(true);
  }
  return(false);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Evolution/Systems/NewtonianEuler/Limiters/Weno.hpp"

#include <array>
#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <optional>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags/TempTensor.hpp"
#include "DataStructures/Tensor/EagerMath/DotProduct.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/SizeOfElement.hpp"
#include "Domain/Structure/Direction.hpp"  // IWYU pragma: keep
#include "Domain/Structure/Element.hpp"    // IWYU pragma: keep
#include "Domain/Structure/ElementId.hpp"  // IWYU pragma: keep
#include "Domain/Tags.hpp"                 // IWYU pragma: keep
#include "Evolution/DiscontinuousGalerkin/Limiters/HwenoImpl.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodTci.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/SimpleWenoImpl.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/Weno.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoType.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/CharacteristicHelpers.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/Flattener.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/KxrcfTci.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/VariablesToLimit.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/Weno.hpp"
#include "Evolution/Systems/NewtonianEuler/Tags.hpp"
#include "NumericalAlgorithms/Interpolation/RegularGridInterpolant.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

namespace {
template <size_t VolumeDim, size_t ThermodynamicDim>
bool characteristic_simple_weno_impl(
    const gsl::not_null<Scalar<DataVector>*> mass_density_cons,
    const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> momentum_density,
    const gsl::not_null<Scalar<DataVector>*> energy_density,
    const double tvb_constant, const double neighbor_linear_weight,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const EquationsOfState::EquationOfState<false, ThermodynamicDim>&
        equation_of_state,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename NewtonianEuler::Limiters::Weno<VolumeDim>::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Storage for transforming neighbor_data into char variables
  using CharacteristicVarsWeno =
      Limiters::Weno<VolumeDim,
                     tmpl::list<NewtonianEuler::Tags::VMinus,
                                NewtonianEuler::Tags::VMomentum<VolumeDim>,
                                NewtonianEuler::Tags::VPlus>>;
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      typename CharacteristicVarsWeno::PackagedData,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_char_data{};
  for (const auto& [key, data] : neighbor_data) {
    neighbor_char_data[key].volume_data.initialize(
        mesh.number_of_grid_points());
    neighbor_char_data[key].mesh = data.mesh;
    neighbor_char_data[key].element_size = data.element_size;
  }

  // Buffers for TCI
  Limiters::Minmod_detail::BufferWrapper<VolumeDim> tci_buffer(mesh);
  const auto effective_neighbor_sizes =
      Limiters::Minmod_detail::compute_effective_neighbor_sizes(element,
                                                                neighbor_data);

  // Buffers for SimpleWeno extrapolated poly
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      intrp::RegularGrid<VolumeDim>,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      interpolator_buffer{};
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      modified_neighbor_solution_buffer{};

  // Outer lambda: wraps applying SimpleWeno to the NewtonianEuler
  // characteristics for one particular choice of characteristic decomposition
  const auto simple_weno_convert_neighbor_data_then_limit =
      [&tci_buffer, &interpolator_buffer, &modified_neighbor_solution_buffer,
       &tvb_constant, &neighbor_linear_weight, &mesh, &element, &element_size,
       &neighbor_data, &neighbor_char_data, &effective_neighbor_sizes](
          const gsl::not_null<Scalar<DataVector>*> char_v_minus,
          const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> char_v_momentum,
          const gsl::not_null<Scalar<DataVector>*> char_v_plus,
          const Matrix& left_eigenvectors) noexcept -> bool {
    // Convert neighbor data to characteristics
    for (const auto& [key, data] : neighbor_data) {
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].volume_data)),
          data.volume_data, left_eigenvectors);
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].means)), data.means,
          left_eigenvectors);
    }

    bool some_component_was_limited_with_this_normal = false;

    // Inner lambda: apply SimpleWeno to one particular tensor
    const auto wrap_minmod_tci_and_simple_weno =
        [&some_component_was_limited_with_this_normal, &tci_buffer,
         &interpolator_buffer, &modified_neighbor_solution_buffer,
         &tvb_constant, &neighbor_linear_weight, &mesh, &element, &element_size,
         &neighbor_char_data,
         &effective_neighbor_sizes](auto tag, const auto tensor) noexcept {
          for (size_t tensor_storage_index = 0;
               tensor_storage_index < tensor->size(); ++tensor_storage_index) {
            // Check TCI
            const auto effective_neighbor_means =
                Limiters::Minmod_detail::compute_effective_neighbor_means<
                    decltype(tag)>(tensor_storage_index, element,
                                   neighbor_char_data);
            const bool component_needs_limiting =
                Limiters::Tci::tvb_minmod_indicator(
                    make_not_null(&tci_buffer), tvb_constant,
                    (*tensor)[tensor_storage_index], mesh, element,
                    element_size, effective_neighbor_means,
                    effective_neighbor_sizes);

            if (component_needs_limiting) {
              if (modified_neighbor_solution_buffer.empty()) {
                // Allocate the neighbor solution buffers only if the limiter is
                // triggered. This reduces allocation when no limiting occurs.
                for (const auto& [neighbor, data] : neighbor_char_data) {
                  (void)data;
                  modified_neighbor_solution_buffer.insert(std::make_pair(
                      neighbor, DataVector(mesh.number_of_grid_points())));
                }
              }
              Limiters::Weno_detail::simple_weno_impl<decltype(tag)>(
                  make_not_null(&interpolator_buffer),
                  make_not_null(&modified_neighbor_solution_buffer), tensor,
                  neighbor_linear_weight, tensor_storage_index, mesh, element,
                  neighbor_char_data);
              some_component_was_limited_with_this_normal = true;
            }
          }
        };
    wrap_minmod_tci_and_simple_weno(NewtonianEuler::Tags::VMinus{},
                                    char_v_minus);
    wrap_minmod_tci_and_simple_weno(
        NewtonianEuler::Tags::VMomentum<VolumeDim>{}, char_v_momentum);
    wrap_minmod_tci_and_simple_weno(NewtonianEuler::Tags::VPlus{}, char_v_plus);
    return some_component_was_limited_with_this_normal;
  };

  return NewtonianEuler::Limiters::
      apply_limiter_to_characteristic_fields_in_all_directions(
          mass_density_cons, momentum_density, energy_density, mesh,
          equation_of_state, simple_weno_convert_neighbor_data_then_limit);
}

template <size_t VolumeDim, size_t ThermodynamicDim>
bool characteristic_hweno_impl(
    const gsl::not_null<Scalar<DataVector>*> mass_density_cons,
    const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> momentum_density,
    const gsl::not_null<Scalar<DataVector>*> energy_density,
    const double kxrcf_constant, const double neighbor_linear_weight,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_logical_to_inertial_jacobian,
    const typename evolution::dg::Tags::NormalCovectorAndMagnitude<
        VolumeDim>::type& normals_and_magnitudes,
    const EquationsOfState::EquationOfState<false, ThermodynamicDim>&
        equation_of_state,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename NewtonianEuler::Limiters::Weno<VolumeDim>::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Hweno checks TCI before limiting any tensors
  const bool cell_is_troubled = NewtonianEuler::Limiters::Tci::kxrcf_indicator(
      kxrcf_constant, *mass_density_cons, *momentum_density, *energy_density,
      mesh, element, element_size, det_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);
  if (not cell_is_troubled) {
    // No limiting is needed
    return false;
  }

  // Storage for transforming neighbor_data into char variables
  using CharacteristicVarsWeno =
      Limiters::Weno<VolumeDim,
                     tmpl::list<NewtonianEuler::Tags::VMinus,
                                NewtonianEuler::Tags::VMomentum<VolumeDim>,
                                NewtonianEuler::Tags::VPlus>>;
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      typename CharacteristicVarsWeno::PackagedData,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_char_data{};
  for (const auto& [key, data] : neighbor_data) {
    neighbor_char_data[key].volume_data.initialize(
        mesh.number_of_grid_points());
    neighbor_char_data[key].mesh = data.mesh;
    neighbor_char_data[key].element_size = data.element_size;
  }

  // Buffers for Hweno extrapolated poly
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      modified_neighbor_solution_buffer{};
  for (const auto& [neighbor, data] : neighbor_char_data) {
    (void)data;
    modified_neighbor_solution_buffer.insert(
        std::make_pair(neighbor, DataVector(mesh.number_of_grid_points())));
  }

  // Lambda wraps applying Hweno to the NewtonianEuler characteristics for one
  // particular choice of characteristic decomposition
  const auto hweno_convert_neighbor_data_then_limit =
      [&modified_neighbor_solution_buffer, &neighbor_linear_weight, &mesh,
       &element, &neighbor_data, &neighbor_char_data](
          const gsl::not_null<Scalar<DataVector>*> char_v_minus,
          const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> char_v_momentum,
          const gsl::not_null<Scalar<DataVector>*> char_v_plus,
          const Matrix& left_eigenvectors) noexcept -> bool {
    // Convert neighbor data to characteristics
    for (const auto& [key, data] : neighbor_data) {
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].volume_data)),
          data.volume_data, left_eigenvectors);
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].means)), data.means,
          left_eigenvectors);
    }

    ::Limiters::Weno_detail::hweno_impl<NewtonianEuler::Tags::VMinus>(
        make_not_null(&modified_neighbor_solution_buffer), char_v_minus,
        neighbor_linear_weight, mesh, element, neighbor_char_data);
    ::Limiters::Weno_detail::hweno_impl<
        NewtonianEuler::Tags::VMomentum<VolumeDim>>(
        make_not_null(&modified_neighbor_solution_buffer), char_v_momentum,
        neighbor_linear_weight, mesh, element, neighbor_char_data);
    ::Limiters::Weno_detail::hweno_impl<NewtonianEuler::Tags::VPlus>(
        make_not_null(&modified_neighbor_solution_buffer), char_v_plus,
        neighbor_linear_weight, mesh, element, neighbor_char_data);
    return true;  // all components were limited
  };

  NewtonianEuler::Limiters::
      apply_limiter_to_characteristic_fields_in_all_directions(
          mass_density_cons, momentum_density, energy_density, mesh,
          equation_of_state, hweno_convert_neighbor_data_then_limit);
  return true;  // all components were limited
}

template <size_t VolumeDim>
bool conservative_hweno_impl(
    const gsl::not_null<Scalar<DataVector>*> mass_density_cons,
    const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> momentum_density,
    const gsl::not_null<Scalar<DataVector>*> energy_density,
    const double kxrcf_constant, const double neighbor_linear_weight,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_logical_to_inertial_jacobian,
    const typename evolution::dg::Tags::NormalCovectorAndMagnitude<
        VolumeDim>::type& normals_and_magnitudes,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename NewtonianEuler::Limiters::Weno<VolumeDim>::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Hweno checks TCI before limiting any tensors
  const bool cell_is_troubled = NewtonianEuler::Limiters::Tci::kxrcf_indicator(
      kxrcf_constant, *mass_density_cons, *momentum_density, *energy_density,
      mesh, element, element_size, det_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);
  if (not cell_is_troubled) {
    // No limiting is needed
    return false;
  }

  // Buffers for Hweno extrapolated poly
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      modified_neighbor_solution_buffer{};
  for (const auto& [neighbor, data] : neighbor_data) {
    (void)data;
    modified_neighbor_solution_buffer.insert(
        std::make_pair(neighbor, DataVector(mesh.number_of_grid_points())));
  }

  ::Limiters::Weno_detail::hweno_impl<NewtonianEuler::Tags::MassDensityCons>(
      make_not_null(&modified_neighbor_solution_buffer), mass_density_cons,
      neighbor_linear_weight, mesh, element, neighbor_data);
  ::Limiters::Weno_detail::hweno_impl<
      NewtonianEuler::Tags::MomentumDensity<VolumeDim>>(
      make_not_null(&modified_neighbor_solution_buffer), momentum_density,
      neighbor_linear_weight, mesh, element, neighbor_data);
  ::Limiters::Weno_detail::hweno_impl<NewtonianEuler::Tags::EnergyDensity>(
      make_not_null(&modified_neighbor_solution_buffer), energy_density,
      neighbor_linear_weight, mesh, element, neighbor_data);
  return true;  // all components were limited
}

}  // namespace

namespace NewtonianEuler::Limiters {

template <size_t VolumeDim>
Weno<VolumeDim>::Weno(
    const ::Limiters::WenoType weno_type,
    const NewtonianEuler::Limiters::VariablesToLimit vars_to_limit,
    const double neighbor_linear_weight,
    const std::optional<double> tvb_constant,
    const std::optional<double> kxrcf_constant, const bool apply_flattener,
    const bool disable_for_debugging, const Options::Context& context)
    : weno_type_(weno_type),
      vars_to_limit_(vars_to_limit),
      neighbor_linear_weight_(neighbor_linear_weight),
      tvb_constant_(tvb_constant),
      kxrcf_constant_(kxrcf_constant),
      apply_flattener_(apply_flattener),
      disable_for_debugging_(disable_for_debugging),
      conservative_vars_weno_(
          weno_type_, neighbor_linear_weight_,
          tvb_constant_.value_or(std::numeric_limits<double>::signaling_NaN()),
          disable_for_debugging_) {
  if (weno_type == ::Limiters::WenoType::Hweno) {
    if (tvb_constant.has_value() or not kxrcf_constant.has_value()) {
      PARSE_ERROR(context,
                  "The Hweno limiter uses the KXRCF TCI. The TvbConstant must "
                  "be set to 'None', and the KxrcfConstant must be set to a "
                  "non-negative value.");
    }
    if (kxrcf_constant.value() < 0.0) {
      PARSE_ERROR(context, "The KXRCF constant must be non-negative, but got: "
                               << kxrcf_constant.value());
    }
  } else {  // SimpleWeno
    if (not tvb_constant.has_value() or kxrcf_constant.has_value()) {
      PARSE_ERROR(context,
                  "The SimpleWeno limiter uses the TVB minmod TCI. The "
                  "TvbConstant must be set to a non-negative value, and the "
                  "KxrcfConstant must be set to 'None'.");
    }
    if (tvb_constant.value() < 0.0) {
      PARSE_ERROR(context, "The TVB constant must be non-negative, but got: "
                               << tvb_constant.value());
    }
  }
}

template <size_t VolumeDim>
// NOLINTNEXTLINE(google-runtime-references)
void Weno<VolumeDim>::pup(PUP::er& p) noexcept {
  p | weno_type_;
  p | vars_to_limit_;
  p | neighbor_linear_weight_;
  p | tvb_constant_;
  p | kxrcf_constant_;
  p | apply_flattener_;
  p | disable_for_debugging_;
}

template <size_t VolumeDim>
void Weno<VolumeDim>::package_data(
    const gsl::not_null<PackagedData*> packaged_data,
    const Scalar<DataVector>& mass_density_cons,
    const tnsr::I<DataVector, VolumeDim>& momentum_density,
    const Scalar<DataVector>& energy_density, const Mesh<VolumeDim>& mesh,
    const std::array<double, VolumeDim>& element_size,
    const OrientationMap<VolumeDim>& orientation_map) const noexcept {
  conservative_vars_weno_.package_data(packaged_data, mass_density_cons,
                                       momentum_density, energy_density, mesh,
                                       element_size, orientation_map);
}

template <size_t VolumeDim>
template <size_t ThermodynamicDim>
bool Weno<VolumeDim>::operator()(
    const gsl::not_null<Scalar<DataVector>*> mass_density_cons,
    const gsl::not_null<tnsr::I<DataVector, VolumeDim>*> momentum_density,
    const gsl::not_null<Scalar<DataVector>*> energy_density,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_inv_logical_to_inertial_jacobian,
    const typename evolution::dg::Tags::NormalCovectorAndMagnitude<
        VolumeDim>::type& normals_and_magnitudes,
    const EquationsOfState::EquationOfState<false, ThermodynamicDim>&
        equation_of_state,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) const noexcept {
  if (UNLIKELY(disable_for_debugging_)) {
    // Do not modify input tensors
    return false;
  }

  // Enforce restrictions on h-refinement, p-refinement
  if (UNLIKELY(alg::any_of(element.neighbors(),
                           [](const auto& direction_neighbors) noexcept {
                             return direction_neighbors.second.size() != 1;
                           }))) {
    ERROR("The Weno limiter does not yet support h-refinement");
    // Removing this limitation will require:
    // - Generalizing the computation of the modified neighbor solutions.
    // - Generalizing the WENO weighted sum for multiple neighbors in each
    //   direction.
  }
  alg::for_each(neighbor_data, [&mesh](const auto& neighbor_and_data) noexcept {
    if (UNLIKELY(neighbor_and_data.second.mesh != mesh)) {
      ERROR("The Weno limiter does not yet support p-refinement");
      // Removing this limitation will require generalizing the
      // computation of the modified neighbor solutions.
    }
  });

  // Checks for the post-timestep, pre-limiter NewtonianEuler state:
#ifdef SPECTRE_DEBUG
  const double mean_density = mean_value(get(*mass_density_cons), mesh);
  ASSERT(mean_density > 0.0,
         "Positivity was violated on a cell-average level.");
  if (ThermodynamicDim == 2) {
    const double mean_energy = mean_value(get(*energy_density), mesh);
    ASSERT(mean_energy > 0.0,
           "Positivity was violated on a cell-average level.");
  }
#endif  // SPECTRE_DEBUG

  bool limiter_activated = false;

  // Small possible optimization: only compute this if needed
  const Scalar<DataVector> det_logical_to_inertial_jacobian{
      1. / get(det_inv_logical_to_inertial_jacobian)};

  if (weno_type_ == ::Limiters::WenoType::Hweno) {
    if (vars_to_limit_ ==
        NewtonianEuler::Limiters::VariablesToLimit::Characteristic) {
      // impl function handles specialized TCI + char transform
      limiter_activated = characteristic_hweno_impl(
          mass_density_cons, momentum_density, energy_density,
          kxrcf_constant_.value(), neighbor_linear_weight_, mesh, element,
          element_size, det_logical_to_inertial_jacobian,
          normals_and_magnitudes, equation_of_state, neighbor_data);
    } else {
      // impl function handles specialized TCI
      limiter_activated = conservative_hweno_impl(
          mass_density_cons, momentum_density, energy_density,
          kxrcf_constant_.value(), neighbor_linear_weight_, mesh, element,
          element_size, det_logical_to_inertial_jacobian,
          normals_and_magnitudes, neighbor_data);
    }
  } else if (weno_type_ == ::Limiters::WenoType::SimpleWeno) {
    if (vars_to_limit_ ==
        NewtonianEuler::Limiters::VariablesToLimit::Characteristic) {
      // impl function handles char transform
      limiter_activated = characteristic_simple_weno_impl(
          mass_density_cons, momentum_density, energy_density,
          tvb_constant_.value(), neighbor_linear_weight_, mesh, element,
          element_size, equation_of_state, neighbor_data);
    } else {
      // Fall back to generic SimpleWeno
      limiter_activated = conservative_vars_weno_(
          mass_density_cons, momentum_density, energy_density, mesh, element,
          element_size, neighbor_data);
    }
  }

  if (apply_flattener_) {
    const auto flattener_action = flatten_solution(
        mass_density_cons, momentum_density, energy_density, mesh,
        det_logical_to_inertial_jacobian, equation_of_state);
    if (flattener_action != FlattenerAction::NoOp) {
      limiter_activated = true;
    }
  }

  // Checks for the post-limiter NewtonianEuler state:
#ifdef SPECTRE_DEBUG
  ASSERT(min(get(*mass_density_cons)) > 0.0, "Bad density after limiting.");
  if constexpr (ThermodynamicDim == 2) {
    const auto specific_internal_energy = Scalar<DataVector>{
        get(*energy_density) / get(*mass_density_cons) -
        0.5 * get(dot_product(*momentum_density, *momentum_density)) /
            square(get(*mass_density_cons))};
    const auto pressure = equation_of_state.pressure_from_density_and_energy(
        *mass_density_cons, specific_internal_energy);
    ASSERT(min(get(pressure)) > 0.0, "Bad energy after limiting.");
  }
#endif  // SPECTRE_DEBUG

  return limiter_activated;
}

template <size_t LocalDim>
bool operator==(const Weno<LocalDim>& lhs, const Weno<LocalDim>& rhs) noexcept {
  // No need to compare the conservative_vars_weno_ member variable because
  // it is constructed from the other member variables.
  return lhs.weno_type_ == rhs.weno_type_ and
         lhs.vars_to_limit_ == rhs.vars_to_limit_ and
         lhs.neighbor_linear_weight_ == rhs.neighbor_linear_weight_ and
         lhs.tvb_constant_ == rhs.tvb_constant_ and
         lhs.kxrcf_constant_ == rhs.kxrcf_constant_ and
         lhs.apply_flattener_ == rhs.apply_flattener_ and
         lhs.disable_for_debugging_ == rhs.disable_for_debugging_;
}

template <size_t VolumeDim>
bool operator!=(const Weno<VolumeDim>& lhs,
                const Weno<VolumeDim>& rhs) noexcept {
  return not(lhs == rhs);
}

#define DIM(data) BOOST_PP_TUPLE_ELEM(0, data)
#define THERMO_DIM(data) BOOST_PP_TUPLE_ELEM(1, data)

#define INSTANTIATE(_, data)                                                \
  template class Weno<DIM(data)>;                                           \
  template bool operator==(const Weno<DIM(data)>&, const Weno<DIM(data)>&); \
  template bool operator!=(const Weno<DIM(data)>&, const Weno<DIM(data)>&);

GENERATE_INSTANTIATIONS(INSTANTIATE, (1, 2, 3))

#undef INSTANTIATE

#define INSTANTIATE(_, data)                                                   \
  template bool Weno<DIM(data)>::operator()(                                   \
      const gsl::not_null<Scalar<DataVector>*>,                                \
      const gsl::not_null<tnsr::I<DataVector, DIM(data)>*>,                    \
      const gsl::not_null<Scalar<DataVector>*>, const Mesh<DIM(data)>&,        \
      const Element<DIM(data)>&, const std::array<double, DIM(data)>&,         \
      const Scalar<DataVector>&,                                               \
      const typename evolution::dg::Tags::NormalCovectorAndMagnitude<DIM(      \
          data)>::type&,                                                       \
      const EquationsOfState::EquationOfState<false, THERMO_DIM(data)>&,       \
      const std::unordered_map<                                                \
          std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>, PackagedData, \
          boost::hash<                                                         \
              std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>>>&)        \
      const noexcept;

GENERATE_INSTANTIATIONS(INSTANTIATE, (1, 2, 3), (1, 2))

#undef INSTANTIATE
#undef DIM
#undef THERMO_DIM

}  // namespace NewtonianEuler::Limiters


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// 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.

#ifndef BOOST_GEOMETRY_PROJECTIONS_LAEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LAEA_HPP

#include <boost/config.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_auth.hpp>
#include <boost/geometry/srs/projections/impl/pj_qsfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace laea
    {
            static const double epsilon10 = 1.e-10;

            enum mode_type {
                n_pole = 0,
                s_pole = 1,
                equit  = 2,
                obliq  = 3
            };

            template <typename T>
            struct par_laea
            {
                T   sinb1;
                T   cosb1;
                T   xmf;
                T   ymf;
                T   mmf;
                T   qp;
                T   dd;
                T   rq;
                detail::apa<T> apa;
                mode_type mode;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_laea_ellipsoid
                : public base_t_fi<base_laea_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_laea<T> m_proj_parm;

                inline base_laea_ellipsoid(const Parameters& par)
                    : base_t_fi<base_laea_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T coslam, sinlam, sinphi, q, sinb=0.0, cosb=0.0, b=0.0;

                    coslam = cos(lp_lon);
                    sinlam = sin(lp_lon);
                    sinphi = sin(lp_lat);
                    q = pj_qsfn(sinphi, this->m_par.e, this->m_par.one_es);

                    if (this->m_proj_parm.mode == obliq || this->m_proj_parm.mode == equit) {
                        sinb = q / this->m_proj_parm.qp;
                        cosb = sqrt(1. - sinb * sinb);
                    }

                    switch (this->m_proj_parm.mode) {
                    case obliq:
                        b = 1. + this->m_proj_parm.sinb1 * sinb + this->m_proj_parm.cosb1 * cosb * coslam;
                        break;
                    case equit:
                        b = 1. + cosb * coslam;
                        break;
                    case n_pole:
                        b = half_pi + lp_lat;
                        q = this->m_proj_parm.qp - q;
                        break;
                    case s_pole:
                        b = lp_lat - half_pi;
                        q = this->m_proj_parm.qp + q;
                        break;
                    }
                    if (fabs(b) < epsilon10) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }

                    switch (this->m_proj_parm.mode) {
                    case obliq:
                        b = sqrt(2. / b);
                        xy_y = this->m_proj_parm.ymf * b * (this->m_proj_parm.cosb1 * sinb - this->m_proj_parm.sinb1 * cosb * coslam);
                        goto eqcon;
                        break;
                    case equit:
                        b = sqrt(2. / (1. + cosb * coslam));
                        xy_y = b * sinb * this->m_proj_parm.ymf;
                eqcon:
                        xy_x = this->m_proj_parm.xmf * b * cosb * sinlam;
                        break;
                    case n_pole:
                    case s_pole:
                        if (q >= 0.) {
                            b = sqrt(q);
                            xy_x = b * sinlam;
                            xy_y = coslam * (this->m_proj_parm.mode == s_pole ? b : -b);
                        } else
                            xy_x = xy_y = 0.;
                        break;
                    }
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    T cCe, sCe, q, rho, ab=0.0;

                    switch (this->m_proj_parm.mode) {
                    case equit:
                    case obliq:
                        xy_x /= this->m_proj_parm.dd;
                        xy_y *=  this->m_proj_parm.dd;
                        rho = boost::math::hypot(xy_x, xy_y);
                        if (rho < epsilon10) {
                            lp_lon = 0.;
                            lp_lat = this->m_par.phi0;
                            return;
                        }
                        sCe = 2. * asin(.5 * rho / this->m_proj_parm.rq);
                        cCe = cos(sCe);
                        sCe = sin(sCe);
                        xy_x *= sCe;
                        if (this->m_proj_parm.mode == obliq) {
                            ab = cCe * this->m_proj_parm.sinb1 + xy_y * sCe * this->m_proj_parm.cosb1 / rho;
                            xy_y = rho * this->m_proj_parm.cosb1 * cCe - xy_y * this->m_proj_parm.sinb1 * sCe;
                        } else {
                            ab = xy_y * sCe / rho;
                            xy_y = rho * cCe;
                        }
                        break;
                    case n_pole:
                        xy_y = -xy_y;
                        BOOST_FALLTHROUGH;
                    case s_pole:
                        q = (xy_x * xy_x + xy_y * xy_y);
                        if (q == 0.0) {
                            lp_lon = 0.;
                            lp_lat = this->m_par.phi0;
                            return;
                        }
                        ab = 1. - q / this->m_proj_parm.qp;
                        if (this->m_proj_parm.mode == s_pole)
                            ab = - ab;
                        break;
                    }
                    lp_lon = atan2(xy_x, xy_y);
                    lp_lat = pj_authlat(asin(ab), this->m_proj_parm.apa);
                }

                static inline std::string get_name()
                {
                    return "laea_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_laea_spheroid
                : public base_t_fi<base_laea_spheroid<T, Parameters>, T, Parameters>
            {
                par_laea<T> m_proj_parm;

                inline base_laea_spheroid(const Parameters& par)
                    : base_t_fi<base_laea_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T fourth_pi = detail::fourth_pi<T>();

                    T  coslam, cosphi, sinphi;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    coslam = cos(lp_lon);
                    switch (this->m_proj_parm.mode) {
                    case equit:
                        xy_y = 1. + cosphi * coslam;
                        goto oblcon;
                    case obliq:
                        xy_y = 1. + this->m_proj_parm.sinb1 * sinphi + this->m_proj_parm.cosb1 * cosphi * coslam;
                oblcon:
                        if (xy_y <= epsilon10) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        xy_y = sqrt(2. / xy_y);
                        xy_x = xy_y * cosphi * sin(lp_lon);
                        xy_y *= this->m_proj_parm.mode == equit ? sinphi :
                           this->m_proj_parm.cosb1 * sinphi - this->m_proj_parm.sinb1 * cosphi * coslam;
                        break;
                    case n_pole:
                        coslam = -coslam;
                        BOOST_FALLTHROUGH;
                    case s_pole:
                        if (fabs(lp_lat + this->m_par.phi0) < epsilon10) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        xy_y = fourth_pi - lp_lat * .5;
                        xy_y = 2. * (this->m_proj_parm.mode == s_pole ? cos(xy_y) : sin(xy_y));
                        xy_x = xy_y * sin(lp_lon);
                        xy_y *= coslam;
                        break;
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T  cosz=0.0, rh, sinz=0.0;

                    rh = boost::math::hypot(xy_x, xy_y);
                    if ((lp_lat = rh * .5 ) > 1.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    lp_lat = 2. * asin(lp_lat);
                    if (this->m_proj_parm.mode == obliq || this->m_proj_parm.mode == equit) {
                        sinz = sin(lp_lat);
                        cosz = cos(lp_lat);
                    }
                    switch (this->m_proj_parm.mode) {
                    case equit:
                        lp_lat = fabs(rh) <= epsilon10 ? 0. : asin(xy_y * sinz / rh);
                        xy_x *= sinz;
                        xy_y = cosz * rh;
                        break;
                    case obliq:
                        lp_lat = fabs(rh) <= epsilon10 ? this->m_par.phi0 :
                           asin(cosz * this->m_proj_parm.sinb1 + xy_y * sinz * this->m_proj_parm.cosb1 / rh);
                        xy_x *= sinz * this->m_proj_parm.cosb1;
                        xy_y = (cosz - sin(lp_lat) * this->m_proj_parm.sinb1) * rh;
                        break;
                    case n_pole:
                        xy_y = -xy_y;
                        lp_lat = half_pi - lp_lat;
                        break;
                    case s_pole:
                        lp_lat -= half_pi;
                        break;
                    }
                    lp_lon = (xy_y == 0. && (this->m_proj_parm.mode == equit || this->m_proj_parm.mode == obliq)) ?
                        0. : atan2(xy_x, xy_y);
                }

                static inline std::string get_name()
                {
                    return "laea_spheroid";
                }

            };

            // Lambert Azimuthal Equal Area
            template <typename Parameters, typename T>
            inline void setup_laea(Parameters& par, par_laea<T>& proj_parm)
            {
                static const T half_pi = detail::half_pi<T>();

                T t;

                t = fabs(par.phi0);
                if (fabs(t - half_pi) < epsilon10)
                    proj_parm.mode = par.phi0 < 0. ? s_pole : n_pole;
                else if (fabs(t) < epsilon10)
                    proj_parm.mode = equit;
                else
                    proj_parm.mode = obliq;
                if (par.es != 0.0) {
                    double sinphi;

                    par.e = sqrt(par.es);
                    proj_parm.qp = pj_qsfn(1., par.e, par.one_es);
                    proj_parm.mmf = .5 / (1. - par.es);
                    proj_parm.apa = pj_authset<T>(par.es);
                    switch (proj_parm.mode) {
                    case n_pole:
                    case s_pole:
                        proj_parm.dd = 1.;
                        break;
                    case equit:
                        proj_parm.dd = 1. / (proj_parm.rq = sqrt(.5 * proj_parm.qp));
                        proj_parm.xmf = 1.;
                        proj_parm.ymf = .5 * proj_parm.qp;
                        break;
                    case obliq:
                        proj_parm.rq = sqrt(.5 * proj_parm.qp);
                        sinphi = sin(par.phi0);
                        proj_parm.sinb1 = pj_qsfn(sinphi, par.e, par.one_es) / proj_parm.qp;
                        proj_parm.cosb1 = sqrt(1. - proj_parm.sinb1 * proj_parm.sinb1);
                        proj_parm.dd = cos(par.phi0) / (sqrt(1. - par.es * sinphi * sinphi) *
                           proj_parm.rq * proj_parm.cosb1);
                        proj_parm.ymf = (proj_parm.xmf = proj_parm.rq) / proj_parm.dd;
                        proj_parm.xmf *= proj_parm.dd;
                        break;
                    }
                } else {
                    if (proj_parm.mode == obliq) {
                        proj_parm.sinb1 = sin(par.phi0);
                        proj_parm.cosb1 = cos(par.phi0);
                    }
                }
            }

    }} // namespace laea
    #endif // doxygen

    /*!
        \brief Lambert Azimuthal Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_laea.gif
    */
    template <typename T, typename Parameters>
    struct laea_ellipsoid : public detail::laea::base_laea_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline laea_ellipsoid(Params const& , Parameters const& par)
            : detail::laea::base_laea_ellipsoid<T, Parameters>(par)
        {
            detail::laea::setup_laea(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Lambert Azimuthal Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_laea.gif
    */
    template <typename T, typename Parameters>
    struct laea_spheroid : public detail::laea::base_laea_spheroid<T, Parameters>
    {
        template <typename Params>
        inline laea_spheroid(Params const& , Parameters const& par)
            : detail::laea::base_laea_spheroid<T, Parameters>(par)
        {
            detail::laea::setup_laea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_laea, laea_spheroid, laea_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(laea_entry, laea_spheroid, laea_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(laea_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(laea, laea_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LAEA_HPP


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

#include <ursa_local_planner/ursa_obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>
#include <costmap_2d/cost_values.h>
#include <iostream>
using namespace std;
using namespace costmap_2d;

namespace ursa_local_planner {

UrsaObstacleCostFunction::UrsaObstacleCostFunction(costmap_2d::Costmap2D* costmap)
    : costmap_(costmap), sum_scores_(false) {
  if (costmap != NULL) {
    world_model_ = new ursa_local_planner::UrsaCostmapModel(*costmap_);
  }
}

UrsaObstacleCostFunction::~UrsaObstacleCostFunction() {
  if (world_model_ != NULL) {
    delete world_model_;
  }
}

void UrsaObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
  // TODO: move this to prepare if possible
  max_trans_vel_ = max_trans_vel;
  max_scaling_factor_ = max_scaling_factor;
  scaling_speed_ = scaling_speed;
}

void UrsaObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
  footprint_spec_ = footprint_spec;
}

bool UrsaObstacleCostFunction::prepare() {
  return true;
}

inline bool checkIfInsideRadius(double x, double y, double pose_x, double pose_y, double multiplier, double radius){
    // get global orientation at robot radius
    int i = 0;
    while   (((pose_x - radius*multiplier) <= x) &&
             (x < (pose_x + radius*multiplier))  &&
             ((pose_y - radius*multiplier) <= y) &&
             (y < (pose_y + radius*multiplier))) {
                return 1;
            }
    return 0;
}

double UrsaObstacleCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  double dx, dy, dth;
  traj.getPoint(0, dx, dy, dth);
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }
    // return the cost of a point just outside the radius of drone
    if (checkIfInsideRadius(px,py,dx,dy,1,footprint_spec_[0].x)){
      cost=f_cost;
      continue;
      //ROS_INFO("obstacle cost -- 1 -- %f", f_cost); // returns cost of last point on trajectory
      //return f_cost;
    }
    // if(sum_scores_)
    //     cost +=  f_cost;
    // else
    //     cost = f_cost;
    // if (f_cost > cost){
    //   cost = f_cost;
    // }
    if (f_cost>cost){
      cost=f_cost;
    }

  }
  // cost = cost / traj.getPointsSize();
  // ROS_INFO("obstacle cost -- %f", cost); // returns cost of last point on trajectory
  return cost;
}

double UrsaObstacleCostFunction::getScalingFactor(base_local_planner::Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
  double vmag = hypot(traj.xv_, traj.yv_);

  //if we're over a certain speed threshold, we'll scale the robot's
  //footprint to make it either slow down or stay further from walls
  double scale = 1.0;
  if (vmag > scaling_speed) {
    //scale up to the max scaling factor linearly... this could be changed later
    double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
    scale = max_scaling_factor * ratio + 1.0;
  }
  return scale;
}

double UrsaObstacleCostFunction::footprintCost (
    const double& x,
    const double& y,
    const double& th,
    double scale,
    std::vector<geometry_msgs::Point> footprint_spec,
    costmap_2d::Costmap2D* costmap,
    base_local_planner::WorldModel* world_model) {

  //check if the footprint is legal
  // TODO: Cache inscribed radius
  //double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);

  unsigned int cell_x, cell_y;
  //we won't allow trajectories that go off the map... shouldn't happen that often anyways
  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
    return -7.0;
  }
  double footprint_cost = costmap->getCost(cell_x, cell_y);
  if (footprint_cost < 0) {
    return -6.0;
  }
  //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
  if(footprint_cost == LETHAL_OBSTACLE || footprint_cost == INSCRIBED_INFLATED_OBSTACLE || footprint_cost == NO_INFORMATION)
    return -1.0;
  return footprint_cost;

  // return the max of (cell cost at footprint (last two points)) and (cell cost at centre of robot)
  // double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));

  // return occ_cost;

}

} /* namespace ursa_local_planner */