// clang -DHAVE_INLINE -O3 mussel.c -o mussel -lgsl -lgslcblas

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

int main(int argc, char *argv[])
{
    // Setup the random number generator:
    gsl_rng_env_setup();
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_taus2);
    const unsigned int seed = (argc == 2)? atof(argv[1]) : time(NULL);
    gsl_rng_set(rng, seed);

    // Parameters:
    const int T = 1000;
    const double A0 = 0.5;
    const double A1 = 0.025;
    const double A2 = 0.2;
    const int N = 1000;
    const int TWO_N = N * N;

    // Allocate memory for the matrix:
    int **m = (int**)malloc(N * sizeof(int*));
    int i;
    for (i = 0; i < N; ++i)
    {
        m[i] = (int*)malloc(N * sizeof(int));
    }

    // Fill the matrix:
    int x0, y0;
    for (x0 = 0; x0 < N; ++x0)
    {
        for (y0 = 0; y0 < N; ++y0)
        {
            m[x0][y0] = (gsl_rng_uniform(rng) < 0.5) ? 0 : 2;
        }
    }

    // Main loop
    int n, x, y, x1, y1, t, cell;
    for (t = 0; t < T; ++t)
    {
        // Loop around the lattice at random to update cells;
        for (cell = 0; cell < TWO_N; ++cell)
        {
            // Select at random the cell to update
            x0 = (int)(gsl_rng_uniform(rng) * N);
            y0 = (int)(gsl_rng_uniform(rng) * N);

            // Update the mussels
            // Disturbed cells automatically transformed into empty cells
            if (m[x0][y0] == 0)
            {
                m[x0][y0] = 1;
            }
            else if (m[x0][y0] == 1) // If the cell is empty, test if there is colonization
            {
                // First calculate the number of neighbours
                n = 0;
                for (x = -1; x <= 1; ++x)
                {
                    for (y = -1; y <= 1; ++y)
                    {
                        if (x0 + x == -1)
                        {
                            x1 = N - 1;
                        }
                        else if (x0 + x == N)
                        {
                            x1 = 0;
                        }
                        else
                        {
                            x1 = x0 + x;
                        }

                        if (y0 + y == -1)
                        {
                            y1 = N - 1;
                        }
                        else if (y0 + y == N)
                        {
                            y1 = 0;
                        }
                        else
                        {
                            y1 = y0 + y;
                        }

                        if (x != 0 && y != 0 && m[x1][y1] == 2)
                        {
                            ++n;
                        }
                    }
                }
                // Calculate if the status if the cell is changed
                if (gsl_rng_uniform(rng) < A2 * n * 0.125)
                {
                    m[x0][y0] = 2;
                }
            }
            else if (m[x0][y0] == 2)
            { // If the cell is occupied, test if there is disturbance
                n = 0;
                for (x = -1; x <= 1; x++)
                {
                    for (y = -1; y <= 1; y++)
                    {
                        if (x0 + x == -1)
                        {
                            x1 = N - 1;
                        }
                        else if (x0 + x == N)
                        {
                            x1 = 0;
                        }
                        else
                        {
                            x1 = x0 + x;
                        }

                        if (y0 + y == -1)
                        {
                            y1 = N - 1;
                        }
                        else if (y0 + y == N)
                        {
                            y1 = 0;
                        }
                        else
                        {
                            y1 = y0 + y;
                        }

                        if (x != 0 && y != 0 && m[x1][y1] == 0)
                        {
                            n = 1;
                        }
                    }
                }
                // Calculate if the status of the cell is changed
                if (gsl_rng_uniform(rng) < A0 * n * 0.125 + A1)
                {
                    m[x0][y0] = 0;
                }
            }
        }
    }

    char buffer[100];
    sprintf(buffer, "mussel-%u.txt", seed);
    FILE *out = fopen(buffer, "w");

    for (x0 = 0; x0 < N; ++x0)
    {
        for (y0 = 0; y0 < N; ++y0)
        {
            fprintf(out, "%d ", m[x0][y0]);
        }
        fprintf(out, "\n");
    }
    fclose(out);

    gsl_rng_free(rng);
    for (i = 0; i < N; ++i)
    {
        free(m[i]);
    }
    free(m);
    return EXIT_SUCCESS;
}


#ifndef PMVS3_OPTIM_H
#define PMVS3_OPTIM_H

#include <vector>
#include "patch.h"
#include <gsl/gsl_multimin.h>

namespace PMVS3 {

class CfindMatch;

class Coptim {
 public:
  Coptim(CfindMatch& findMatch);

  void init(void);

  //-----------------------------------------------------------------
  // Image manipulation
  //-----------------------------------------------------------------
  void collectImages(const int index, std::vector<int>& indexes) const;
  void addImages(Patch::Cpatch& patch) const;
  void removeImagesEdge(Patch::Cpatch& patch) const;

  float getUnit(const int index, const Vec4f& coord) const;

  void computeUnits(const Patch::Cpatch& patch,
                    std::vector<int>& indexes,
                    std::vector<float>& fineness,
                    std::vector<Vec4f>& rays) const;
  void computeUnits(const Patch::Cpatch& patch,
                    std::vector<float>& fineness) const;

  //-----------------------------------------------------------------
  // Optimization
  //-----------------------------------------------------------------

  int preProcess(Patch::Cpatch& patch, const int id, const int seed);
  void refinePatch(Patch::Cpatch& patch, const int id, const int time);

  void refinePatchBFGS(Patch::Cpatch& patch, const int id, const int time);
  void refinePatchBFGS(Patch::Cpatch& patch, const int id, const int time,
                       const int ncc);
  bool refinePatchBFGS2(Patch::Cpatch& patch, const int id, const int time,
                        const int ncc); // LM version
  void refineDepthBFGS(Patch::Cpatch& patch, const int id, const int time,
                       const int ncc);

  int postProcess(Patch::Cpatch& patch, const int id, const int seed);

  void setRefImage(Patch::Cpatch& patch, const int id);

  int check(Patch::Cpatch& patch);

  std::vector<int> m_status;

 protected:
  void filterImagesByAngle(Patch::Cpatch& patch);

  void sortImages(Patch::Cpatch& patch) const;
  void constraintImages(Patch::Cpatch& patch, const float nccThreshold,
                        const int id);
  void setRefConstraintImages(Patch::Cpatch& patch, const float nccThreshold,
                              const int id);

  void setINCCs(const Patch::Cpatch& patch,
                std::vector<float> & nccs,
                const std::vector<int>& indexes,
                const int id, const int robust);

  void setINCCs(const Patch::Cpatch& patch,
                std::vector<std::vector<float> >& nccs,
                const std::vector<int>& indexes,
                const int id, const int robust);

  int grabTex(const Vec4f& coord, const Vec4f& pxaxis, const Vec4f& pyaxis,
              const Vec4f& pzaxis, const int index, const int size,
              std::vector<float>& tex) const;

  int grabSafe(const int index, const int size, const Vec3f& center,
               const Vec3f& dx, const Vec3f& dy, const int level) const;

  double computeSSD(const Vec4f& coord, const Vec4f& normal,
                    const std::vector<int>& indexes, const int id);

  double computeSSD(const Vec4f& coord, const Vec4f& normal,
                    const std::vector<int>& indexes, const Vec4f& pxaxis,
                    const Vec4f& pyaxis, const int id);
  /*
  double computeINCC(const Vec4f& coord, const Vec4f& normal,
                     const std::vector<int>& indexes, const int id,
                     const int robust);
  */
  double computeINCC(const Vec4f& coord, const Vec4f& normal,
                     const std::vector<int>& indexes, const Vec4f& pxaxis,
                     const Vec4f& pyaxis, const int id,
                     const int robust);

 public:
  static void normalize(std::vector<float>& tex);
  static void normalize(std::vector<std::vector<float> >& texs, const int size);

  float dot(const std::vector<float>& tex0, const std::vector<float>& tex1) const;
  float ssd(const std::vector<float>& tex0, const std::vector<float>& tex1) const;
 protected:
  static void lfunc(double* p, double* hx, int m, int n, void* adata);
  void func(int m, int n, double* x, double* fvec, int* iflag, void* arg);

  //BFGS
  static double my_f(const gsl_vector *v, void *params);
  static void my_f_lm(const double *par, int m_dat, const void *data, double *fvec, int *info); // LM version
  static void my_df(const gsl_vector *v, void *params,
                    gsl_vector *df);
  static void my_fdf(const gsl_vector *x, void *params,
                     double *f, gsl_vector *df);
  // for derivative computation
  static double my_f0(double x, void* params);
  static double my_f1(double x, void* params);
  static double my_f2(double x, void* params);
  //----------------------------------------------------------------------
  // For ssd
  static double my_f_ssd(const gsl_vector *v, void *params);
  static void my_df_ssd(const gsl_vector *v, void *params,
                        gsl_vector *df);
  static void my_fdf_ssd(const gsl_vector *x, void *params,
                         double *f, gsl_vector *df);
  // for derivative computation
  static double my_f_ssd0(double x, void* params);
  static double my_f_ssd1(double x, void* params);
  static double my_f_ssd2(double x, void* params);
  //----------------------------------------------------------------------
  // For debugging depth
  static double my_f_depth(const gsl_vector *v, void *params);
  static void my_df_depth(const gsl_vector *v, void *params,
                    gsl_vector *df);
  static void my_fdf_depth(const gsl_vector *x, void *params,
                     double *f, gsl_vector *df);
  // for derivative computation
  static double my_f0_depth(double x, void* params);

  void encode(const Vec4f& coord,
              double* const vect, const int id) const;
  void encode(const Vec4f& coord, const Vec4f& normal,
              double* const vect, const int id) const;
  void decode(Vec4f& coord, Vec4f& normal,
	      const double* const vect, const int id) const;
  void decode(Vec4f& coord, const double* const vect, const int id) const;

 public:
  void setWeightsT(const Patch::Cpatch& patch, const int id);

  double computeINCC(const Vec4f& coord, const Vec4f& normal,
                     const std::vector<int>& indexes, const int id,
                     const int robust);
  void getPAxes(const int index, const Vec4f& coord, const Vec4f& normal,
                Vec4f& pxaxis, Vec4f& pyaxis) const;

  static inline float robustincc(const float rhs) {
    return rhs / (1 + 3 * rhs);
  }

  static inline float unrobustincc(const float rhs) {
    return rhs / (1 - 3 * rhs);
  }

 protected:

  void setAxesScales(void);

  static Coptim* m_one;
  CfindMatch& m_fm;

  //-----------------------------------------------------------------
  // Axes
  std::vector<Vec3f> m_xaxes;
  std::vector<Vec3f> m_yaxes;
  std::vector<Vec3f> m_zaxes;
  // Scales
  std::vector<float> m_ipscales;

