diff --git a/cpp_src/dsp/lasp_freqsmooth.cpp b/cpp_src/dsp/lasp_freqsmooth.cpp
index 795809b..8381771 100644
--- a/cpp_src/dsp/lasp_freqsmooth.cpp
+++ b/cpp_src/dsp/lasp_freqsmooth.cpp
@@ -1,13 +1,14 @@
 // #define DEBUGTRACE_ENABLED
-#include "debugtrace.hpp"
 #include "lasp_freqsmooth.h"
 
+#include "debugtrace.hpp"
+
 using rte = std::runtime_error;
 
 vd freqSmooth(const vd& freq, const vd& X, const unsigned w) {
   DEBUGTRACE_ENTER;
   if (freq.size() < 2) {
-    throw rte("Invalid frequency size");
+    throw rte("Invalid frequency size. Should be > 2");
   }
   if (freq.size() != X.size()) {
     throw rte("Sizes of freq and X do not match");
@@ -18,70 +19,94 @@ vd freqSmooth(const vd& freq, const vd& X, const unsigned w) {
   if (w == 0) {
     throw rte("Invalid number of octaves");
   }
+  const us Nfreq = freq.size();
 
   // Smoothing width in unit of number of octaves
   const d Delta = 1 / d(w);
 
+  // Minimum frequency and maximum frequency to smooth on (frequency range that
+  // is interpolated to a log scale)
   d freq_min;
-  if (freq(0) == 0) {
+  const d freq_max = freq(Nfreq - 1);
+  const bool firstFreqEqZero = (d_abs(freq(0)) < 1e-15);
+
+  // AC-signal power
+  d ac_pwr;
+  if (firstFreqEqZero) {
     freq_min = freq(1);
+    ac_pwr = arma::sum(X.subvec(1, Nfreq - 1));
   } else {
     freq_min = freq(0);
+    ac_pwr = arma::sum(X);
   }
-  const d freq_max = freq(freq.size() - 1);
   DEBUGTRACE_PRINT(freq_min);
   DEBUGTRACE_PRINT(freq_max);
   const vd freq_log =
-      arma::logspace(d_log10(freq_min), d_log10(freq_max), 10 * freq.size());
+      arma::logspace(d_log10(freq_min), d_log10(freq_max), 10 * Nfreq);
   DEBUGTRACE_PRINT("freq_log = ");
-//   std::cerr << freq_log << std::endl;
 
-  const long N = freq_log.size();
+  const long Nfreq_sm = freq_log.size();
 
   // Interpolate X to logscale
   vd X_log;
   DEBUGTRACE_PRINT("X_log = :");
   arma::interp1(freq, X, freq_log, X_log, "*linear");
-  // Last point is not interpolated well.
-  X_log(N - 1) = X(X.size() - 1);
-//   std::cerr << X_log << std::endl;
 
+  // First and last point are not interpolated well, could be minimally out of
+  // the interpolation range, due to roundoff errors. Armadillo sets these
+  // points to nan, so we have to manually "interpolate" them.
+  X_log(Nfreq_sm - 1) = X(X.size() - 1);
+  if (firstFreqEqZero) {
+    X_log(0) = X(1);
+  } else {
+    X_log(0) = X(0);
+  }
+
+  // Allocate space for smoothed X on log scale
   vd Xsm_log(freq_log.size());
-  const d beta = d_log10(N) / d_log10(2) / (N - 1);
-  DEBUGTRACE_PRINT(beta);
-
-  const long mu = int(Delta / 2 / beta);
+  const d beta = d_log10(Nfreq_sm) / d_log10(2) / (Nfreq_sm - 1);
+  // int rounds down
+  const long mu = int(Delta / d(2) / beta);
   DEBUGTRACE_PRINT(mu);
+
   // Long is at least 32 bits. So +/- 2M points length
-  for (long k = 0; k < freq_log.size(); k++) {
+  for (long k = 0; k < Nfreq_sm; k++) {
     // const d fcur = freq_log(k);
     const us idx_start = std::max(k - mu, 0l);
-    const us idx_stop = std::min(k + mu, N - 1);
+    const us idx_stop = std::min(k + mu, Nfreq_sm - 1);
     // DEBUGTRACE_PRINT(idx_start)
     // DEBUGTRACE_PRINT(idx_stop);
 
     Xsm_log(k) = arma::mean(X_log.subvec(idx_start, idx_stop));
   }
   DEBUGTRACE_PRINT("Xsm_log:");
-//   std::cerr << Xsm_log << std::endl;
+  //   std::cerr << Xsm_log << std::endl;
 
-  // Back-interpolate to a linear scale
-  vd Xsm(freq.size());
-  if (freq(0) == 0) {
-    Xsm(0) = X(0);
+  // Back-interpolate to a linear scale, and be wary of nans at the start end
+  // and range. Also interpolates power
+  vd Xsm(Nfreq);
+  if (firstFreqEqZero) {
     vd Xsm_gt0;
-    arma::interp1(freq_log, Xsm_log, freq.subvec(1, freq.size() - 1), Xsm_gt0,
+    arma::interp1(freq_log, Xsm_log, freq.subvec(1, Nfreq - 1), Xsm_gt0,
                   "*linear");
-    Xsm.subvec(1, freq.size() - 1) = Xsm_gt0;
+    Xsm(0) = X(0);
+    Xsm.subvec(1, Nfreq - 1) = Xsm_gt0;
     Xsm(1) = Xsm_log(1);
+    Xsm(Nfreq - 1) = Xsm_log(Nfreq_sm - 1);
+
+    // Final step: power-correct smoothed spectrum
+    d new_acpwr = arma::sum(Xsm.subvec(1, Nfreq - 1));
+    Xsm.subvec(1, Nfreq - 1) *= ac_pwr / new_acpwr;
 
   } else {
-    Xsm(0) = X(0);
     arma::interp1(freq_log, Xsm_log, freq, Xsm, "*linear");
+    Xsm(0) = X(0);
+    Xsm(Nfreq - 1) = Xsm_log(Nfreq_sm - 1);
+
+    // Final step: power-correct smoothed spectrum
+    d new_acpwr = arma::sum(Xsm);
+    Xsm *= ac_pwr / new_acpwr;
   }
-  Xsm(freq.size() - 1) = Xsm_log(N - 1);
-  DEBUGTRACE_PRINT("Xsm:");
-//   std::cerr << Xsm << std::endl;
 
   return Xsm;
 }
\ No newline at end of file
diff --git a/cpp_src/pybind11/lasp_dsp_pybind.cpp b/cpp_src/pybind11/lasp_dsp_pybind.cpp
index 91c9647..906c6a4 100644
--- a/cpp_src/pybind11/lasp_dsp_pybind.cpp
+++ b/cpp_src/pybind11/lasp_dsp_pybind.cpp
@@ -7,7 +7,7 @@
 #include "lasp_biquadbank.h"
 #include "lasp_fft.h"
 #include "lasp_filter.h"
-#include "lasp_freqsmoothing.h"
+#include "lasp_freqsmooth.h"
 #include "lasp_slm.h"
 #include "lasp_streammgr.h"
 #include "lasp_window.h"