// lasp_siggen_impl.cpp
//
// Author: J.A. de Jong -ASCEE
//
// Description:
// Signal generators implementation
//////////////////////////////////////////////////////////////////////
/* #define DEBUGTRACE_ENABLED */
#include "lasp_siggen_impl.h"
#include "debugtrace.hpp"
#include "lasp_mathtypes.h"
#include <cassert>

using rte = std::runtime_error;

DEBUGTRACE_VARIABLES;

/** The fixed number of Newton iterations t.b.d. for tuning the sweep start and
 * stop frequency in logarithmic sweeps */
#define NITER_NEWTON 20

Noise::Noise(){DEBUGTRACE_ENTER}

vd Noise::genSignalUnscaled(us nframes) {
  return arma::randn<vd>(nframes);
}
void Noise::resetImpl() {}

Sine::Sine(const d freq) : omg(2 * arma::datum::pi * freq) { DEBUGTRACE_ENTER; }

vd Sine::genSignalUnscaled(const us nframes) {
  /* DEBUGTRACE_ENTER; */
  const d pi = arma::datum::pi;
  vd phase_vec =
      arma::linspace(phase, phase + omg * (nframes - 1) / _fs, nframes);
  phase += omg * nframes / _fs;
  while (phase > 2 * arma::datum::pi) {
    phase -= 2 * pi;
  }
  return arma::sin(phase_vec);
}

vd Periodic::genSignalUnscaled(const us nframes) {

  vd res(nframes);
  if (_signal.size() == 0) {
    throw rte("No signal defined while calling");
  }
  for (us i = 0; i < nframes; i++) {
    res(i) = _signal[_cur_pos];
    _cur_pos++;
    _cur_pos %= _signal.size();
  }
  return res;
}

Sweep::Sweep(const d fl, const d fu, const d Ts, const d Tq, const us flags)
    : fl_(fl), fu_(fu), Ts(Ts), Tq(Tq), flags(flags) {
  if (fl <= 0 || fu < fl || Ts <= 0) {
    throw rte("Invalid sweep parameters");
  }
  if ((flags & ForwardSweep) && (flags & BackwardSweep)) {
    throw rte(
        "Both forward and backward sweep flag set. Please only set either one "
        "or none for a continuous sweep");
  }
  if ((flags & LinearSweep) && (flags & LogSweep)) {
    throw rte(
        "Both logsweep and linear sweep flag set. Please only set either one.");
  }
  if (!((flags & LinearSweep) || (flags & LogSweep))) {
    throw rte("Either LinearSweep or LogSweep should be given as flag");
  }
}