  //-----------------------------------------------------------------
  // For threads
  std::vector<float> m_vect0T;
  std::vector<Vec4f> m_centersT;
  std::vector<Vec4f> m_raysT;
  std::vector<std::vector<int> > m_indexesT;
  std::vector<float> m_dscalesT;
  std::vector<float> m_ascalesT;

  // stores current parameters for derivative computation
  std::vector<Vec3f> m_paramsT;

  // Grabbed texture
  std::vector<std::vector<std::vector<float> > > m_texsT; // last is 7x7x3 patch
  // weights for refineDepthOrientationWeighed
  std::vector<std::vector<float> > m_weightsT;
  // Working array for levmar
  std::vector<std::vector<double> > m_worksT;

};
};

#endif // PMVS3_OPTIM_H


/**
 * \file Component.h
 */

#ifndef ATK_MODELLING_COMPONENT_H
#define ATK_MODELLING_COMPONENT_H

#include <ATK/Modelling/config.h>
#include <ATK/Modelling/Types.h>

#include <gsl/gsl>

#include <Eigen/Eigen>

#include <tuple>
#include <vector>

namespace ATK
{
  template<typename DataType_>
  class DynamicModellerFilter;

  enum class Linear {
    IsLinear,
    IsNotLinear
  };

  /// Base class for all components
  template<typename DataType_>
  class ATK_MODELLING_EXPORT Component
  {
  public:
    using DataType = DataType_;

  protected:
    /// Local pins to which this component is connected to
    std::vector<std::tuple<PinType, gsl::index>> pins;

    /// dt value, not useful for every component
    DataType dt;

    /// The current modeller where the component is located
    DynamicModellerFilter<DataType>* modeller;

  public:
    /// Virtual destructor
    virtual ~Component();

    Component() = default;
    Component(const Component&) = delete;
    Component& operator=(const Component&) = delete;

    virtual Linear isLinear() const;

    /**
     * sets the pins for the component
     * @params pins is the set of pins for this component
     */
    void set_pins(std::vector<std::tuple<PinType, gsl::index>> pins);

    /// Returns the pins of this component
    const std::vector<std::tuple<PinType, gsl::index>>& get_pins() const
    {
      return pins;
    }

    /**
     * Used to indicate if the modeller needs to update its set of equations with those provided by this component
     * @param modeller the modeller to update
     */
    virtual void update_model(DynamicModellerFilter<DataType>* modeller);

    /**
     * Update the component for its steady state condition
     * @param dt is the delat that will be used in following updates
     */
    virtual void update_steady_state(DataType dt);

    /**
     * Update the component for its current state condition
     */
    virtual void update_state();

    /**
     * Precompute internal value before asking current and gradients
     * @param steady_state is a flag to indcate steady state computation (used for some components)
     */
    virtual void precompute(bool steady_state);

    /**
     * Get current for the given pin based on the state
     * @param pin_index is the pin from which to compute the current
     * @param steady_state is a flag to indcate steady state computation (used for some components)
     */
    virtual DataType get_current(gsl::index pin_index, bool steady_state) const = 0;

    /**
     * Get current gradient for the given pins based on the state
     * @param pin_index_ref is the pin of the current from which the gradient is computed
     * @param pin_index is the pin from which to compute the gradient of the pin_index current
     * @param steady_state is a flag to indcate steady state computation (used for some components)
     */
    virtual DataType get_gradient(gsl::index pin_index_ref, gsl::index pin_index, bool steady_state) const = 0;

    /**
     * Add a new equation to the modeller
     * @param eq_index is equation row in the state vector and the jacobian
     * @param eq_number is the equation number to add
     * @param eqs is the state vector to update
     * @param jacobian is the jacobian to update
     * @param steady_state is a flag to indcate steady state computation (used for some components)
     */
    virtual void add_equation(gsl::index eq_index, gsl::index eq_number, Eigen::Matrix<DataType, Eigen::Dynamic, 1>& eqs, Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>& jacobian, bool steady_state) const;

    virtual gsl::index get_number_parameters() const;

    /// Get the name of a parameter
    virtual std::string get_parameter_name(gsl::index identifier) const;

    /// Get the value of a parameter
    virtual DataType_ get_parameter(gsl::index identifier) const;

    /// Set the value of a parameter
    virtual void set_parameter(gsl::index identifier, DataType_ value);
  };
}

#endif


/*
 *  ht_neuron.h
 *
 *  This file is part of NEST.
 *
 *  Copyright (C) 2004 The NEST Initiative
 *
 *  NEST is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  NEST is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with NEST.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef HT_NEURON_H
#define HT_NEURON_H

// Generated includes:
#include "config.h"

#ifdef HAVE_GSL

// C++ includes:
#include <string>
#include <vector>

// C includes:
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

// Includes from nestkernel:
#include "archiving_node.h"
#include "connection.h"
#include "recordables_map.h"
#include "ring_buffer.h"
#include "universal_data_logger.h"

// Includes from sli:
#include "stringdatum.h"

/* BeginDocumentation
   Name: ht_neuron - Neuron model after Hill & Tononi (2005).

   Description:
   This model neuron implements a slightly modified version of the
   neuron model described in [1]. The most important properties are:

   - Integrate-and-fire with threshold adaptive threshold.
   - Repolarizing potassium current instead of hard reset.
   - AMPA, NMDA, GABA_A, and GABA_B conductance-based synapses with
     beta-function (difference of exponentials) time course.
   - Voltage-dependent NMDA with instantaneous or two-stage unblocking [1, 2].
   - Intrinsic currents I_h, I_T, I_Na(p), and I_KNa.
   - Synaptic "minis" are not implemented.

   Documentation and Examples:
   - docs/model_details/HillTononiModels.ipynb
   - pynest/examples/intrinsic_currents_spiking.py
   - pynest/examples/intrinsic_currents_subthreshold.py

   Parameters:
   V_m            - membrane potential
   tau_m          - membrane time constant applying to all currents except
                    repolarizing K-current (see [1], p 1677)
   t_ref          - refractory time and duration of post-spike repolarizing
                    potassium current (t_spike in [1])
   tau_spike      - membrane time constant for post-spike repolarizing
                    potassium current
   voltage_clamp  - if true, clamp voltage to value at beginning of simulation
                    (default: false, mainly for testing)
   theta, theta_eq, tau_theta - threshold, equilibrium value, time constant
   g_KL, E_K, g_NaL, E_Na     - conductances and reversal potentials for K and
                                Na leak currents
   {E_rev,g_peak,tau_rise,tau_decay}_{AMPA,NMDA,GABA_A,GABA_B}
                                - reversal potentials, peak conductances and
                                  time constants for synapses (tau_rise/
                                  tau_decay correspond to tau_1/tau_2 in the
                                  paper)
   V_act_NMDA, S_act_NMDA, tau_Mg_{fast, slow}_NMDA
                                - parameters for voltage dependence of NMDA-
                                  conductance, see above
   instant_unblock_NMDA         - instantaneous NMDA unblocking (default: false)
   {E_rev,g_peak}_{h,T,NaP,KNa} - reversal potential and peak conductance for
                                  intrinsic currents
   tau_D_KNa                    - relaxation time constant for I_KNa
   receptor_types               - dictionary mapping synapse names to ports on
                                  neuron model
   recordables                  - list of recordable quantities
   equilibrate                  - if given and true, time-dependent activation
                                  and inactivation state variables (h, m) of
                                  intrinsic currents and NMDA channels are set
                                  to their equilibrium values during this
                                  SetStatus call; otherwise they retain their
                                  present values.

   Note: Conductances are unitless in this model and currents are in mV.

   Author: Hans Ekkehard Plesser

   Sends: SpikeEvent
   Receives: SpikeEvent, CurrentEvent, DataLoggingRequest

   FirstVersion: October 2009; full revision November 2016

   References:
   [1] S Hill and G Tononi (2005). J Neurophysiol 93:1671-1698.
   [2] M Vargas-Caballero HPC Robinson (2003). J Neurophysiol 89:2778-2783.

   SeeAlso: ht_synapse
*/

namespace nest
{
/**
 * Function computing right-hand side of ODE for GSL solver.
 * @note Must be declared here so we can befriend it in class.
 * @note Must have C-linkage for passing to GSL. Internally, it is
 *       a first-class C++ function, but cannot be a member function
 *       because of the C-linkage.
 * @note No point in declaring it inline, since it is called
 *       through a function pointer.
 * @param void* Pointer to model neuron instance.
 */
extern "C" int ht_neuron_dynamics( double, const double*, double*, void* );

class ht_neuron : public Archiving_Node
{
public:
  ht_neuron();
  ht_neuron( const ht_neuron& );
  ~ht_neuron();

  /**
   * Import sets of overloaded virtual functions.
   * @see Technical Issues / Virtual Functions: Overriding, Overloading, and
   * Hiding
   */
  using Node::handle;
  using Node::handles_test_event;

  port send_test_event( Node&, rport, synindex, bool );

  void handle( SpikeEvent& e );
  void handle( CurrentEvent& e );
  void handle( DataLoggingRequest& );

  port handles_test_event( SpikeEvent&, rport );
  port handles_test_event( CurrentEvent&, rport );
  port handles_test_event( DataLoggingRequest&, rport );

  void get_status( DictionaryDatum& ) const;
  void set_status( const DictionaryDatum& );

private:
  /**
   * Synapse types to connect to
   * @note Excluded upper and lower bounds are defined as INF_, SUP_.
   *       Excluding port 0 avoids accidental connections.
   */
  enum SynapseTypes
  {
    INF_SPIKE_RECEPTOR = 0,
    AMPA,
    NMDA,
    GABA_A,
    GABA_B,
    SUP_SPIKE_RECEPTOR
  };

  void init_state_( const Node& proto );
  void init_buffers_();
  void calibrate();

  void update( Time const&, const long, const long );

  double get_synapse_constant( double, double, double );

  // END Boilerplate function declarations ----------------------------

  // Friends --------------------------------------------------------

  // make dynamics function quasi-member
  friend int ht_neuron_dynamics( double, const double*, double*, void* );

  // ----------------------------------------------------------------

  /**
   * Independent parameters of the model.
   */
  struct Parameters_
  {
    Parameters_();

    void get( DictionaryDatum& ) const; //!< Store current values in dictionary
    void set( const DictionaryDatum& ); //!< Set values from dicitonary

    // Note: Conductances are unitless
    // Leaks
    double E_Na; // mV
    double E_K;  // mV
    double g_NaL;
    double g_KL;
    double tau_m; // ms

    // Dynamic threshold
    double theta_eq;  // mV
    double tau_theta; // ms

    // Post-spike potassium current
    double tau_spike; // ms, membrane time constant for this current
    double t_ref;     // ms, refractory time

    // Parameters for synapse of type AMPA, GABA_A, GABA_B and NMDA
    double g_peak_AMPA;
    double tau_rise_AMPA;  // ms
    double tau_decay_AMPA; // ms
    double E_rev_AMPA;     // mV

    double g_peak_NMDA;
    double tau_rise_NMDA;  // ms
    double tau_decay_NMDA; // ms
    double E_rev_NMDA;     // mV
    double V_act_NMDA;     // mV, inactive for V << Vact, inflection of sigmoid
    double S_act_NMDA;     // mV, scale of inactivation
    double tau_Mg_slow_NMDA; // ms
    double tau_Mg_fast_NMDA; // ms
    bool instant_unblock_NMDA;

    double g_peak_GABA_A;
    double tau_rise_GABA_A;  // ms
    double tau_decay_GABA_A; // ms
    double E_rev_GABA_A;     // mV

    double g_peak_GABA_B;
    double tau_rise_GABA_B;  // ms
    double tau_decay_GABA_B; // ms
    double E_rev_GABA_B;     // mV

    // parameters for intrinsic currents
    double g_peak_NaP;
    double E_rev_NaP; // mV

    double g_peak_KNa;
    double E_rev_KNa; // mV
    double tau_D_KNa; // ms

    double g_peak_T;
    double E_rev_T; // mV

    double g_peak_h;
    double E_rev_h; // mV

    bool voltage_clamp;
  };

  // ----------------------------------------------------------------

  /**
   * State variables of the model.
   */
public:
  struct State_
  {

    // y_ = [V, theta, Synapses]
    enum StateVecElems_
    {
      V_M = 0,
      THETA,
      DG_AMPA,
      G_AMPA,
      DG_NMDA_TIMECOURSE,
      G_NMDA_TIMECOURSE,
      DG_GABA_A,
      G_GABA_A,
      DG_GABA_B,
      G_GABA_B, // DO NOT INSERT ANYTHING UP TO HERE, WILL MIX UP
                // SPIKE DELIVERY
      m_fast_NMDA,
      m_slow_NMDA,
      m_Ih,
      D_IKNa,
      m_IT,
      h_IT,
      STATE_VEC_SIZE
    };

    //! neuron state, must be C-array for GSL solver
    double y_[ STATE_VEC_SIZE ];

    /** Timer (counter) for spike-activated repolarizing potassium current.
     * Neuron is absolutely refractory during this period.
     */
    long ref_steps_;

    double I_NaP_; //!< Persistent Na current; member only to allow recording
    double I_KNa_; //!< Depol act. K current; member only to allow recording
    double I_T_;   //!< Low-thresh Ca current; member only to allow recording
    double I_h_;   //!< Pacemaker current; member only to allow recording

    State_( const ht_neuron&, const Parameters_& p );
    State_( const State_& s );
    ~State_();

    State_& operator=( const State_& s );

    void get( DictionaryDatum& ) const;
    void set( const DictionaryDatum&, const ht_neuron& );
  };

private:
  // These friend declarations must be precisely here.
  friend class RecordablesMap< ht_neuron >;
  friend class UniversalDataLogger< ht_neuron >;

  // ----------------------------------------------------------------

  /**
   * Buffers of the model.
   */
  struct Buffers_
  {
    Buffers_( ht_neuron& );
    Buffers_( const Buffers_&, ht_neuron& );

    UniversalDataLogger< ht_neuron > logger_;

    /** buffers and sums up incoming spikes/currents */
    std::vector< RingBuffer > spike_inputs_;
    RingBuffer currents_;

    /** GSL ODE stuff */
    gsl_odeiv_step* s_;    //!< stepping function
    gsl_odeiv_control* c_; //!< adaptive stepsize control function
    gsl_odeiv_evolve* e_;  //!< evolution function
    gsl_odeiv_system sys_; //!< struct describing system

    // IntergrationStep_ should be reset with the neuron on ResetNetwork,
    // but remain unchanged during calibration. Since it is initialized with
    // step_, and the resolution cannot change after nodes have been created,
    // it is safe to place both here.
    double step_;             //!< step size in ms
    double integration_step_; //!< current integration time step, updated by GSL

    /**
     * Input current injected by CurrentEvent.
     * This variable is used to transport the current applied into the
     * _dynamics function computing the derivative of the state vector.
     * It must be a part of Buffers_, since it is initialized once before
     * the first simulation, but not modified before later Simulate calls.
     */
    double I_stim_;
  };

  // ----------------------------------------------------------------

  /**
   * Internal variables of the model.
   */
  struct Variables_
  {
    //! size of conductance steps for arriving spikes
    std::vector< double > cond_steps_;

    //! Duration of potassium current.
    int PotassiumRefractoryCounts_;

    //! Voltage at beginning of simulation, for clamping
    double V_clamp_;
  };

  // readout functions, can use template for vector elements
  template < State_::StateVecElems_ elem >
  double
  get_y_elem_() const
  {
    return S_.y_[ elem ];
  }
  double
  get_I_NaP_() const
  {
    return S_.I_NaP_;
  }
  double
  get_I_KNa_() const
  {
    return S_.I_KNa_;
  }
  double
  get_I_T_() const
  {
    return S_.I_T_;
  }
  double
  get_I_h_() const
  {
    return S_.I_h_;
  }

  double get_g_NMDA_() const;

  /**
   * NMDA activation for given parameters
   * Needs to take parameter values explicitly since it is called from
   * _dynamics.
   */
  double m_NMDA_( double V, double m_eq, double m_fast, double m_slow ) const;

  /**
   * Return equilibrium value of I_h activation
   *
   * @param V Membrane potential for which to evaluate
   *        (may differ from y_[V_M] when clamping)
   */
  double m_eq_h_( double V ) const;

  /**
   * Return equilibrium value of I_T activation
   *
   * @param V Membrane potential for which to evaluate
   *        (may differ from y_[V_M] when clamping)
   */
  double m_eq_T_( double V ) const;

  /**
   * Return equilibrium value of I_T inactivation
   *
   * @param V Membrane potential for which to evaluate
   *        (may differ from y_[V_M] when clamping)
   */
  double h_eq_T_( double V ) const;

  /**
   * Return steady-state magnesium unblock ratio.
   *
   * Receives V_m as argument since it is called from ht_neuron_dyamics
   * with temporary state values.
   */
  double m_eq_NMDA_( double V ) const;

  /**
   * Steady-state "D" value for given voltage.
   */
  double D_eq_KNa_( double V ) const;

  static RecordablesMap< ht_neuron > recordablesMap_;

  Parameters_ P_;
  State_ S_;
  Variables_ V_;
  Buffers_ B_;
};

inline port
ht_neuron::send_test_event( Node& target, rport receptor_type, synindex, bool )
{
  SpikeEvent e;
  e.set_sender( *this );

  return target.handles_test_event( e, receptor_type );
}

inline port
ht_neuron::handles_test_event( SpikeEvent&, rport receptor_type )
{
  assert( B_.spike_inputs_.size() == 4 );

  if ( not( INF_SPIKE_RECEPTOR < receptor_type
         && receptor_type < SUP_SPIKE_RECEPTOR ) )
  {
    throw UnknownReceptorType( receptor_type, get_name() );
    return 0;
  }
  else
  {
    return receptor_type - 1;
  }

  /*
if (receptor_type != 0)
{
  throw UnknownReceptorType(receptor_type, get_name());
}
return 0;*/
}

inline port
ht_neuron::handles_test_event( CurrentEvent&, rport receptor_type )
{
  if ( receptor_type != 0 )
  {
    throw UnknownReceptorType( receptor_type, get_name() );
  }
  return 0;
}

inline port
ht_neuron::handles_test_event( DataLoggingRequest& dlr, rport receptor_type )
{
  if ( receptor_type != 0 )
  {
    throw UnknownReceptorType( receptor_type, get_name() );
  }
  return B_.logger_.connect_logging_device( dlr, recordablesMap_ );
}
}

#endif // HAVE_GSL
#endif // HT_NEURON_H


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     mpepc_optimizer.h
 * \author   Collin Johnson and Jong Jin Park
 *
 * Declaration of MPEPCOptimizer.
 */

#ifndef MPEPC_OPTIMIZER_H
#define MPEPC_OPTIMIZER_H

#include "core/pose.h"
#include "mpepc/trajectory/params.h"
#include "mpepc/trajectory/robot_trajectory_info.h"
#include <memory>
#include <nlopt.h>
#include <vector>

namespace vulcan
{
namespace mpepc
{

const int kOptimizationDimension = 5;

const int kIdxR = 0;
const int kIdxTheta = 1;
const int kIdxDelta = 2;
const int kIdxVGain = 3;
const int kIdxK1 = 4;

/**
 * optimizer_coords_t is a coordinate of optimization space for the MPEPCOptimizer,
 * i.e. the argument to the objective function to be minimized.
 */
struct optimizer_coords_t
{
    double r;
    double theta;
    double delta;
    double velocityGain;
    double k1;
    double k2;

    explicit optimizer_coords_t(double r = 0,
                                double theta = 0,
                                double delta = 0,
                                double velocityGain = 0,
                                double k1 = 1.0,
                                double k2 = 1.0)
    : r(r)
    , theta(theta)
    , delta(delta)
    , velocityGain(velocityGain)
    , k1(k1)
    , k2(k2)
    {
    }

    explicit optimizer_coords_t(const double x[kOptimizationDimension])
    : r(x[kIdxR])
    , theta(x[kIdxTheta])
    , delta(x[kIdxDelta])
    , velocityGain(x[kIdxVGain])
    , k1(x[kIdxK1])
    {
    }

    void toArray(double x[kOptimizationDimension]) const
    {
        x[kIdxR] = r;
        x[kIdxTheta] = theta;
        x[kIdxDelta] = delta;
        x[kIdxVGain] = velocityGain;
        x[kIdxK1] = k1;
    }

    bool operator==(const optimizer_coords_t& rhs) const
    {
        return (r == rhs.r) && (theta == rhs.theta) && (delta == rhs.delta) && (velocityGain == rhs.velocityGain)
          && (k1 == rhs.k1);
    }

    bool operator!=(const optimizer_coords_t& rhs) const { return !(*this == rhs); }
};

class RobotSimulator;
class TrajectoryEvaluator;
class TaskManifold;
struct motion_target_t;
struct trajectory_planner_debug_info_t;

/**
 * MPEPCTrajectoryOptimizer handles the optimization task within the MPEPC planner.
 * The optimizer recieves the simulator and the cost evaluator to use, along with
 * the state for the current planning problem, i.e. planner_data_t, and finds the
 * time-optimal trajectory in the form of a motion target.
 *
 * The optimizer handles the simulate/evaluate loop internally and finds the optimal
 * target along with the intermediate trajectories generated by the optimization
 * algorithm. NLopt package is used for the numerical optimzation, and a number of
 * different algorithms can be implemented.
 */
class MPEPCOptimizer
{

public:
    /**
     * Constructor for MPEPCOptimizer.
     *
     * \param    params          Parameters for the optimization
     */
    MPEPCOptimizer(const mpepc_optimizer_params_t& params);