void Sweep::resetImpl() {

  DEBUGTRACE_ENTER;

  _cur_pos = 0;

  bool forward_sweep = flags & ForwardSweep;
  bool backward_sweep = flags & BackwardSweep;

  const d Dt = 1 / _fs; // Deltat

  // Estimate N, the number of samples in the sweep part (non-quiescent part):
  const us Ns = (us)(Ts * _fs);
  const us Nq = (us)(Tq * _fs);
  const us N = Ns + Nq;

  _signal = vd(N, arma::fill::zeros);
  index = 0;

  d fl, fu;
  /* Swap fl and fu for a backward sweep */
  if (backward_sweep) {
    fu = fl_;
    fl = fu_;
  } else {
    /* Case of continuous sweep, or forward sweep */
    fl = fl_;
    fu = fu_;
  }

  d phase = 0;

  /* Linear sweep */
  if (flags & LinearSweep) {
    if (forward_sweep || backward_sweep) {
      /* Forward or backward sweep */
      /* TRACE(15, "Forward or backward sweep"); */
      us K = (us)(Dt * (fl * Ns + 0.5 * (Ns - 1) * (fu - fl)));
      d eps_num = ((d)K) / Dt - fl * Ns - 0.5 * (Ns - 1) * (fu - fl);
      d eps = eps_num / (0.5 * (Ns - 1));
      /* iVARTRACE(15, K); */
      /* dVARTRACE(15, eps); */

      for (us n = 0; n < Ns; n++) {
        _signal(n) = d_sin(phase);
        d fn = fl + ((d)n) / Ns * (fu + eps - fl);
        phase += 2 * arma::datum::pi * Dt * fn;
      }
    } else {
      /* Continous sweep */
      /* TRACE(15, "continuous sweep"); */

      /* iVARTRACE(17, N); */
      /* dVARTRACE(17, fl); */
      /* dVARTRACE(17, fu); */

      const us Nf = Ns / 2;
      const us Nb = Ns - Nf;

      /* Phi halfway */
      d phih = 2 * number_pi * Dt * (fl * Nf + 0.5 * (Nf - 1) * (fu - fl));

      us K =
          (us)(phih / (2 * number_pi) + Dt * (fu * Nb - (Nb - 1) * (fu - fl)));

      d eps_num1 = (K - phih / (2 * number_pi)) / Dt;
      d eps_num2 = -fu * Nb + (Nb - 1) * (fu - fl);
      d eps = (eps_num1 + eps_num2) / (0.5 * (Nb + 1));

      /* iVARTRACE(15, K); */
      /* dVARTRACE(15, eps); */
      d phase = 0;

      for (us n = 0; n <= Ns; n++) {
        /* iVARTRACE(17, n); */
        if (n < N) {
          _signal[n] = d_sin(phase);
        }

        d fn;
        if (n <= Nf) {
          fn = fl + ((d)n) / Nf * (fu - fl);
        } else {
          fn = fu - ((d)n - Nf) / Nb * (fu + eps - fl);
        }
        /* dbgassert(fn >= 0, "BUG"); */

        phase += 2 * number_pi * Dt * fn;
      }
      /* This should be a very small number!! */
      /* dVARTRACE(15, phase); */
    }
  } else if (flags & LogSweep) {

    DEBUGTRACE_PRINT("Log sweep");
    if (forward_sweep || backward_sweep) {
      /* Forward or backward sweep */
      DEBUGTRACE_PRINT("Forward or backward sweep");
      d k1 = (fu / fl);
      us K = (us)(Dt * fl * (k1 - 1) / (d_pow(k1, 1.0 / Ns) - 1));
      d k = k1;

      /* Iterate k to the right solution */
      d E;
      for (us iter = 0; iter < 10; iter++) {
        E = 1 + K / (Dt * fl) * (d_pow(k, 1.0 / Ns) - 1) - k;
        d dEdk = K / (Dt * fl) * d_pow(k, 1.0 / Ns) / (Ns * k) - 1;
        k -= E / dEdk;
      }

      DEBUGTRACE_PRINT(K);
      DEBUGTRACE_PRINT(k1);
      DEBUGTRACE_PRINT(k);
      DEBUGTRACE_PRINT(E);

      for (us n = 0; n < Ns; n++) {
        _signal[n] = d_sin(phase);
        d fn = fl * d_pow(k, ((d)n) / Ns);
        phase += 2 * number_pi * Dt * fn;
      }
    } else {

      DEBUGTRACE_PRINT("Continuous sweep");

      const us Nf = Ns / 2;
      const us Nb = Ns - Nf;
      const d k1 = (fu / fl);
      const d phif1 =
          2 * number_pi * Dt * fl * (k1 - 1) / (d_pow(k1, 1.0 / Nf) - 1);
      const us K = (us)(phif1 / (2 * number_pi) +
                        Dt * fu * (1 / k1 - 1) / (d_pow(1 / k1, 1.0 / Nb) - 1));

      d E;
      d k = k1;

      /* Newton iterations to converge k to the value such that the sweep is
       * continuous */
      for (us iter = 0; iter < NITER_NEWTON; iter++) {
        E = (k - 1) / (d_pow(k, 1.0 / Nf) - 1) +
            (k - 1) / (1 - d_pow(k, -1.0 / Nb)) - K / Dt / fl;
        DEBUGTRACE_PRINT(E);

        /* All parts of the derivative of above error E to k */
        d dEdk1 = 1 / (d_pow(k, 1.0 / Nf) - 1);
        d dEdk2 = (1 / k - 1) / (d_pow(k, -1.0 / Nb) - 1);
        d dEdk3 = -1 / (k * (d_pow(k, -1.0 / Nb) - 1));
        d dEdk4 = d_pow(k, -1.0 / Nb) * (1 / k - 1) /
                  (Nb * d_pow(d_pow(k, -1.0 / Nb) - 1, 2));
        d dEdk5 = -d_pow(k, 1.0 / Nf) * (k - 1) /
                  (Nf * k * d_pow(d_pow(k, 1.0 / Nf) - 1, 2));
        d dEdk = dEdk1 + dEdk2 + dEdk3 + dEdk4 + dEdk5;

        /* Iterate! */
        k -= E / dEdk;
      }

      DEBUGTRACE_PRINT(K);
      DEBUGTRACE_PRINT(k1);
      DEBUGTRACE_PRINT(k);
      DEBUGTRACE_PRINT(E);

      for (us n = 0; n <= Ns; n++) {
        /* iVARTRACE(17, n); */
        if (n < Ns) {
          _signal[n] = d_sin(phase);
        }

        d fn;
        if (n <= Nf) {
          fn = fl * d_pow(k, ((d)n) / Nf);
        } else {
          fn = fl * k * d_pow(1 / k, ((d)n - Nf) / Nb);
        }
        /* dbgassert(fn >= 0, "BUG"); */

        phase += 2 * number_pi * Dt * fn;
        while (phase > 2 * number_pi)
          phase -= 2 * number_pi;
        /* dVARTRACE(17, phase); */
      }
      /* This should be a very small number!! */
      DEBUGTRACE_PRINT(phase);
    }
  } // End of log sweep
  else {
    // Either log or linear sweep had to be given as flags.
    assert(false);

  }
}