    /**
     * Destructor for MPEPCOptimizer.
     */
    ~MPEPCOptimizer(void);

    /**
     * Setup sets up the optmizer with the robot simulator and trajectory evaluator to be used.
     *
     * \param    simulator       RobotSimulator to use for the trajectory generation.
     * \param    evaluator       TrajectoryEvaluator for calculating the cost of a trajectory.
     */
    void setup(RobotSimulator& simulator,
               TrajectoryEvaluator& evaluator);   // NOTE: This feels very unsafe. A better way?

    /**
     * setInitialGuesses adds a goal pose (if navigation task) and the previous optimum
     * to the initial guesses for local optimizer.
     *
     * \param    task                 TaskManifold that contains the goal.
     * \param    previousMotionTarget Previous output of the metric planner, i.e. the previous optimum.
     */
    void setInitialGuesses(const TaskManifold& task, const motion_target_t& previousMotionTarget);

    /**
     * run runs the optimizer and finds the best motion target by evaluating the expected
     * costs of an estimated trajectory generated from a candidate motion target.
     *
     * It returns the optimal target found, while the intermediate information including
     * all trajectories considered are stored in the provided debug info instance.
     *
     * \param    debugInfo        Place to store relevant information
     * \return   Optimal target found
     */
    motion_target_t runOptimizer(trajectory_planner_debug_info_t& debugInfo);

    /**
     * evaluateCost calculates the cost of a fixed-length trajectory represented by the given optimizer_target_t.
     * The length of the trajectory is specified in the given instance of the planner_data_t.
     *
     * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a
     * velocity gain for the kinematic control law. \return   Cost of the trajectory.
     */
    double evaluateCost(const optimizer_coords_t& coords);

    // get getSurvivability of the trajectroy associated with the optimizer coords. Will assert failure if the given
    // coordinate is not the same as the one used for cost evaluation
    double getChanceConstraint(const optimizer_coords_t& coords);

    /**
     * evaluateGradient calculates the gradient around a particular point, by a simple forward differencing.
     *
     * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a
     * velocity gain for the kinematic control law. \param    cost            Evaluated cost at the given coordinate.
     * \param    gradient        Calculated gradient for each dimension (r, theta, delta, velocity) [output]
     */
    void evaluateGradient(const optimizer_coords_t& coords, double cost, double gradient[kOptimizationDimension]);

    bool haveFoundSolution(void) const { return haveFoundSolution_; };

    //     /**
    //     * evaluateSingleMotionTarget is a method which resets the data and info struct and evaluate a single
    //     optimizer target.
    //     * The optimization_info_t will hold a single trajectory considered. TODO: Do i need this?
    //     *
    //     * \param candidateMotionTarget
    //     * \param data
    //     * \param info
    //     */
    //     double evaluateSingleMotionTarget(const motion_target_t& candidateMotionTarget, const planner_data_t& data,
    //     optimization_info_t& info);

private:
    // setting up the optimizer
    void setupGlobalOptimizer(void);
    void setupLocalOptimizer(void);

    // loading a predetermined set of initial points for local optimizer
    void loadGuessesFromFile(const std::string& filename);

    // checking if the target is within a specified bounds
    bool isTargetValid(const optimizer_coords_t& target) const;

    // selecting the best inital point for the local optimizer
    // assumes there are valid trajectories stored in optimizerInfo_
    void selectBestInitialPoint(const std::vector<optimizer_coords_t>& initialGuesses,
                                double x[kOptimizationDimension]);
    std::size_t bestTrajectoryIdx(void);

    // optimizers
    nlopt_opt globalOptimizer;
    nlopt_opt localOptimizer;

    optimizer_coords_t coordEvaluated_;

    double globalOptimizerSeed_[kOptimizationDimension];

    // simulator and evaluator
    RobotSimulator* simulator_;
    TrajectoryEvaluator* evaluator_;

    robot_trajectory_info_t trajectory;   // Trajectory to use for everything -- just reset it before each use

    // debug info
    trajectory_planner_debug_info_t* optimizerInfo_;

    // a set of initial guesses for optmization
    std::vector<optimizer_coords_t> defaultGuesses_;
    std::vector<optimizer_coords_t> newGuesses_;

    // inital robot state
    pose_t robotPose_;

    // indicator for finding a right solution
    bool haveFoundSolution_;

    // parameters
    mpepc_optimizer_params_t params_;
};

}   // namespace mpepc
}   // namespace vulcan

#endif   // MPEPC_OPTIMIZER_H


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"

#include "tests.h"

void
test_hpr (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int order = 101;
   int uplo = 121;
   int N = 2;
   float alpha = 0.1f;
   float Ap[] = { -0.273f, -0.499f, -0.305f, -0.277f, 0.238f, -0.369f };
   float X[] = { 0.638f, -0.905f, 0.224f, 0.182f };
   int incX = -1;
   float Ap_expected[] = { -0.26467f, 0.0f, -0.30718f, -0.245116f, 0.360607f, 0.0f };
   cblas_chpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr(case 1418) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr(case 1418) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 2;
   float alpha = 0.1f;
   float Ap[] = { -0.273f, -0.499f, -0.305f, -0.277f, 0.238f, -0.369f };
   float X[] = { 0.638f, -0.905f, 0.224f, 0.182f };
   int incX = -1;
   float Ap_expected[] = { -0.26467f, 0.0f, -0.30718f, -0.308884f, 0.360607f, 0.0f };
   cblas_chpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr(case 1419) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr(case 1419) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 2;
   float alpha = 0.1f;
   float Ap[] = { -0.273f, -0.499f, -0.305f, -0.277f, 0.238f, -0.369f };
   float X[] = { 0.638f, -0.905f, 0.224f, 0.182f };
   int incX = -1;
   float Ap_expected[] = { -0.26467f, 0.0f, -0.30718f, -0.245116f, 0.360607f, 0.0f };
   cblas_chpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr(case 1420) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr(case 1420) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 2;
   float alpha = 0.1f;
   float Ap[] = { -0.273f, -0.499f, -0.305f, -0.277f, 0.238f, -0.369f };
   float X[] = { 0.638f, -0.905f, 0.224f, 0.182f };
   int incX = -1;
   float Ap_expected[] = { -0.26467f, 0.0f, -0.30718f, -0.308884f, 0.360607f, 0.0f };
   cblas_chpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr(case 1421) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr(case 1421) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 121;
   int N = 2;
   double alpha = 1;
   double Ap[] = { 0.265, 0.362, -0.855, 0.035, 0.136, 0.133 };
   double X[] = { -0.278, -0.686, -0.736, -0.918 };
   int incX = -1;
   double Ap_expected[] = { 1.64942, 0.0, -0.020644, -0.214692, 0.68388, 0.0 };
   cblas_zhpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr(case 1422) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr(case 1422) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 2;
   double alpha = 1;
   double Ap[] = { 0.265, 0.362, -0.855, 0.035, 0.136, 0.133 };
   double X[] = { -0.278, -0.686, -0.736, -0.918 };
   int incX = -1;
   double Ap_expected[] = { 1.64942, 0.0, -0.020644, 0.284692, 0.68388, 0.0 };
   cblas_zhpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr(case 1423) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr(case 1423) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 2;
   double alpha = 1;
   double Ap[] = { 0.265, 0.362, -0.855, 0.035, 0.136, 0.133 };
   double X[] = { -0.278, -0.686, -0.736, -0.918 };
   int incX = -1;
   double Ap_expected[] = { 1.64942, 0.0, -0.020644, -0.214692, 0.68388, 0.0 };
   cblas_zhpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr(case 1424) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr(case 1424) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 2;
   double alpha = 1;
   double Ap[] = { 0.265, 0.362, -0.855, 0.035, 0.136, 0.133 };
   double X[] = { -0.278, -0.686, -0.736, -0.918 };
   int incX = -1;
   double Ap_expected[] = { 1.64942, 0.0, -0.020644, 0.284692, 0.68388, 0.0 };
   cblas_zhpr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr(case 1425) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr(case 1425) imag");
     };
   };
  };

}


/**
 * header file for data mining sharpening (DMS)
 * written by Feng Gao on June 2012
 * v1.0_HDF: uses LEDAPS SR in HDF format (in-house LEDAPS product)
 * v1.1_ENVI: accepts GeoTIFF, ENVI and HDF format (USGS Landsat SR product)
 * v1.2_ENVI: uses Landsat cloud mask (USGS Landsat fmask product)
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <strings.h>

/* header files for GeoTIFF */
#include "geotiffio.h"
#include "xtiffio.h"
#include "tiffio.h"

/* header file for GNU Scientific Libraries */
//#include <gsl/gsl_multifit.h>

#define MIN_NSAMPLES 20
#define MAX_STRLEN   1000
#define MAX_NBANDS   10
#define EC_VALID_TH  0.5

#define SUCCESS 0
#define FAILURE -1
#define FILLV   -9999

/* define structure to hold all variables from input */
typedef struct {

  /* for original shortwave spectral bands */
  int    nbands;
  char   inFile[MAX_NBANDS][MAX_STRLEN];
  char   cloudFile[MAX_STRLEN];
  char   fileType[MAX_STRLEN];
  int    nrows;
  int    ncols;
  int    fillv;
  int    range[2];
  float  res;

  /* geolocation information (same for shortwave spectral bands and thermal band */
  float  ulx, uly;
  long   insys;
  long   inzone;
  double inparm[15];
  long   inunit;
  long   indatum;

  /* original temperature (float for binary file or byte for GeoTIFF) */
  char  org_th_File[MAX_STRLEN];
  char  org_th_fileType[MAX_STRLEN];
  int   org_th_nrows;
  int   org_th_ncols;
  float org_th_res;
  float org_th_range[2];

  /* reduced and rescaled T in coarse resolution */
  int   res_th_nrows;
  int   res_th_ncols;
  float res_th_res;

  char  cubistFile[MAX_STRLEN]; /* filestem for cubist */
  char  outFile[MAX_STRLEN];    /* sharpened T (output) */

  float CV_TH;        /* coeficient of variation threshold */
  int   SMOOTH_FLAG;  /* 1=apply smooth operation to difference map in EC application; 0=do not apply */

  /* processing window */
  int s_row;  /* start row */
  int s_col;  /* start column */
  int e_row;  /* end row */
  int e_col;  /* end column */

} INPUT_PARS;

/* define structure to store data (shortwave bands or thermal band ) */
typedef struct {
  int    nbands;
  char   fileName[MAX_NBANDS][MAX_STRLEN];
  char   fileType[MAX_STRLEN];
  int    nrows;
  int    ncols;
  int    fillValue;
  float  range[2];
  long   total_valid_data;
  double ulx;              /* x value of up-left corner */
  double uly;              /* y value of up-left corner */
  double res;              /* spatial resolution */

  long   insys;
  long   inzone;
  double inparm[15];
  long   inunit;
  long   indatum;
  float  homoTest[MAX_NBANDS];   /* homogeneous test (stdev/mean) */

  FILE   *fp[MAX_NBANDS];        /* input file pointer for binary file */
  TIFF   *fp_tiff[MAX_NBANDS];   /* input file pointer for GeoTIFF file */
  int16  **data;                 /* one row in [iband][icol] for shortwave spectral bands */
  float  **fdata;                /* whole image in [irow][icol] for thermal band in float point */
  int8   **qa;                   /* qa data for output (not used) */

  FILE   *fp_cloud;              /* for cloud mask (added on 10/2015) */
  TIFF   *fp_tiff_cloud;
  uint8  **cloud;
  int    CMASK_FLAG;

  float  min[MAX_NBANDS], max[MAX_NBANDS];
  int scale;     /* zoom in scale to the coarse resolution temperature (shortwave and thermal band may different) */

} SENSOR;

/* structure to store samples */
typedef struct {
  float ref[MAX_NBANDS];  /* shortwave bands reflectance */
  float st;               /* skin/surface temperature */
  float w;                /* weight */
} SAMPLES;

void parseParameters(char *ifile, INPUT_PARS *ipar);
void printoutParameters(INPUT_PARS *ipars);
int  getSensorMetaInfo(SENSOR *sensor, SENSOR *st, SENSOR *th, SENSOR *sth, INPUT_PARS *pars);
int  getMetaDataFromGeoTIFF(SENSOR *sensor, int iband);
int  openForWrite(SENSOR *sth);
int  loadThermal(SENSOR *th);
int  applyEC(SENSOR *spec, SENSOR *st, SENSOR *th, SENSOR *sth, INPUT_PARS *ipar);
int  savePureSamples(SENSOR *spec, SENSOR *th, INPUT_PARS *ipar);
int  extractSamples(SENSOR *spec, SENSOR *th, INPUT_PARS *ipar, SAMPLES *sam);
int  resize(SENSOR *st, SENSOR *th);
int  loadSensorRow(SENSOR *sensor, int irow);
int  cleanUpSensor(SENSOR *sensor, SENSOR *st, SENSOR *th, SENSOR *sth);
void dec2bin(unsigned short int num, int *bitpattern, int limit);
void gslRegression(SENSOR *spec, SAMPLES *sam, int NPT, double *coeff);
void localPrediction(SENSOR *spec, SENSOR *sth, INPUT_PARS *pars, double *coeff);
int  openOutput(SENSOR *sth);

void alloc_1dim_contig (void **, int, int);
void alloc_2dim_contig (void ***, int, int, int);
void alloc_3dim_contig (void ****, int, int, int, int);
void alloc_4dim_contig (void *****, int, int, int, int, int);
void free_2dim_contig  (void **);
void free_3dim_contig  (void ***);
void free_4dim_contig  (void ****);
void writeENVIHeader(SENSOR *sensor, char *fname, int nbands, int dtype);

/*#define DEBUG*/
#ifdef DEBUG
#define DEBUG_icol 450
#define DEBUG_irow 450
#endif

/*#define SAVE_COARSE_T*/


/**
 *
 * @file qwrapper_zgemm_tile.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <cblas.h>
#include "common.h"

/***************************************************************************//**
 *
 * Version of zgemm for tile storage, to avoid dependency problem when
 * computations are done within the tile. alpha and beta are passed as
 * pointers so they can depend on runtime values.
 *
 * @param[in] Alock
 *          Pointer to tile owning submatrix A.
 *
 * @param[in] Block
 *          Pointer to tile owning submatrix B.
 *
 * @param[in] Clock
 *          Pointer to tile owning submatrix C.
 *
 **/
void QUARK_CORE_zgemm_tile(Quark *quark, Quark_Task_Flags *task_flags,
                           PLASMA_enum transA, PLASMA_enum transB,
                           int m, int n, int k, int nb,
                           const PLASMA_Complex64_t *alpha, const PLASMA_Complex64_t *A, int lda,
                                                            const PLASMA_Complex64_t *B, int ldb,
                           const PLASMA_Complex64_t *beta,        PLASMA_Complex64_t *C, int ldc,
                           const PLASMA_Complex64_t *Alock,
                           const PLASMA_Complex64_t *Block,
                           const PLASMA_Complex64_t *Clock)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_zgemm_tile_quark, task_flags,
        sizeof(PLASMA_enum),              &transA, VALUE,
        sizeof(PLASMA_enum),              &transB, VALUE,
        sizeof(int),                      &m,      VALUE,
        sizeof(int),                      &n,      VALUE,
        sizeof(int),                      &k,      VALUE,
        sizeof(PLASMA_Complex64_t),       alpha,           INPUT,
        sizeof(PLASMA_Complex64_t)*nb*nb, A,               NODEP,          /* input; see Alock */
        sizeof(int),                      &lda,    VALUE,
        sizeof(PLASMA_Complex64_t)*nb*nb, B,               NODEP,          /* input; see Block */
        sizeof(int),                      &ldb,    VALUE,
        sizeof(PLASMA_Complex64_t),       beta,            INPUT,
        sizeof(PLASMA_Complex64_t)*nb*nb, C,                       NODEP,  /* inout; see Clock */
        sizeof(int),                      &ldc,    VALUE,
        sizeof(PLASMA_Complex64_t)*nb*nb, Alock,           INPUT,
        sizeof(PLASMA_Complex64_t)*nb,    Block,           INPUT,
        sizeof(PLASMA_Complex64_t)*nb,    Clock,                   INOUT,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zgemm_tile_quark = PCORE_zgemm_tile_quark
#define CORE_zgemm_tile_quark PCORE_zgemm_tile_quark
#endif
void CORE_zgemm_tile_quark(Quark *quark)
{
    PLASMA_enum transA, transB;
    int m, n, k, lda, ldb, ldc;
    const PLASMA_Complex64_t *alpha, *beta;
    const PLASMA_Complex64_t *A, *B;
    PLASMA_Complex64_t *C;

    quark_unpack_args_13( quark, transA, transB, m, n, k, alpha, A, lda, B, ldb, beta, C, ldc );
    cblas_zgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        m, n, k,
        CBLAS_SADDR(*alpha), A, lda,
                             B, ldb,
        CBLAS_SADDR(*beta),  C, ldc );
}


#ifndef AMGIF_H
#define AMGIF_H

#include <memory>
#include <utility>
#include <vector>

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "constants.h"
#include "operators_buffer.h"

using std::pair;
using std::shared_ptr;
using std::tuple;
using std::vector;

// Smart pointer to matrix

// Shorthand for template types
using VecTriplet = tuple<gsl_vector *, gsl_vector *, gsl_vector *>;
using OpBuf = OperatorBuffer;

// Compute the AM-GIF estimates
VecTriplet computeAMGIF(
        OpBuf& Cbuf,
        gsl_vector *me,
        gsl_vector *pe,
        const gsl_matrix *L,
        const double alpha,
        const double beta,
        const double tau,
        const double eps
);

#endif // AMGIF_H


#ifndef _DELTA_H_
#define _DELTA_H_

#include <vector>

using namespace std;

#include <gsl/gsl_vector.h>

class Delta{
private:
    double cdf(double);
public:
    Delta();
    double shift(double p, double s, int c, int l, int r);
    double bigshift(double p, double s, int c, int l, int r);
    vector<double> delta(int, int, int);
};

class OptimizeShift{
private:
    Delta * delta;
    int maxiterations;
    double stoppingprecision;
    int c,l,r;
    double GetShift(const gsl_vector * variables);
    static double GetShift_gsl(const gsl_vector * variables, void *obj);
public:
    OptimizeShift(Delta *);
    vector<double> optimize_shift();
    void setCLR(int,int,int);
};

#endif /* _DELTA_H_ */


/* multifit/multireg.c
 *
 * Copyright (C) 2015 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * References:
 *
 * [1] P. C. Hansen & D. P. O'Leary, "The use of the L-curve in
 * the regularization of discrete ill-posed problems",  SIAM J. Sci.
 * Comput. 14 (1993), pp. 1487-1503.
 *
 * [2] P. C. Hansen, "Discrete Inverse Problems: Insight and Algorithms,"
 * SIAM Press, 2010.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "linear_common.c"

int
gsl_multifit_linear_solve (const double lambda,
                           const gsl_matrix * X,
                           const gsl_vector * y,
                           gsl_vector * c,
                           double *rnorm,
                           double *snorm,
                           gsl_multifit_linear_workspace * work)
{
  size_t rank;
  int status;

  status = multifit_linear_solve(X, y, GSL_DBL_EPSILON, lambda, &rank, c,
                                 rnorm, snorm, work);

  return status;
} /* gsl_multifit_linear_solve() */

/*
gsl_multifit_linear_applyW()
  Apply weight matrix to (X,y) LS system

Inputs: X    - least squares matrix n-by-p
        w    - weight vector n-by-1 or NULL for W = I
        y    - right hand side n-by-1
        WX   - (output) sqrt(W) X, n-by-p
        Wy   - (output) sqrt(W) y, n-by-1

Notes:
1) If w = NULL, on output WX = X and Wy = y
2) It is allowed for WX = X and Wy = y for in-place transform
*/

int
gsl_multifit_linear_applyW(const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           gsl_matrix * WX,
                           gsl_vector * Wy)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weight vector does not match X", GSL_EBADLEN);
    }
  else if (n != WX->size1 || p != WX->size2)
    {
      GSL_ERROR("WX matrix dimensions do not match X", GSL_EBADLEN);
    }
  else if (n != Wy->size)
    {
      GSL_ERROR("Wy vector must be length n", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* copy WX = X; Wy = y if distinct pointers */
      if (WX != X)
        gsl_matrix_memcpy(WX, X);
      if (Wy != y)
        gsl_vector_memcpy(Wy, y);

      if (w != NULL)
        {
          /* construct WX = sqrt(W) X and Wy = sqrt(W) y */
          for (i = 0; i < n; ++i)
            {
              double wi = gsl_vector_get(w, i);
              double swi;
              gsl_vector_view row = gsl_matrix_row(WX, i);
              double *yi = gsl_vector_ptr(Wy, i);

              if (wi < 0.0)
                wi = 0.0;

              swi = sqrt(wi);
              gsl_vector_scale(&row.vector, swi);
              *yi *= swi;
            }
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_wstdform1()
  Using regularization matrix
L = diag(l_1,l_2,...,l_p), transform to Tikhonov standard form:

X~ = sqrt(W) X L^{-1}
y~ = sqrt(W) y
c~ = L c

Inputs: L    - Tikhonov matrix as a vector of diagonal elements p-by-1;
               or NULL for L = I
        X    - least squares matrix n-by-p
        y    - right hand side vector n-by-1
        w    - weight vector n-by-1; or NULL for W = I
        Xs   - least squares matrix in standard form X~ n-by-p
        ys   - right hand side vector in standard form y~ n-by-1
        work - workspace

Return: success/error

Notes:
1) It is allowed for X = Xs and y = ys
*/

int
gsl_multifit_linear_wstdform1 (const gsl_vector * L,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("observation matrix larger than workspace", GSL_EBADLEN);
    }
  else if (L != NULL && p != L->size)
    {
      GSL_ERROR("L vector does not match X", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weight vector does not match X", GSL_EBADLEN);
    }
  else if (n != Xs->size1 || p != Xs->size2)
    {
      GSL_ERROR("Xs matrix dimensions do not match X", GSL_EBADLEN);
    }
  else if (n != ys->size)
    {
      GSL_ERROR("ys vector must be length n", GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;

      /* compute Xs = sqrt(W) X and ys = sqrt(W) y */
      status = gsl_multifit_linear_applyW(X, w, y, Xs, ys);
      if (status)
        return status;

      if (L != NULL)
        {
          size_t j;

          /* construct X~ = sqrt(W) X * L^{-1} matrix */
          for (j = 0; j < p; ++j)
            {
              gsl_vector_view Xj = gsl_matrix_column(Xs, j);
              double lj = gsl_vector_get(L, j);

              if (lj == 0.0)
                {
                  GSL_ERROR("L matrix is singular", GSL_EDOM);
                }

              gsl_vector_scale(&Xj.vector, 1.0 / lj);
            }
        }

      return status;
    }
}

/*
gsl_multifit_linear_stdform1()
  Using regularization matrix L = diag(l_1,l_2,...,l_p),
and W = I, transform to Tikhonov standard form:

X~ = X L^{-1}
y~ = y
c~ = L c

Inputs: L    - Tikhonov matrix as a vector of diagonal elements p-by-1
        X    - least squares matrix n-by-p
        y    - right hand side vector n-by-1
        Xs   - least squares matrix in standard form X~ n-by-p
        ys   - right hand side vector in standard form y~ n-by-1
        work - workspace

Return: success/error

Notes:
1) It is allowed for X = Xs
*/

int
gsl_multifit_linear_stdform1 (const gsl_vector * L,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wstdform1(L, X, NULL, y, Xs, ys, work);

  return status;
}

int
gsl_multifit_linear_L_decomp (gsl_matrix * L, gsl_vector * tau)
{
  const size_t m = L->size1;
  const size_t p = L->size2;
  int status;

  if (tau->size != GSL_MIN(m, p))
    {
      GSL_ERROR("tau vector must be min(m,p)", GSL_EBADLEN);
    }
  else if (m >= p)
    {
      /* square or tall L matrix */
      status = gsl_linalg_QR_decomp(L, tau);
      return status;
    }
  else
    {
      /* more columns than rows, compute qr(L^T) */
      gsl_matrix_view LTQR = gsl_matrix_view_array(L->data, p, m);
      gsl_matrix *LT = gsl_matrix_alloc(p, m);

      /* XXX: use temporary storage due to difficulties in transforming
       * a rectangular matrix in-place */
      gsl_matrix_transpose_memcpy(LT, L);
      gsl_matrix_memcpy(&LTQR.matrix, LT);
      gsl_matrix_free(LT);

      status = gsl_linalg_QR_decomp(&LTQR.matrix, tau);

      return status;
    }
}

/*
gsl_multifit_linear_wstdform2()
  Using regularization matrix L which is m-by-p, transform to Tikhonov
standard form. This routine is separated into two cases:

Case 1: m >= p, here we can use the QR decomposition of L = QR, and note
that ||L c|| = ||R c|| where R is p-by-p. Therefore,

X~ = X R^{-1} is n-by-p
y~ = y is n-by-1
c~ is p-by-1
M is not used

Case 2: m < p

X~ is (n - p + m)-by-m
y~ is (n - p + m)-by-1
c~ is m-by-1
M is n-by-p (workspace)

Inputs: LQR  - output from gsl_multifit_linear_L_decomp()
        Ltau - output from gsl_multifit_linear_L_decomp()
        X    - least squares matrix n-by-p
        w    - weight vector n-by-1; or NULL for W = I
        y    - right hand side vector n-by-1
        Xs   - (output) least squares matrix in standard form
               case 1: n-by-p
               case 2: (n - p + m)-by-m
        ys   - (output) right hand side vector in standard form
               case 1: n-by-1
               case 2: (n - p + m)-by-1
        M    - (output) workspace matrix needed to reconstruct solution vector
               case 1: not used
               case 2: n-by-p
        work - workspace

Return: success/error

Notes:
1) If m >= p, on output:
   Xs = X R^{-1}
   ys = y

2) If m < p, on output:
   M(:,1:pm) contains QR decomposition of A * K_o, needed to reconstruct
   solution vector, where pm = p - m; M(:,p) contains Householder scalars
*/

int
gsl_multifit_linear_wstdform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_matrix * M,
                               gsl_multifit_linear_workspace * work)
{
  const size_t m = LQR->size1;
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("observation matrix larger than workspace", GSL_EBADLEN);
    }
  else if (p != LQR->size2)
    {
      GSL_ERROR("LQR and X matrices have different numbers of columns", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weights vector must be length n", GSL_EBADLEN);
    }
  else if (m >= p) /* square or tall L matrix */
    {
      /* the sizes of Xs and ys depend on whether m >= p or m < p */
      if (n != Xs->size1 || p != Xs->size2)
        {
          GSL_ERROR("Xs matrix must be n-by-p", GSL_EBADLEN);
        }
      else if (n != ys->size)
        {
          GSL_ERROR("ys vector must have length n", GSL_EBADLEN);
        }
      else
        {
          int status;
          size_t i;
          gsl_matrix_const_view R = gsl_matrix_const_submatrix(LQR, 0, 0, p, p);

          /* compute Xs = sqrt(W) X and ys = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, Xs, ys);
          if (status)
            return status;

          /* compute X~ = X R^{-1} using QR decomposition of L */
          for (i = 0; i < n; ++i)
            {
              gsl_vector_view v = gsl_matrix_row(Xs, i);

              /* solve: R^T y = X_i */
              gsl_blas_dtrsv(CblasUpper, CblasTrans, CblasNonUnit, &R.matrix, &v.vector);
            }

          return GSL_SUCCESS;
        }
    }
  else /* L matrix with m < p */
    {
      const size_t pm = p - m;
      const size_t npm = n - pm;

      /*
       * This code closely follows section 2.6.1 of Hansen's
       * "Regularization Tools" manual
       */

      if (npm != Xs->size1 || m != Xs->size2)
        {
          GSL_ERROR("Xs matrix must be (n-p+m)-by-m", GSL_EBADLEN);
        }
      else if (npm != ys->size)
        {
          GSL_ERROR("ys vector must be of length (n-p+m)", GSL_EBADLEN);
        }
      else if (n != M->size1 || p != M->size2)
        {
          GSL_ERROR("M matrix must be n-by-p", GSL_EBADLEN);
        }
      else
        {
          int status;
          gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
          gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);

          gsl_matrix_view LTQR = gsl_matrix_view_array(LQR->data, p, m);           /* qr(L^T) */
          gsl_matrix_view Rp = gsl_matrix_view_array(LQR->data, m, m);             /* R factor of L^T */
          gsl_vector_const_view LTtau = gsl_vector_const_subvector(Ltau, 0, m);

          /*
           * M(:,1:p-m) will hold QR decomposition of A K_o; M(:,p) will hold
           * Householder scalars
           */
          gsl_matrix_view MQR = gsl_matrix_submatrix(M, 0, 0, n, pm);
          gsl_vector_view Mtau = gsl_matrix_subcolumn(M, p - 1, 0, GSL_MIN(n, pm));

          gsl_matrix_view AKo, AKp, HqTAKp;
          gsl_vector_view v;
          size_t i;

          /* compute A = sqrt(W) X and b = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
          if (status)
            return status;

          /* compute: A <- A K = [ A K_p ; A K_o ] */
          gsl_linalg_QR_matQ(&LTQR.matrix, &LTtau.vector, &A.matrix);
          AKp = gsl_matrix_submatrix(&A.matrix, 0, 0, n, m);
          AKo = gsl_matrix_submatrix(&A.matrix, 0, m, n, pm);

          /* compute QR decomposition [H,T] = qr(A * K_o) and store in M */
          gsl_matrix_memcpy(&MQR.matrix, &AKo.matrix);
          gsl_linalg_QR_decomp(&MQR.matrix, &Mtau.vector);

          /* AKp currently contains A K_p; apply H^T from the left to get H^T A K_p */
          gsl_linalg_QR_QTmat(&MQR.matrix, &Mtau.vector, &AKp.matrix);

          /* the last npm rows correspond to H_q^T A K_p */
          HqTAKp = gsl_matrix_submatrix(&AKp.matrix, pm, 0, npm, m);

          /* solve: Xs R_p^T = H_q^T A K_p for Xs */
          gsl_matrix_memcpy(Xs, &HqTAKp.matrix);
          for (i = 0; i < npm; ++i)
            {
              gsl_vector_view x = gsl_matrix_row(Xs, i);
              gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &Rp.matrix, &x.vector);
            }

          /*
           * compute: ys = H_q^T b; this is equivalent to computing
           * the last q elements of H^T b (q = npm)
           */
          v = gsl_vector_subvector(&b.vector, pm, npm);
          gsl_linalg_QR_QTvec(&MQR.matrix, &Mtau.vector, &b.vector);
          gsl_vector_memcpy(ys, &v.vector);

          return GSL_SUCCESS;
        }
    }
}

int
gsl_multifit_linear_stdform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_matrix * M,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wstdform2(LQR, Ltau, X, NULL, y, Xs, ys, M, work);

  return status;
}

/*
gsl_multifit_linear_genform1()
  Backtransform regularized solution vector using matrix
L = diag(L)
*/

int
gsl_multifit_linear_genform1 (const gsl_vector * L,
                              const gsl_vector * cs,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work)
{
  if (L->size > work->pmax)
    {
      GSL_ERROR("L vector does not match workspace", GSL_EBADLEN);
    }
  else if (L->size != cs->size)
    {
      GSL_ERROR("cs vector does not match L", GSL_EBADLEN);
    }
  else if (L->size != c->size)
    {
      GSL_ERROR("c vector does not match L", GSL_EBADLEN);
    }
  else
    {
      /* compute true solution vector c = L^{-1} c~ */
      gsl_vector_memcpy(c, cs);
      gsl_vector_div(c, L);

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_wgenform2()
  Backtransform regularized solution vector in standard form to recover
original vector

Inputs: LQR  - output from gsl_multifit_linear_L_decomp()
        Ltau - output from gsl_multifit_linear_L_decomp()
        X    - original least squares matrix n-by-p
        w    - original weight vector n-by-1 or NULL for W = I
        y    - original rhs vector n-by-1
        cs   - standard form solution vector
        c    - (output) original solution vector p-by-1
        M    - matrix computed by gsl_multifit_linear_wstdform2()
        work - workspace
*/

int
gsl_multifit_linear_wgenform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               const gsl_vector * cs,
                               const gsl_matrix * M,
                               gsl_vector * c,
                               gsl_multifit_linear_workspace * work)
{
  const size_t m = LQR->size1;
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("X matrix does not match workspace", GSL_EBADLEN);
    }
  else if (p != LQR->size2)
    {
      GSL_ERROR("LQR matrix does not match X", GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR("c vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("w vector does not match X", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (m >= p)                    /* square or tall L matrix */
    {
      if (p != cs->size)
        {
          GSL_ERROR("cs vector must be length p", GSL_EBADLEN);
        }
      else
        {
          int s;
          gsl_matrix_const_view R = gsl_matrix_const_submatrix(LQR, 0, 0, p, p); /* R factor of L */

          /* solve R c = cs for true solution c, using QR decomposition of L */
          gsl_vector_memcpy(c, cs);
          s = gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &R.matrix, c);

          return s;
        }
    }
  else                                /* rectangular L matrix with m < p */
    {
      if (m != cs->size)
        {
          GSL_ERROR("cs vector must be length m", GSL_EBADLEN);
        }
      else if (n != M->size1 || p != M->size2)
        {
          GSL_ERROR("M matrix must be size n-by-p", GSL_EBADLEN);
        }
      else
        {
          int status;
          const size_t pm = p - m;
          gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
          gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);
          gsl_matrix_view Rp = gsl_matrix_view_array(LQR->data, m, m); /* R_p */
          gsl_matrix_view LTQR = gsl_matrix_view_array(LQR->data, p, m);
          gsl_vector_const_view LTtau = gsl_vector_const_subvector(Ltau, 0, m);
          gsl_matrix_const_view MQR = gsl_matrix_const_submatrix(M, 0, 0, n, pm);
          gsl_vector_const_view Mtau = gsl_matrix_const_subcolumn(M, p - 1, 0, GSL_MIN(n, pm));
          gsl_matrix_const_view To = gsl_matrix_const_submatrix(&MQR.matrix, 0, 0, pm, pm);
          gsl_vector_view workp = gsl_vector_subvector(work->xt, 0, p);
          gsl_vector_view v1, v2;

          /* compute A = sqrt(W) X and b = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
          if (status)
            return status;

          /* initialize c to zero */
          gsl_vector_set_zero(c);

          /* compute c = L_inv cs = K_p R_p^{-T} cs */

          /* set c(1:m) = R_p^{-T} cs */
          v1 = gsl_vector_subvector(c, 0, m);
          gsl_vector_memcpy(&v1.vector, cs);
          gsl_blas_dtrsv(CblasUpper, CblasTrans, CblasNonUnit, &Rp.matrix, &v1.vector);

          /* c <- K R_p^{-T} cs = [ K_p R_p^{_T} cs ; 0 ] */
          gsl_linalg_QR_Qvec(&LTQR.matrix, &LTtau.vector, c);

          /* compute: b1 = b - A L_inv cs */
          gsl_blas_dgemv(CblasNoTrans, -1.0, &A.matrix, c, 1.0, &b.vector);

          /* compute: b2 = H^T b1 */
          gsl_linalg_QR_QTvec(&MQR.matrix, &Mtau.vector, &b.vector);

          /* compute: b3 = T_o^{-1} b2 */
          v1 = gsl_vector_subvector(&b.vector, 0, pm);
          gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &To.matrix, &v1.vector);

          /* compute: b4 = K_o b3 */
          gsl_vector_set_zero(&workp.vector);
          v2 = gsl_vector_subvector(&workp.vector, m, pm);
          gsl_vector_memcpy(&v2.vector, &v1.vector);
          gsl_linalg_QR_Qvec(&LTQR.matrix, &LTtau.vector, &workp.vector);

          /* final solution vector */
          gsl_vector_add(c, &workp.vector);

          return GSL_SUCCESS;
        }
    }
}

int
gsl_multifit_linear_genform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              const gsl_vector * cs,
                              const gsl_matrix * M,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wgenform2(LQR, Ltau, X, NULL, y, cs, M, c, work);

  return status;
}

/*
gsl_multifit_linear_lreg()
  Calculate regularization parameters to use in L-curve
analysis

Inputs: smin      - smallest singular value of LS system
        smax      - largest singular value of LS system > 0
        reg_param - (output) vector of regularization parameters
                    derived from singular values

Return: success/error
*/

int
gsl_multifit_linear_lreg (const double smin, const double smax,
                          gsl_vector * reg_param)
{
  if (smax <= 0.0)
    {
      GSL_ERROR("smax must be positive", GSL_EINVAL);
    }
  else
    {
      const size_t N = reg_param->size;

      /* smallest regularization parameter */
      const double smin_ratio = 16.0 * GSL_DBL_EPSILON;
      const double new_smin = GSL_MAX(smin, smax*smin_ratio);
      double ratio;
      size_t i;

      gsl_vector_set(reg_param, N - 1, new_smin);

      /* ratio so that reg_param(1) = s(1) */
      ratio = pow(smax / new_smin, 1.0 / ((double)N - 1.0));

      /* calculate the regularization parameters */
      for (i = N - 1; i > 0 && i--; )
        {
          double rp1 = gsl_vector_get(reg_param, i + 1);
          gsl_vector_set(reg_param, i, ratio * rp1);
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_lcurve()
  Calculate L-curve using regularization parameters estimated
from singular values of least squares matrix

Inputs: y         - right hand side vector
        reg_param - (output) vector of regularization parameters
                    derived from singular values
        rho       - (output) vector of residual norms ||y - X c||
        eta       - (output) vector of solution norms ||lambda c||
        work      - workspace

Return: success/error

Notes:
1) SVD of X must be computed first by calling multifit_linear_svd();
   work->n and work->p are initialized by this function
*/

int
gsl_multifit_linear_lcurve (const gsl_vector * y,
                            gsl_vector * reg_param,
                            gsl_vector * rho, gsl_vector * eta,
                            gsl_multifit_linear_workspace * work)
{
  const size_t n = y->size;
  const size_t N = rho->size; /* number of points on L-curve */

  if (n != work->n)
    {
      GSL_ERROR("y vector does not match workspace", GSL_EBADLEN);
    }
  else if (N < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (N != eta->size)
    {
      GSL_ERROR ("size of rho and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else if (reg_param->size != eta->size)
    {
      GSL_ERROR ("size of reg_param and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;
      const size_t p = work->p;

      size_t i, j;

      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_vector_view S = gsl_vector_subvector(work->S, 0, p);
      gsl_vector_view xt = gsl_vector_subvector(work->xt, 0, p);
      gsl_vector_view workp = gsl_matrix_subcolumn(work->QSI, 0, 0, p);
      gsl_vector_view workp2 = gsl_vector_subvector(work->D, 0, p); /* D isn't used for regularized problems */

      const double smax = gsl_vector_get(&S.vector, 0);
      const double smin = gsl_vector_get(&S.vector, p - 1);

      double dr; /* residual error from projection */
      double normy = gsl_blas_dnrm2(y);
      double normUTy;

      /* compute projection xt = U^T y */
      gsl_blas_dgemv (CblasTrans, 1.0, &A.matrix, y, 0.0, &xt.vector);

      normUTy = gsl_blas_dnrm2(&xt.vector);
      dr = normy*normy - normUTy*normUTy;

      /* calculate regularization parameters */
      gsl_multifit_linear_lreg(smin, smax, reg_param);

      for (i = 0; i < N; ++i)
        {
          double lambda = gsl_vector_get(reg_param, i);
          double lambda_sq = lambda * lambda;

          for (j = 0; j < p; ++j)
            {
              double sj = gsl_vector_get(&S.vector, j);
              double xtj = gsl_vector_get(&xt.vector, j);
              double f = sj / (sj*sj + lambda_sq);

              gsl_vector_set(&workp.vector, j, f * xtj);
              gsl_vector_set(&workp2.vector, j, (1.0 - sj*f) * xtj);
            }

          gsl_vector_set(eta, i, gsl_blas_dnrm2(&workp.vector));
          gsl_vector_set(rho, i, gsl_blas_dnrm2(&workp2.vector));
        }

      if (n > p && dr > 0.0)
        {
          /* add correction to residual norm (see eqs 6-7 of [1]) */
          for (i = 0; i < N; ++i)
            {
              double rhoi = gsl_vector_get(rho, i);
              double *ptr = gsl_vector_ptr(rho, i);

              *ptr = sqrt(rhoi*rhoi + dr);
            }
        }

      /* restore D to identity matrix */
      gsl_vector_set_all(work->D, 1.0);

      return status;
    }
} /* gsl_multifit_linear_lcurve() */

/*
gsl_multifit_linear_lcorner()
  Determine point on L-curve of maximum curvature. For each
set of 3 points on the L-curve, the circle which passes through
the 3 points is computed. The radius of the circle is then used
as an estimate of the curvature at the middle point. The point
with maximum curvature is then selected.

Inputs: rho - vector of residual norms ||A x - b||
        eta - vector of solution norms ||L x||
        idx - (output) index i such that
              (log(rho(i)),log(eta(i))) is the point of
              maximum curvature

Return: success/error
*/

int
gsl_multifit_linear_lcorner(const gsl_vector *rho,
                            const gsl_vector *eta,
                            size_t *idx)
{
  const size_t n = rho->size;

  if (n < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (n != eta->size)
    {
      GSL_ERROR ("size of rho and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int s = GSL_SUCCESS;
      size_t i;
      double x1, y1;      /* first point of triangle on L-curve */
      double x2, y2;      /* second point of triangle on L-curve */
      double rmin = -1.0; /* minimum radius of curvature */

      /* initial values */
      x1 = log(gsl_vector_get(rho, 0));
      y1 = log(gsl_vector_get(eta, 0));

      x2 = log(gsl_vector_get(rho, 1));
      y2 = log(gsl_vector_get(eta, 1));

      for (i = 1; i < n - 1; ++i)
        {
          /*
           * The points (x1,y1), (x2,y2), (x3,y3) are the previous,
           * current, and next point on the L-curve. We will find
           * the circle which fits these 3 points and take its radius
           * as an estimate of the curvature at this point.
           */
          double x3 = log(gsl_vector_get(rho, i + 1));
          double y3 = log(gsl_vector_get(eta, i + 1));

          double x21 = x2 - x1;
          double y21 = y2 - y1;
          double x31 = x3 - x1;
          double y31 = y3 - y1;
          double h21 = x21*x21 + y21*y21;
          double h31 = x31*x31 + y31*y31;
          double d = fabs(2.0 * (x21*y31 - x31*y21));
          double r = sqrt(h21*h31*((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2))) / d;

          /* if d =~ 0 then there are nearly colinear points */
          if (gsl_finite(r))
            {
              /* check for smallest radius of curvature */
              if (r < rmin || rmin < 0.0)
                {
                  rmin = r;
                  *idx = i;
                }
            }

          /* update previous/current L-curve values */
          x1 = x2;
          y1 = y2;
          x2 = x3;
          y2 = y3;
        }

      /* check if a minimum radius was found */
      if (rmin < 0.0)
        {
          /* possibly co-linear points */
          GSL_ERROR("failed to find minimum radius", GSL_EINVAL);
        }

      return s;
    }
} /* gsl_multifit_linear_lcorner() */

/*
gsl_multifit_linear_lcorner2()
  Determine point on L-curve (lambda^2, ||c||^2) of maximum curvature.
For each set of 3 points on the L-curve, the circle which passes through
the 3 points is computed. The radius of the circle is then used
as an estimate of the curvature at the middle point. The point
with maximum curvature is then selected.

This routine is based on the paper

M. Rezghi and S. M. Hosseini, "A new variant of L-curve for Tikhonov
regularization", J. Comp. App. Math., 231 (2009).

Inputs: reg_param - vector of regularization parameters
        eta       - vector of solution norms ||L x||
        idx       - (output) index i such that
                    (lambda(i)^2,eta(i)^2) is the point of
                    maximum curvature

Return: success/error
*/

int
gsl_multifit_linear_lcorner2(const gsl_vector *reg_param,
                             const gsl_vector *eta,
                             size_t *idx)
{
  const size_t n = reg_param->size;

  if (n < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (n != eta->size)
    {
      GSL_ERROR ("size of reg_param and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int s = GSL_SUCCESS;
      size_t i;
      double x1, y1;      /* first point of triangle on L-curve */
      double x2, y2;      /* second point of triangle on L-curve */
      double rmin = -1.0; /* minimum radius of curvature */

      /* initial values */
      x1 = gsl_vector_get(reg_param, 0);
      x1 *= x1;
      y1 = gsl_vector_get(eta, 0);
      y1 *= y1;

      x2 = gsl_vector_get(reg_param, 1);
      x2 *= x2;
      y2 = gsl_vector_get(eta, 1);
      y2 *= y2;

      for (i = 1; i < n - 1; ++i)
        {
          /*
           * The points (x1,y1), (x2,y2), (x3,y3) are the previous,
           * current, and next point on the L-curve. We will find
           * the circle which fits these 3 points and take its radius
           * as an estimate of the curvature at this point.
           */
          double lamip1 = gsl_vector_get(reg_param, i + 1);
          double etaip1 = gsl_vector_get(eta, i + 1);
          double x3 = lamip1 * lamip1;
          double y3 = etaip1 * etaip1;

          double x21 = x2 - x1;
          double y21 = y2 - y1;
          double x31 = x3 - x1;
          double y31 = y3 - y1;
          double h21 = x21*x21 + y21*y21;
          double h31 = x31*x31 + y31*y31;
          double d = fabs(2.0 * (x21*y31 - x31*y21));
          double r = sqrt(h21*h31*((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2))) / d;

          /* if d =~ 0 then there are nearly colinear points */
          if (gsl_finite(r))
            {
              /* check for smallest radius of curvature */
              if (r < rmin || rmin < 0.0)
                {
                  rmin = r;
                  *idx = i;
                }
            }

          /* update previous/current L-curve values */
          x1 = x2;
          y1 = y2;
          x2 = x3;
          y2 = y3;
        }

      /* check if a minimum radius was found */
      if (rmin < 0.0)
        {
          /* possibly co-linear points */
          GSL_ERROR("failed to find minimum radius", GSL_EINVAL);
        }

      return s;
    }
} /* gsl_multifit_linear_lcorner2() */

#define GSL_MULTIFIT_MAXK      100

/*
gsl_multifit_linear_L()
  Compute discrete approximation to derivative operator of order
k on a regular grid of p points, ie: L is (p-k)-by-p
*/

int
gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L)
{
  if (p <= k)
    {
      GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
    }
  else if (k >= GSL_MULTIFIT_MAXK - 1)
    {
      GSL_ERROR("derivative order k too large", GSL_EBADLEN);
    }
  else if (p - k != L->size1 || p != L->size2)
    {
      GSL_ERROR("L matrix must be (p-k)-by-p", GSL_EBADLEN);
    }
  else
    {
      double c_data[GSL_MULTIFIT_MAXK];
      gsl_vector_view cv = gsl_vector_view_array(c_data, k + 1);
      size_t i, j;

      /* zeroth derivative */
      if (k == 0)
        {
          gsl_matrix_set_identity(L);
          return GSL_SUCCESS;
        }

      gsl_matrix_set_zero(L);

      gsl_vector_set_zero(&cv.vector);
      gsl_vector_set(&cv.vector, 0, -1.0);
      gsl_vector_set(&cv.vector, 1, 1.0);

      for (i = 1; i < k; ++i)
        {
          double cjm1 = 0.0;

          for (j = 0; j < k + 1; ++j)
            {
              double cj = gsl_vector_get(&cv.vector, j);

              gsl_vector_set(&cv.vector, j, cjm1 - cj);
              cjm1 = cj;
            }
        }

      /* build L, the c_i are the entries on the diagonals */
      for (i = 0; i < k + 1; ++i)
        {
          gsl_vector_view v = gsl_matrix_superdiagonal(L, i);
          double ci = gsl_vector_get(&cv.vector, i);

          gsl_vector_set_all(&v.vector, ci);
        }

      return GSL_SUCCESS;
    }
} /* gsl_multifit_linear_Lk() */

/*
gsl_multifit_linear_Lsobolev()
  Construct Sobolev smoothing norm operator

L = [ a_0 I; a_1 L_1; a_2 L_2; ...; a_k L_k ]

by computing the Cholesky factor of L^T L

Inputs: p     - number of columns of L
        kmax  - maximum derivative order (< p)
        alpha - vector of weights; alpha_k multiplies L_k, size kmax + 1
        L     - (output) upper triangular Sobolev matrix p-by-p,
                stored in upper triangle
        work  - workspace

Notes:
1) work->Q is used to store intermediate L_k matrices
*/

int
gsl_multifit_linear_Lsobolev(const size_t p, const size_t kmax,
                             const gsl_vector *alpha, gsl_matrix *L,
                             gsl_multifit_linear_workspace *work)
{
  if (p > work->pmax)
    {
      GSL_ERROR("p is larger than workspace", GSL_EBADLEN);
    }
  else if (p <= kmax)
    {
      GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
    }
  else if (kmax + 1 != alpha->size)
    {
      GSL_ERROR("alpha must be size kmax + 1", GSL_EBADLEN);
    }
  else if (p != L->size1)
    {
      GSL_ERROR("L matrix is wrong size", GSL_EBADLEN);
    }
  else if (L->size1 != L->size2)
    {
      GSL_ERROR("L matrix is not square", GSL_ENOTSQR);
    }
  else
    {
      int s;
      size_t j, k;
      gsl_vector_view d = gsl_matrix_diagonal(L);
      const double alpha0 = gsl_vector_get(alpha, 0);

      /* initialize L to alpha0^2 I */
      gsl_matrix_set_zero(L);
      gsl_vector_add_constant(&d.vector, alpha0 * alpha0);

      for (k = 1; k <= kmax; ++k)
        {
          gsl_matrix_view Lk = gsl_matrix_submatrix(work->Q, 0, 0, p - k, p);
          double ak = gsl_vector_get(alpha, k);

          /* compute a_k L_k */
          s = gsl_multifit_linear_Lk(p, k, &Lk.matrix);
          if (s)
            return s;

          gsl_matrix_scale(&Lk.matrix, ak);

          /* LTL += L_k^T L_k */
          gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &Lk.matrix, 1.0, L);
        }

      s = gsl_linalg_cholesky_decomp(L);
      if (s)
        return s;

      /* copy Cholesky factor to upper triangle and zero out bottom */
      gsl_matrix_transpose_tricpy('L', 1, L, L);

      for (j = 0; j < p; ++j)
        {
          for (k = 0; k < j; ++k)
            gsl_matrix_set(L, j, k, 0.0);
        }

      return GSL_SUCCESS;
    }
}


#pragma once

#include <set>
#ifndef _NOGSL
#include <gsl/gsl_vector.h>
#endif
#include "Util.h"
using namespace std;

class Interface;

#ifdef _NOGSL
class gsl_vector;
#endif

class LocalState {
public:
	vector<gsl_vector*> startStates;
	vector<gsl_vector*> localSols; // local sol for different betas
	vector<double> errors;

	LocalState(int ncontrols, int nbetas) {
		for (int i = 0; i < nbetas; i++) {
			startStates.push_back(gsl_vector_alloc(ncontrols));
			localSols.push_back(gsl_vector_alloc(ncontrols));
		}
		errors.resize(nbetas);
	}
	~LocalState() {
		for (size_t i = 0; i < localSols.size(); i++) {
			gsl_vector_free(localSols[i]);
		}
		for (size_t i = 0; i < startStates.size(); i++) {
			gsl_vector_free(startStates[i]);
		}
	}

	void print() {
		cout << "beta = -1:  " << Util::print(localSols[0]) << endl;
		cout << "beta = -10: " << Util::print(localSols[1]) << endl;
		cout << "beta = -50: " << Util::print(localSols[2]) << endl;
	}
};

class OptimizationWrapper {
public:
	virtual bool optimize(Interface* inputs, gsl_vector* initState, const set<int>& constraints, int minimizeNode, bool suppressPrint, int MAX_TRIES, LocalState* localState, int level = -1) = 0;
	virtual gsl_vector* getMinState() = 0;
	virtual void randomizeCtrls(gsl_vector* x, Interface* inputs, const set<int>& constraints, int minimizeNode) = 0;
	virtual double getObjectiveVal() = 0;
	//virtual bool maximize(Interface* inputs, const gsl_vector* initState, const set<int>& assertConstraints, int minimizeNode, float beta, Predicate* pred, int predVal, int level) = 0;
};