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

    Recurrent Neural Network Language Model
    v0.95

    一部のコードは "Recurrent neural network based statistical language modeling toolkit Version 0.4a" を元にしています.

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

#include <stdio.h>
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
//#include <stddef.h>
#include <stdlib.h>
#include <string.h>

#include <iostream>
#include <string>
#include <math.h>

#include <time.h>
#include <cfloat>

#include <unordered_map>
#include <boost/unordered_map.hpp>
#include "rnnlmlib_dynamic.h"

#ifndef exp10
    #define exp10(x) pow((double)10, (x))
#endif

namespace RNNLM{

    inline void FreadAllOrDie_dyn(void* ptr, size_t size, size_t count, FILE* fo, const char* message) {/*{{{*/
        size_t read = fread(ptr, size, count, fo);
        if (read != count) {
            fprintf(
                    stderr, "ERROR: expected to read %zu elements, but read %zu elements (%s)\n",
                    count, read, message);
            exit(1);
        }
    }/*}}}*/

    void CRnnLM_dyn::ReadFRHeader(FILE* file) {/*{{{*/
        const uint64_t kVersionStepSize = 10000;
        const int kCurrentVersion = 6;
        const unsigned kMaxLayerTypeName = 64; // maximum size of layer name type in bytes (including \0)
        const std::string kDefaultLayerType = "sigmoid";
        const char* error_message = "error@reading config";

        // READ ...

        // leyer1 のサイズ
        uint64_t quazi_layer_size; //10000 *version + layer size が入っている
        FreadAllOrDie_dyn(&quazi_layer_size, sizeof(int64_t), 1, file, "failed to read layer size.");
        unsigned int layer_size = quazi_layer_size % kVersionStepSize; //
        int version = quazi_layer_size / kVersionStepSize;
        if(version > kCurrentVersion || version < kCurrentVersion){
            fprintf(stderr, "unknown version: %" PRIu64, quazi_layer_size / kVersionStepSize );
            exit(1);
        }

        // direct
        unsigned long int maxent_hash_size;
        FreadAllOrDie_dyn(&maxent_hash_size, sizeof(int64_t), 1, file, error_message);
        unsigned int maxent_order;
        FreadAllOrDie_dyn(&maxent_order, sizeof(int), 1, file, error_message);

        // nce
        real nce_lnz_ = 9; // default value
        bool use_nce_;
        FreadAllOrDie_dyn(&use_nce_, sizeof(bool), 1, file, error_message);
        use_nce = use_nce_;
        FreadAllOrDie_dyn(&nce_lnz_, sizeof(real), 1, file, error_message);
        nce_lnz = nce_lnz_;

        // 読み込むだけのパラメタ (RNNLM toolkit に関係の無いパラメタ)
        bool reverse_sentence;
        char buffer[kMaxLayerTypeName];
        int layer_count;
        int hs_arity;
        FreadAllOrDie_dyn(&reverse_sentence, sizeof(bool), 1, file, error_message);
        FreadAllOrDie_dyn(&buffer, sizeof(char), kMaxLayerTypeName, file, error_message); // 固定長
        FreadAllOrDie_dyn(&layer_count, sizeof(int), 1, file, error_message);
        FreadAllOrDie_dyn(&hs_arity, sizeof(int), 1, file, error_message);

        // 決め打ちでセットするパラメタ (faster-rnnlm には無いパラメタ)
        independent = true; // 文は独立

        // RNNLM 用のパラメタを設定
        layer0_size = vocab_size + layer_size;
        layer1_size = layer_size;
        layerc_size = 0;
        layer2_size = vocab_size;
        direct_size = maxent_hash_size;
        direct_order = maxent_order;

    }/*}}}*/

    real CRnnLM_dyn::calc_direct_score(int word, context* c){/*{{{*/
        unsigned long long hash[MAX_NGRAM_ORDER] = {};
        unsigned long long hash_max = direct_size - vocab_size;
        unsigned int a,b;
        unsigned int d_o = direct_order;
        double direct_score = 0;

        for (a=0; a<d_o; ++a) {
            b=0;
            hash[a]=PRIMES[0]*PRIMES[1];

            for (b=1; b<=a; b++){
                hash[a]+=PRIMES[(a*PRIMES[b]+b)%PRIMES_SIZE]*(unsigned long long)(c->history[b-1]+1);
            }
            hash[a]=(hash[a]%(hash_max));
        }

        for (a=0; a<d_o; ++a) {
            direct_score += (*syn_d)[(hash[a] + word)%hash_max];
            if(a>0 && c->history[a-1] == 0) break;
        }
        return direct_score;
    }/*}}}*/

    real CRnnLM_dyn::random(real min, real max)/*{{{*/
    {
        return rand()/(real)RAND_MAX*(max-min)+min;
    }/*}}}*/

    void CRnnLM_dyn::setRnnLMFile(const char *str)/*{{{*/
    {
        strcpy(rnnlm_file, str);
    }/*}}}*/

    int CRnnLM_dyn::searchVocab(char *word)
    {//{{{
        //*mmfstr = word;
        //std::string stdkey = word;
        //auto vitr = vocab_map->find(*mmfstr);
        auto vitr = vocab_map->find(shash(std::string(word)));
        if( vitr == vocab_map->end()){
            return -1;
        }else{
            return vitr->second;
        }

        return -1; //return OOV if not found
    }//}}}

    void CRnnLM_dyn::saveFullContext(context *dest)
    {//{{{
        int a;

        dest->l1_neuron.resize(layer1_size);
        dest->history.resize(MAX_NGRAM_ORDER);
        dest->last_word = history[0];
        dest->have_recurrent = false;

        //vector<real> l1_neuron = layer1_size;
        //vector<cahr> history = layer1_size;
        for (a=0; a<layer1_size; a++) dest->l1_neuron[a] = neu1[a].ac;
        for (a=0; a<MAX_NGRAM_ORDER; a++) dest->history[a] = history[a];
    }//}}}

    void CRnnLM_dyn::CacheRecurrent(context *dest)
    {//{{{
        dest->have_recurrent = true;
        for (int a=0; a<layer1_size; a++) dest->l1_neuron[a] = neu1[a].ac;
    }//}}}

    void CRnnLM_dyn::restoreFullContext(const context *dest)
    {//{{{
        //std::cerr << "restoreFullContext history_size:" << dest->history.size() << " " << (int)dest->last_word << " ," << (int)dest->history[0] << "," << (int)dest->history[1] << std::endl; //H?
        int a;
        for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
        for (a=0; a<MAX_NGRAM_ORDER; a++) history[a] = dest->history[a];
        if(dest->have_recurrent){
            for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
        }else{
            // TODO: 直接 neu0に 移したほうが速い... 下を後で試す
            for (a=0; a<layer1_size; a++) neu0[a+vocab_size].ac = dest->l1_neuron[a];

            //for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
            //copyHiddenLayerToInput(); // neu0 に移す
        }
    }//}}}

    void CRnnLM_dyn::initNetFR()
    {//{{{

        neu0=(struct neuron *)calloc(layer0_size, sizeof(struct neuron));
        neu1=(struct neuron *)calloc(layer1_size, sizeof(struct neuron));
        neu2=(struct neuron *)calloc(layer2_size, sizeof(struct neuron));

    }//}}}

    // RNNLM モデル，語彙の読み込みを行う(faster-rnnlm 対応版)
    void CRnnLM_dyn::restoreNet_FR()    //will read whole network structure
    {//{{{
        int a, b;
        float fl;

        // Filename
        const std::string model_vocab_file = rnnlm_file;
        const std::string model_weight_file = model_vocab_file + ".nnet"; // ネットワークの重み

        // 語彙 (vocab, vocab_map)
        FILE *vocab_file = fopen(model_vocab_file.c_str(), "rb");
        if (vocab_file == NULL){
            fprintf(stderr, "Error: vocaburary file %s not found. \n", model_vocab_file.c_str());
            exit(1);
        }

        std::string MapVocabFilePath= model_vocab_file+".map";
        if( access( MapVocabFilePath.c_str(), F_OK ) != -1 ){ //メモリマップからの読み込み
            if (debug_mode>0)
                std::cerr << "read mapped file" << std::endl;
            p_file_vocab = new bip::managed_mapped_file(bip::open_read_only, MapVocabFilePath.c_str());
            vocab_map = p_file_vocab->find<umap_vocab>("map_vocab").first;
        }else{ //モデルファイルの読み込み
            if (debug_mode>0)
                std::cerr << "read original file" << std::endl;

            // 大きめに1GB (10M文で85MB程度)とる(あとでshrink)
            unsigned long long map_vocab_size = 1024 * 1024 * 1024;
            // メモリマップファイルを作成
            p_file_vocab = new bip::managed_mapped_file(bip::create_only, MapVocabFilePath.c_str(), map_vocab_size );
            // メモリマップ内にvocablary 用の領域を確保
            vocab_map = p_file_vocab->construct<umap_vocab>("map_vocab")( 0, boost::hash<uint64_t>(), std::equal_to<uint64_t>(), p_file_vocab->get_allocator<hashPair>());

            // 語彙の vocab, と vocab_map への登録
            for (int line_number = 0; !feof(vocab_file); ++line_number) {
                char buffer[MAX_STRING];
                uint64_t count;
                if (fscanf(vocab_file, "%s %" PRIu64 " ", buffer, &count) != 2) {
                    fprintf(stderr, "WARNING: Skipping ill-formed line #%d in the vocabulary\n", line_number);
                    continue;
                }

                int wid = line_number;
                auto hash_key = shash(std::string(buffer));
                if(vocab_map->find(hash_key) == vocab_map->end())
                    (*vocab_map)[shash(std::string(buffer))] = wid;
                else
                    std::cerr << "Collision:" << buffer << ", " << wid << std::endl;

                if(debug_mode>1)
                    std::cerr << buffer << ", " << wid << std::endl;
            }
            // メモリマップへの書き込みを flush
            p_file_vocab->flush();
            // メモリマップのサイズ shrink
            bip::managed_mapped_file::shrink_to_fit(MapVocabFilePath.c_str());
        }
        fclose(vocab_file);

        // vocab_map を全て読み込んでから，vocab に逆向きのmap を作成(デバッグ用)
        vocab_size = vocab_map->size();
        if (debug_mode>0) {
            vocab_file = fopen(model_vocab_file.c_str(), "rb");

            if (vocab!=NULL) free(vocab);
            vocab_max_size=vocab_size+1000;
            vocab=(struct vocab_word *)calloc(vocab_max_size, sizeof(struct vocab_word));    //initialize memory for vocabulary

            for (int line_number = 0; !feof(vocab_file); ++line_number) {
                char buffer[MAX_STRING];
                uint64_t count;
                if (fscanf(vocab_file, "%s %" PRIu64 " ", buffer, &count) != 2) {
                    fprintf(stderr, "WARNING: Skipping ill-formed line #%d in the vocabulary\n", line_number);
                    continue;
                }

                auto wid = searchVocab(buffer);
                // Debug に必要であれば vocab ファイルに登録
                if(wid != -1)
                    strncpy(vocab[wid].word, buffer, strlen(buffer));
            }

            fclose(vocab_file);
        }

        // ニューラルネットワークの重み 読み出し
        FILE *model_file = fopen(model_weight_file.c_str(), "rb");
        if (model_file == NULL){
            fprintf(stderr, "Error: model file %s not found. \n", model_weight_file.c_str());
            exit(1);
        }

        ReadFRHeader(model_file);

        if (debug_mode>0)
            std::cerr << "reading model file" << std::endl;

        if (neu0==NULL) initNetFR(); //memory allocation (neu0, neu1, neu2)

        if (use_nce != true){
            std::cerr << "err:use nce" << std::endl;
            exit(1); //DEB
        }

        const std::string MapFilePath( model_vocab_file + ".nnet.map" );
        if( access( MapFilePath.c_str(), F_OK ) != -1 ){ //読み込みのみ
            p_file_syn = new bip::managed_mapped_file(bip::open_read_only, MapFilePath.c_str());
            if(debug_mode>0)
                std::cerr << "reading RNNLM model" << std::endl;
            syn_vocab_l1 = p_file_syn->find<vector_syn>("syn_vocab_l1").first;
            syn_rec      = p_file_syn->find<vector_syn>("syn_rec").first;
            syn_l1_l2    = p_file_syn->find<vector_syn>("syn_l1_l2").first;
        }else{ //通常読み込み
            unsigned long long syn_size = sizeof(real)*(vocab_size*layer1_size + layer1_size*layer1_size + layer2_size*layer1_size) + 4096;
            p_file_syn = new bip::managed_mapped_file(bip::create_only, MapFilePath.c_str(), syn_size );
            syn_vocab_l1 = p_file_syn->construct<vector_syn>("syn_vocab_l1")(p_file_syn->get_segment_manager());
            syn_rec      = p_file_syn->construct<vector_syn>("syn_rec")(p_file_syn->get_segment_manager());
            syn_l1_l2    = p_file_syn->construct<vector_syn>("syn_l1_l2")(p_file_syn->get_segment_manager());

            if(debug_mode>0)
                std::cerr << "reading embedding" << std::endl;
            syn_vocab_l1->resize(vocab_size*layer1_size);
            for (a=0; a<vocab_size; a++) { //vocab_size < layer_0.size
                for (b=0; b<layer1_size; b++) {
                    fread(&fl, sizeof(fl), 1, model_file); //real
                    (*syn_vocab_l1)[b+a*layer1_size]=fl;
                }
            }

            // TODO: 初期化．どうせ使う時初期化するはずなので，不要かも
            for (a=0; a<layer1_size; a++) {
                neu1[a].ac=0;
            }

            if(debug_mode>0)
                std::cerr << "reading NCE layer " << std::endl;
            // W:layer1 -> output (word embedding => layer1
            syn_l1_l2->resize(layer1_size*layer2_size);
            for (b=0; b<layer2_size; b++) {
                for (a=0; a<layer1_size; a++) {
                    fread(&fl, sizeof(fl), 1, model_file);
                    (*syn_l1_l2)[a+b*layer1_size]=fl;
                }
            }

            if(debug_mode>0)
                std::cerr << "reading reccurent weight" << std::endl;
            // Recurrent weight
            syn_rec->resize(layer1_size*layer1_size);
            for (b=0; b<layer1_size; b++) {
                for (a=0; a<layer1_size; a++) {
                    fread(&fl, sizeof(fl), 1, model_file);
                    (*syn_rec)[a+b*layer1_size]=fl;
                }
            }
            p_file_syn->flush();
        }

        if(debug_mode>0)
            std::cerr << "reading direct weight" << std::endl;

        const std::string FilePath( model_vocab_file + ".direct" );
        if( access( FilePath.c_str(), F_OK ) != -1 ){ //読み込みのみ
            p_file_direct = new bip::managed_mapped_file( bip::open_read_only, FilePath.c_str() );
            syn_d = p_file_direct->find<vector_syn>("MyVector").first;
            if(debug_mode>0)
                std::cerr << "read finished"<< std::endl;
        }else{
            // 初回起動時は Memory mapped file に読み込み
            p_file_direct = new bip::managed_mapped_file( bip::create_only, FilePath.c_str(), direct_size*sizeof(real)+4096 );
            syn_d = p_file_direct->construct<vector_syn>("MyVector")(p_file_direct->get_segment_manager());
            if(debug_mode>0)
                std::cerr << "Creating memory mapped file:" << std::endl;

            real fl;
            (*syn_d).resize(direct_size);
            for (unsigned long long b=0; b<direct_size; b++) {
                fread(&fl, sizeof(real), 1, model_file); //real
                (*syn_d)[b]=fl;
            }
            p_file_direct->flush();
            if(debug_mode>0)
                std::cerr << "Creating memory mapped file: finished."<< std::endl;
        }

        fclose(model_file);
        return;
    }//}}}

    void CRnnLM_dyn::matrixXvector(struct neuron *dest, struct neuron *srcvec, vector_syn *srcmatrix, int matrix_width, int from, int to, int from2, int to2, int type)
    {//{{{
        int a, b;
        real val1, val2, val3, val4;
        real val5, val6, val7, val8;

        if (type==0) {		//ac mod
            for (b=0; b<(to-from)/8; b++) {
                val1=0;
                val2=0;
                val3=0;
                val4=0;

                val5=0;
                val6=0;
                val7=0;
                val8=0;

                for (a=from2; a<to2; a++) {
                    val1 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+0)*matrix_width];
                    val2 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+1)*matrix_width];
                    val3 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+2)*matrix_width];
                    val4 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+3)*matrix_width];

                    val5 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+4)*matrix_width];
                    val6 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+5)*matrix_width];
                    val7 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+6)*matrix_width];
                    val8 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+7)*matrix_width];
                }
                dest[b*8+from+0].ac += val1;
                dest[b*8+from+1].ac += val2;
                dest[b*8+from+2].ac += val3;
                dest[b*8+from+3].ac += val4;

                dest[b*8+from+4].ac += val5;
                dest[b*8+from+5].ac += val6;
                dest[b*8+from+6].ac += val7;
                dest[b*8+from+7].ac += val8;
            }

            for (b=b*8; b<to-from; b++) {
                for (a=from2; a<to2; a++) {
                    dest[b+from].ac += srcvec[a].ac * (*srcmatrix)[a+(b+from)*matrix_width];
                }
            }
        }
        else {		//er mod
            for (a=0; a<(to2-from2)/8; a++) {
                val1=0;
                val2=0;
                val3=0;
                val4=0;

                val5=0;
                val6=0;
                val7=0;
                val8=0;

                for (b=from; b<to; b++) {
                    val1 += srcvec[b].er * (*srcmatrix)[a*8+from2+0+b*matrix_width];
                    val2 += srcvec[b].er * (*srcmatrix)[a*8+from2+1+b*matrix_width];
                    val3 += srcvec[b].er * (*srcmatrix)[a*8+from2+2+b*matrix_width];
                    val4 += srcvec[b].er * (*srcmatrix)[a*8+from2+3+b*matrix_width];

                    val5 += srcvec[b].er * (*srcmatrix)[a*8+from2+4+b*matrix_width];
                    val6 += srcvec[b].er * (*srcmatrix)[a*8+from2+5+b*matrix_width];
                    val7 += srcvec[b].er * (*srcmatrix)[a*8+from2+6+b*matrix_width];
                    val8 += srcvec[b].er * (*srcmatrix)[a*8+from2+7+b*matrix_width];
                }
                dest[a*8+from2+0].er += val1;
                dest[a*8+from2+1].er += val2;
                dest[a*8+from2+2].er += val3;
                dest[a*8+from2+3].er += val4;

                dest[a*8+from2+4].er += val5;
                dest[a*8+from2+5].er += val6;
                dest[a*8+from2+6].er += val7;
                dest[a*8+from2+7].er += val8;
            }

            for (a=a*8; a<to2-from2; a++) {
                for (b=from; b<to; b++) {
                    dest[a+from2].er += srcvec[b].er * (*srcmatrix)[a+from2+b*matrix_width];
                }
            }

            if (gradient_cutoff>0)
                for (a=from2; a<to2; a++) {
                    if (dest[a].er>gradient_cutoff) dest[a].er=gradient_cutoff;
                    if (dest[a].er<-gradient_cutoff) dest[a].er=-gradient_cutoff;
                }
        }

        //this is normal implementation (about 3x slower):

        /*if (type==0) {		//ac mod
          for (b=from; b<to; b++) {
          for (a=from2; a<to2; a++) {
          dest[b].ac += srcvec[a].ac * srcmatrix[a+b*matrix_width].weight;
          }
          }
          }
          else 		//er mod
          if (type==1) {
          for (a=from2; a<to2; a++) {
          for (b=from; b<to; b++) {
          dest[a].er += srcvec[b].er * srcmatrix[a+b*matrix_width].weight;
          }
          }
          }*/
    }//}}}

    // 今までの状態を上書きし，破壊することに注意
    void CRnnLM_dyn::computeNet_selfnm(int word, context *context)
    {//{{{
        int a, b;
        int last_word_local = context->history[0];

        // 語彙数次元のベクトル
        if (last_word_local!=-1) neu0[last_word_local].ac=1;

        // 0-(vocab)-(layer1)-layer0_size
        // 0...vocab(layer0_size-layer1_size -1) が語彙の次元
        // layer0_size-layer1_size > layer0_size がコンテクストの次元
        restoreFullContext(context); // コンテクストをneu0 のコンテクストへコピー，history をセット

        // 未知語に対しては計算しない
        if(word == -1) return;

        if(context->have_recurrent){
            // context が recurrent に含まれるなら，それを読み込むだけに留める.

        }else{
            //propagate 0->1
            for (a=0; a<layer1_size; a++) neu1[a].ac=0; //初期化

            // コンテクストvectorについての計算 context vector X reccurrent weight
            auto *syn_r = syn_rec; //syn0 +vocab_size*layer1_size;
            auto *neu_r = neu0 +vocab_size;
            matrixXvector(neu1, neu_r, syn_r, layer1_size, 0, layer1_size, 0, layer1_size, 0);

            CacheRecurrent(context);
        }

        // last_word について計算(one hot)
        if(last_word_local != -1)
            for (b=0; b<layer1_size; b++) {
                a=last_word_local;
                neu1[b].ac += (*syn_vocab_l1)[b+a*layer1_size];
            }

        // layer 1 の activation(sigmoid)
        for (a=0; a<layer1_size; a++) {
            neu1[a].ac=1/(1+std::exp(-neu1[a].ac));
        }

        //1->2 class
        double direct_score = 0.0;
        if (direct_size>0) { //RNNME を使う場合
            direct_score = calc_direct_score(word, context);
        }

        double rnn_score = 0.0;
        for (a=0; a<layer1_size; a++) {
            rnn_score += neu1[a].ac * (*syn_l1_l2)[a + word*layer1_size];
        }

        // exp 前 スコア
        neu2[word].ac = std::exp( rnn_score + direct_score - nce_lnz );

        if(debug_mode > 0)
            std::cerr << "p(" << vocab[word].word << "|" << vocab[context->history[0]].word << ", " << vocab[context->history[1]].word << ", ...) = " << neu2[word].ac << " (nn_score:" << rnn_score << " direct_score:" << direct_score << ")"<< std::endl;

    }//}}}

    void CRnnLM_dyn::copyHiddenLayerToInput()
    {//{{{
        //std::cout << "copyHiddenLayerToInput"<< std::endl;
        int a;

        for (a=0; a<layer1_size; a++) {
            neu0[a+layer0_size-layer1_size].ac=neu1[a].ac;
        }
    }//}}}

    void CRnnLM_dyn::get_initial_context_FR(context *c)
    {//{{{
        if (debug_mode>0) std::cerr << "initializing RNNLM FR" << std::endl;
        restoreNet_FR(); // initialize モデル読込  重い
        //computeNet(0, 0); // initialize
        copyHiddenLayerToInput();
        //saveContext();
        //saveContext2(); //必要？
        for (int a=0; a<MAX_NGRAM_ORDER; a++) history[a]=-1;
        history[0]=0;
        saveFullContext(c); //文頭としてInitial context を作成
    }//}}}

    real CRnnLM_dyn::test_word_selfnm(context *c, context *new_c, std::string next_word, size_t word_length)
    {//{{{
        int last_word;
        last_word = c->last_word;
        real senp;

        real lambda=1;
        real logp=0;
        senp=0; //

        int word = searchVocab((char*)next_word.c_str());

        // 文区切りを0に対応させる アドホックな対処
        if(next_word == "<EOS>" || next_word == "<BOS>")
            word = 0;

        // RNN の実行 (結果はneu2[word].ac に書き込まれる
        computeNet_selfnm(word,c);

        double ln_score = neu2[word].ac;

        if (word!=-1) { //OOVでない
            logp+=log10(ln_score);
            senp+=log10(ln_score*lambda);
        } else {
            //assign to OOVs some score to correctly rescore nbest lists, reasonable value can be less than 1/|V| or backoff LM score (in case it is trained on more data)
            //this means that PPL results from nbest list rescoring are not true probabilities anymore (as in open vocabulary LMs)

            // 文字の長さに対してlinear に設定する
            real oov_penalty=-5; //log penalty
            if(lpenalty){ //penalty を文字の長さに対して線形に与える.
               oov_penalty -= lweight * word_length;
            }

            logp+=oov_penalty;
            senp+=oov_penalty;
        }

        copyHiddenLayerToInput(); //必要？
        if (last_word!=-1) neu0[last_word].ac=0;  //delete previous activation

        // history の更新
        for (int a=MAX_NGRAM_ORDER-1; a>0; a--)
            history[a] = c->history[a-1];
        history[0] = word;

        // context の保存 // history の渡し方が暗黙的
        saveFullContext(new_c);

        return senp;
    }//}}}

}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_LARRE_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_LARRE_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The LAPACK-backend for larre is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t larre( const char range, const fortran_int_t n,
        float& vl, float& vu, const fortran_int_t il, const fortran_int_t iu,
        float* d, float* e, float* e2, const float rtol1, const float rtol2,
        const float spltol, fortran_int_t& nsplit, fortran_int_t* isplit,
        fortran_int_t& m, float* w, float* werr, float* wgap,
        fortran_int_t* iblock, fortran_int_t* indexw, float* gers,
        float& pivmin, float* work, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_SLARRE( &range, &n, &vl, &vu, &il, &iu, d, e, e2, &rtol1, &rtol2,
            &spltol, &nsplit, isplit, &m, w, werr, wgap, iblock, indexw, gers,
            &pivmin, work, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t larre( const char range, const fortran_int_t n,
        double& vl, double& vu, const fortran_int_t il,
        const fortran_int_t iu, double* d, double* e, double* e2,
        const double rtol1, const double rtol2, const double spltol,
        fortran_int_t& nsplit, fortran_int_t* isplit, fortran_int_t& m,
        double* w, double* werr, double* wgap, fortran_int_t* iblock,
        fortran_int_t* indexw, double* gers, double& pivmin, double* work,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_DLARRE( &range, &n, &vl, &vu, &il, &iu, d, e, e2, &rtol1, &rtol2,
            &spltol, &nsplit, isplit, &m, w, werr, wgap, iblock, indexw, gers,
            &pivmin, work, iwork, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to larre.
//
template< typename Value >
struct larre_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorD, typename VectorE, typename VectorE2,
            typename VectorISPLIT, typename VectorW, typename VectorWERR,
            typename VectorWGAP, typename VectorIBLOCK, typename VectorINDEXW,
            typename VectorGERS, typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const char range, const fortran_int_t n,
            real_type& vl, real_type& vu, const fortran_int_t il,
            const fortran_int_t iu, VectorD& d, VectorE& e, VectorE2& e2,
            const real_type rtol1, const real_type rtol2,
            const real_type spltol, fortran_int_t& nsplit,
            VectorISPLIT& isplit, fortran_int_t& m, VectorW& w,
            VectorWERR& werr, VectorWGAP& wgap, VectorIBLOCK& iblock,
            VectorINDEXW& indexw, VectorGERS& gers, real_type& pivmin,
            detail::workspace2< WORK, IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorE2 >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorW >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorWERR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorWGAP >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorGERS >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorISPLIT >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorIBLOCK >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorISPLIT >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorINDEXW >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorD >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorE2 >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorISPLIT >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorW >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorWERR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorWGAP >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorIBLOCK >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorINDEXW >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorGERS >::value) );
        BOOST_ASSERT( bindings::size(d) >= n );
        BOOST_ASSERT( bindings::size(e) >= n );
        BOOST_ASSERT( bindings::size(e2) >= n );
        BOOST_ASSERT( bindings::size(gers) >= 2*n );
        BOOST_ASSERT( bindings::size(indexw) >= n );
        BOOST_ASSERT( bindings::size(isplit) >= n );
        BOOST_ASSERT( bindings::size(w) >= n );
        BOOST_ASSERT( bindings::size(werr) >= n );
        BOOST_ASSERT( bindings::size(wgap) >= n );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( n ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( n ));
        BOOST_ASSERT( range == 'A' || range == 'V' || range == 'I' );
        return detail::larre( range, n, vl, vu, il, iu,
                bindings::begin_value(d), bindings::begin_value(e),
                bindings::begin_value(e2), rtol1, rtol2, spltol, nsplit,
                bindings::begin_value(isplit), m, bindings::begin_value(w),
                bindings::begin_value(werr), bindings::begin_value(wgap),
                bindings::begin_value(iblock), bindings::begin_value(indexw),
                bindings::begin_value(gers), pivmin,
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename VectorD, typename VectorE, typename VectorE2,
            typename VectorISPLIT, typename VectorW, typename VectorWERR,
            typename VectorWGAP, typename VectorIBLOCK, typename VectorINDEXW,
            typename VectorGERS >
    static std::ptrdiff_t invoke( const char range, const fortran_int_t n,
            real_type& vl, real_type& vu, const fortran_int_t il,
            const fortran_int_t iu, VectorD& d, VectorE& e, VectorE2& e2,
            const real_type rtol1, const real_type rtol2,
            const real_type spltol, fortran_int_t& nsplit,
            VectorISPLIT& isplit, fortran_int_t& m, VectorW& w,
            VectorWERR& werr, VectorWGAP& wgap, VectorIBLOCK& iblock,
            VectorINDEXW& indexw, VectorGERS& gers, real_type& pivmin,
            minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work( n ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( n ) );
        return invoke( range, n, vl, vu, il, iu, d, e, e2, rtol1, rtol2,
                spltol, nsplit, isplit, m, w, werr, wgap, iblock, indexw,
                gers, pivmin, workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename VectorD, typename VectorE, typename VectorE2,
            typename VectorISPLIT, typename VectorW, typename VectorWERR,
            typename VectorWGAP, typename VectorIBLOCK, typename VectorINDEXW,
            typename VectorGERS >
    static std::ptrdiff_t invoke( const char range, const fortran_int_t n,
            real_type& vl, real_type& vu, const fortran_int_t il,
            const fortran_int_t iu, VectorD& d, VectorE& e, VectorE2& e2,
            const real_type rtol1, const real_type rtol2,
            const real_type spltol, fortran_int_t& nsplit,
            VectorISPLIT& isplit, fortran_int_t& m, VectorW& w,
            VectorWERR& werr, VectorWGAP& wgap, VectorIBLOCK& iblock,
            VectorINDEXW& indexw, VectorGERS& gers, real_type& pivmin,
            optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( range, n, vl, vu, il, iu, d, e, e2, rtol1, rtol2,
                spltol, nsplit, isplit, m, w, werr, wgap, iblock, indexw,
                gers, pivmin, minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return 6*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return 5*n;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the larre_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for larre. Its overload differs for
// * User-defined workspace
//
template< typename VectorD, typename VectorE, typename VectorE2,
        typename VectorISPLIT, typename VectorW, typename VectorWERR,
        typename VectorWGAP, typename VectorIBLOCK, typename VectorINDEXW,
        typename VectorGERS, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
larre( const char range, const fortran_int_t n,
        typename remove_imaginary< typename bindings::value_type<
        VectorD >::type >::type& vl, typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type& vu,
        const fortran_int_t il, const fortran_int_t iu, VectorD& d,
        VectorE& e, VectorE2& e2, const typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type rtol1,
        const typename remove_imaginary< typename bindings::value_type<
        VectorD >::type >::type rtol2, const typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type spltol,
        fortran_int_t& nsplit, VectorISPLIT& isplit, fortran_int_t& m,
        VectorW& w, VectorWERR& werr, VectorWGAP& wgap, VectorIBLOCK& iblock,
        VectorINDEXW& indexw, VectorGERS& gers, typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type& pivmin,
        Workspace work ) {
    return larre_impl< typename bindings::value_type<
            VectorD >::type >::invoke( range, n, vl, vu, il, iu, d, e, e2,
            rtol1, rtol2, spltol, nsplit, isplit, m, w, werr, wgap, iblock,
            indexw, gers, pivmin, work );
}

//
// Overloaded function for larre. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename VectorD, typename VectorE, typename VectorE2,
        typename VectorISPLIT, typename VectorW, typename VectorWERR,
        typename VectorWGAP, typename VectorIBLOCK, typename VectorINDEXW,
        typename VectorGERS >
inline typename boost::disable_if< detail::is_workspace< VectorGERS >,
        std::ptrdiff_t >::type
larre( const char range, const fortran_int_t n,
        typename remove_imaginary< typename bindings::value_type<
        VectorD >::type >::type& vl, typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type& vu,
        const fortran_int_t il, const fortran_int_t iu, VectorD& d,
        VectorE& e, VectorE2& e2, const typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type rtol1,
        const typename remove_imaginary< typename bindings::value_type<
        VectorD >::type >::type rtol2, const typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type spltol,
        fortran_int_t& nsplit, VectorISPLIT& isplit, fortran_int_t& m,
        VectorW& w, VectorWERR& werr, VectorWGAP& wgap, VectorIBLOCK& iblock,
        VectorINDEXW& indexw, VectorGERS& gers, typename remove_imaginary<
        typename bindings::value_type< VectorD >::type >::type& pivmin ) {
    return larre_impl< typename bindings::value_type<
            VectorD >::type >::invoke( range, n, vl, vu, il, iu, d, e, e2,
            rtol1, rtol2, spltol, nsplit, isplit, m, w, werr, wgap, iblock,
            indexw, gers, pivmin, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


/* Copyright 2019, LAAS-CNRS
 *
 * Olivier Stasse
 *
 */
#include <sstream>
#include <iostream>

#include <example-robot-data/path.hpp>

#include <dynamic-graph/factory.h>

// Needed for sendMsg.
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>

#include <sot/torque_control/control-manager.hh>

#include <sot/core/robot-utils.hh>
#include <sot/core/debug.hh>

#define BOOST_TEST_MODULE test - control - manager

#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>

using boost::test_tools::output_test_stream;
namespace dyn_sot_tc = dynamicgraph::sot::torque_control;

BOOST_AUTO_TEST_CASE(testControlManager) {
  dgADD_OSTREAM_TO_RTLOG(std::cout);

  dyn_sot_tc::ControlManager &a_control_manager = *(dynamic_cast<dyn_sot_tc::ControlManager *>(
      dynamicgraph::FactoryStorage::getInstance()->newEntity("ControlManager", "a_control_manager")));

  a_control_manager.setLoggerVerbosityLevel(dynamicgraph::VERBOSITY_ALL);

  std::string robotRef("simple_humanoid_description");
  a_control_manager.init(0.001, EXAMPLE_ROBOT_DATA_MODEL_DIR "/simple_humanoid_description/urdf/simple_humanoid.urdf", robotRef);

  dynamicgraph::Vector av;
  a_control_manager.m_uSOUT.needUpdate(6);

  dynamicgraph::RealTimeLogger::destroy();
}


/**
* \ file GainColoredCompressorFilter.cpp
*/

#include <ATK/Dynamic/GainColoredCompressorFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainColoredCompressorFilter, softness_test)
{
  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainColoredCompressorFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainColoredCompressorFilter, color_test)
{
  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter;
  filter.set_color(0.5);
  ASSERT_EQ(filter.get_color(), 0.5);
}

TEST(GainColoredCompressorFilter, quality_test)
{
  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter;
  filter.set_quality(0.5);
  ASSERT_EQ(filter.get_quality(), 0.5);
}

TEST(GainColoredCompressorFilter, quality_range_test)
{
  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter;
  ASSERT_THROW(filter.set_quality(0), ATK::RuntimeError);
}

TEST(GainColoredCompressorFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainColoredCompressorFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1,
        outdata[i],
        0.1); // if input is zero, we still need a gain of 1 to have a progression of 1 for values < threshold
  }
}

TEST(GainColoredCompressorFilter, const_1_threshold_05_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.836990654, outdata[i], 0.1);
  }
}

TEST(GainColoredCompressorFilter, const_1_threshold_05_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.765739262, outdata[i], 0.1);
  }
}

TEST(GainColoredCompressorFilter, always_more_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = i / 1024.;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(1);
  filter.set_quality(.1);
  filter.set_color(.1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_GE(outdata[i], 1 - 10 * std::numeric_limits<float>::epsilon());
  }
}

TEST(GainColoredCompressorFilter, always_less_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = i / 1024.;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainColoredCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(1);
  filter.set_quality(.1);
  filter.set_color(-.1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_LE(outdata[i], 1 + 10 * std::numeric_limits<float>::epsilon());
  }
}


// Copyright (c) 2015-2020 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "gc_content.hpp"

#include <boost/variant.hpp>

#include "io/variant/vcf_record.hpp"
#include "utils/sequence_utils.hpp"
#include "../facets/reference_context.hpp"

namespace octopus { namespace csr {

const std::string GCContent::name_ = "GC";

std::unique_ptr<Measure> GCContent::do_clone() const
{
    return std::make_unique<GCContent>(*this);
}

Measure::ValueType GCContent::get_value_type() const
{
    return double {};
}

Measure::ResultType GCContent::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& reference = get_value<ReferenceContext>(facets.at("ReferenceContext"));
    return ValueType {utils::gc_content(reference.sequence(expand(mapped_region(call), 50)))};
}

Measure::ResultCardinality GCContent::do_cardinality() const noexcept
{
    return ResultCardinality::one;
}

const std::string& GCContent::do_name() const
{
    return name_;
}

std::string GCContent::do_describe() const
{
    return "GC bias of the reference in a window centred on the call";
}

std::vector<std::string> GCContent::do_requirements() const
{
    return {"ReferenceContext"};
}

} // namespace csr
} // namespace octopus


/*
 * (C) Copyright 2021 Met Office UK
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include <Eigen/Dense>
#include <algorithm>
#include <set>

#include "ioda/ObsDataVector.h"
#include "oops/util/IntSetParser.h"
#include "oops/util/missingValues.h"
#include "ufo/filters/obsfunctions/CloudCostFunction.h"
#include "ufo/filters/Variable.h"
#include "ufo/utils/ufo_utils.interface.h"

namespace ufo {

static ObsFunctionMaker<CloudCostFunction> makerCloudCostFunction_("CloudCostFunction");

CloudCostFunction::CloudCostFunction(const eckit::LocalConfiguration & conf)
  : invars_() {
  // Initialize options
  options_.deserialize(conf);

  // List of field names for B matrix
  fields_ = options_.field_names.value();

  // Get channels for computing scattering index from options
  std::set<int> chanset = oops::parseIntSet(options_.chanlist.value());
  channels_.assign(chanset.begin(), chanset.end());

  // List of required data
  for (size_t i = 0; i < fields_.size(); ++i) {
    invars_ += Variable("brightness_temperature_jacobian_"+fields_[i]+"@ObsDiag", channels_);
  }
  invars_ += Variable("brightness_temperature@ObsValue", channels_);
  invars_ += Variable("brightness_temperature@"+options_.HofXGroup.value(), channels_);
  invars_ += Variable("latitude@MetaData");
  if (options_.qtotal_lnq_gkg.value()) {
    invars_ += Variable("specific_humidity@GeoVaLs");
    invars_ += Variable("mass_content_of_cloud_liquid_water_in_atmosphere_layer@GeoVaLs");
    invars_ += Variable("mass_content_of_cloud_ice_in_atmosphere_layer@GeoVaLs");
    invars_ += Variable("air_pressure@GeoVaLs");
    invars_ += Variable("air_temperature@GeoVaLs");
    invars_ += Variable("surface_pressure@GeoVaLs");
    invars_ += Variable("surface_temperature@GeoVaLs");
    invars_ += Variable("specific_humidity_at_two_meters_above_surface@GeoVaLs");
  }
}

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

void CloudCostFunction::compute(const ObsFilterData & in,
                                    ioda::ObsDataVector<float> & out) const {
  // Get dimensions
  const size_t nlocs = in.nlocs();
  const size_t nchans = channels_.size();
  ASSERT(nchans > 0);
  ASSERT(out.nvars() == 1);

  // B, R error covariance objects
  eckit::LocalConfiguration bMatrixConf;
  bMatrixConf.set("BMatrix", options_.bmatrix_filepath.value());
  bMatrixConf.set("background fields", options_.field_names.value());
  bMatrixConf.set("qtotal", options_.qtotal_lnq_gkg.value());
  MetOfficeBMatrixStatic staticB(bMatrixConf);

  eckit::LocalConfiguration rMatrixConf;
  rMatrixConf.set("RMatrix", options_.rmatrix_filepath.value());
  MetOfficeRMatrixRadiance staticR(rMatrixConf);

  bool split_rain = options_.qtotal_split_rain.value();

  const std::string clw_name = "mass_content_of_cloud_liquid_water_in_atmosphere_layer";
  const std::string ciw_name = "mass_content_of_cloud_ice_in_atmosphere_layer";
  std::vector<float> gv_pres(nlocs), gv_temp(nlocs), gv_qgas(nlocs), gv_clw(nlocs), gv_ciw(nlocs),
                     humidity_total(nlocs);

  // Determine if pressure is ascending or descending (B-matrix assumption)
  size_t np = in.nlevs(Variable("air_pressure@GeoVaLs"));
  std::vector<float> gv_pres_1(nlocs), gv_pres_N(nlocs);
  in.get(Variable("air_pressure@GeoVaLs"), 1, gv_pres_1);
  in.get(Variable("air_pressure@GeoVaLs"), np, gv_pres_N);
  const float missing = util::missingValue(missing);
  ASSERT(gv_pres_1[0] != missing);
  ASSERT(gv_pres_N[0] != missing);
  bool p_ascending = (gv_pres_N[0] > gv_pres_1[0]);

  // Assemble combined Jacobian from component fields
  std::vector<std::vector<std::vector<float>>>
       jac_vec(nlocs, std::vector<std::vector<float>>(nchans, std::vector<float>()));
  for (size_t ifield = 0; ifield < fields_.size(); ++ifield) {
    if (options_.qtotal_lnq_gkg.value() &&
            (fields_[ifield] == clw_name || fields_[ifield] == ciw_name)) {
      // qtotal ln(g/kg) Jacobian calculated when "specific_humidity" is reached in field list
      continue;
    }
    std::string jac_name = "brightness_temperature_jacobian_"+fields_[ifield]+"@ObsDiag";
    size_t nlevs = in.nlevs(Variable(jac_name, channels_)[0]);
    std::vector<float> jac_store(nlocs);
    for (size_t ilev = 0; ilev < nlevs; ++ilev) {
      int level_gv = (p_ascending ? ilev+1 : nlevs-ilev);
      int level_jac = (options_.reverse_Jacobian.value() ? nlevs-level_gv+1 : level_gv);
      if (fields_[ifield] == "specific_humidity" && options_.qtotal_lnq_gkg.value()) {
        in.get(Variable("air_pressure@GeoVaLs"), level_gv, gv_pres);
        in.get(Variable("air_temperature@GeoVaLs"), level_gv, gv_temp);
        in.get(Variable("specific_humidity@GeoVaLs"), level_gv, gv_qgas);
        in.get(Variable(clw_name+"@GeoVaLs"), level_gv, gv_clw);
        in.get(Variable(ciw_name+"@GeoVaLs"), level_gv, gv_ciw);
        std::vector<float> qsaturated(nlocs);
        ufo_ops_satrad_qsatwat_f90(qsaturated.data(), gv_temp.data(), gv_pres.data(),
                                   static_cast<int>(nlocs));
        for (size_t iloc = 0; iloc < nlocs; ++iloc) {
          // Ensure specific humidity is within limits
          gv_qgas[iloc] = std::max(gv_qgas[iloc], options_.min_q.value());
          gv_qgas[iloc] = std::min(gv_qgas[iloc], qsaturated[iloc]);
          humidity_total[iloc] = gv_qgas[iloc] + gv_clw[iloc] + gv_ciw[iloc];
        }
      }

      for (size_t ichan = 0; ichan < nchans; ++ichan) {
        in.get(Variable(jac_name, channels_)[ichan], level_jac, jac_store);

        if (fields_[ifield] == "specific_humidity" && options_.qtotal_lnq_gkg.value()) {
          std::vector<float> jac_clw(nlocs), jac_ciw(nlocs);
          in.get(Variable("brightness_temperature_jacobian_"+clw_name+"@ObsDiag", channels_)[ichan],
                 level_jac, jac_clw);
          in.get(Variable("brightness_temperature_jacobian_"+ciw_name+"@ObsDiag", channels_)[ichan],
                 level_jac, jac_ciw);
          std::vector<float> dq_dqtotal(nlocs), dql_dqtotal(nlocs), dqi_dqtotal(nlocs);
          int qsplit_mode = 2;  // compute derivatives
          ufo_ops_satrad_qsplit_f90(qsplit_mode, static_cast<int>(nlocs), gv_pres.data(),
                                    gv_temp.data(), humidity_total.data(), dq_dqtotal.data(),
                                    dql_dqtotal.data(), dqi_dqtotal.data(), split_rain);
          // Jacobian dy/dx for observation y, humdity x in units kg/kg
          // For alternative units of B-matrix humidity z in ln(g/kg)
          // chain rule gives dy/dz = x.(dy/dx)
          // Gradient due to ice is ignored unless we are using scattering radiative transfer
          // dTb/dln(qt) = qt*(dTb/dq*dq/dqt + dTb/dql*dql/dqt [+ dTb/dqi*dqi/dqt])
          for (size_t iloc = 0; iloc < nlocs; ++iloc) {
            jac_store[iloc] *= dq_dqtotal[iloc];
            jac_store[iloc] += jac_clw[iloc]*dql_dqtotal[iloc];
            if (options_.scattering_switch.value()) {
              jac_store[iloc] += jac_ciw[iloc]*dqi_dqtotal[iloc];
            }
            jac_store[iloc] *= humidity_total[iloc];
          }
        }

        if (fields_[ifield] == "specific_humidity_at_two_meters_above_surface"
            && options_.qtotal_lnq_gkg.value()) {
          in.get(Variable("surface_pressure@GeoVaLs"), level_gv, gv_pres);
          in.get(Variable("surface_temperature@GeoVaLs"), level_gv, gv_temp);
          in.get(Variable("specific_humidity_at_two_meters_above_surface@GeoVaLs"),
                 level_gv, gv_qgas);
          std::vector<float> qsaturated(nlocs);
          ufo_ops_satrad_qsatwat_f90(qsaturated.data(), gv_temp.data(), gv_pres.data(),
                                     static_cast<int>(nlocs));
          for (size_t iloc = 0; iloc < nlocs; ++iloc) {
            gv_qgas[iloc] = std::max(gv_qgas[iloc], options_.min_q.value());
            gv_qgas[iloc] = std::min(gv_qgas[iloc], qsaturated[iloc]);
            // dTb/dln(q2m) = q2m*(dTb/dq2m)
            jac_store[iloc] *= gv_qgas[iloc];
          }
        }

        for (size_t iloc = 0; iloc < nlocs; ++iloc) {
          jac_vec[iloc][ichan].push_back(jac_store[iloc]);
        }
      }
    }
  }

  const size_t sizeB = staticB.getsize();
  for (size_t iloc = 0; iloc < nlocs; ++iloc) {
    for (size_t ichan = 0; ichan < nchans; ++ichan) {
      ASSERT(jac_vec[iloc][ichan].size() == sizeB);
    }
  }

  // Get departures = ObsValue - HofX (where HofX is bias corrected)
  std::vector<std::vector<float>> departures(nlocs, std::vector<float>(nchans));
  std::vector<float> obsvalues(nlocs);
  std::vector<float> bgvalues(nlocs);
  std::vector<bool> is_out_of_bounds(nlocs, false);
  for (size_t ichan = 0; ichan < nchans; ++ichan) {
    in.get(Variable("brightness_temperature@ObsValue", channels_)[ichan], obsvalues);
    in.get(Variable("brightness_temperature@"+options_.HofXGroup.value(), channels_)[ichan],
           bgvalues);
    for (size_t iloc = 0; iloc < nlocs; ++iloc) {
      departures[iloc][ichan] = obsvalues[iloc] - bgvalues[iloc];
      // Flag observations outside expected bounds
      if (obsvalues[iloc] < options_.minTb.value() || obsvalues[iloc] > options_.maxTb.value()) {
        is_out_of_bounds[iloc] = true;
      }
    }
  }

  std::vector<float> latitude(nlocs);
  in.get(Variable("latitude@MetaData"), latitude);
  Eigen::MatrixXf Hmatrix(nchans, sizeB);

  for (size_t iloc = 0; iloc < nlocs; ++iloc) {
    if (is_out_of_bounds[iloc]) {
      out[0][iloc] = options_.maxCost.value();
      continue;
    }

    for (size_t ichan = 0; ichan < nchans; ++ichan) {
      Hmatrix.row(ichan) = Eigen::Map<Eigen::VectorXf>(jac_vec[iloc][ichan].data(), sizeB);
    }

    // Matrix of departures dy
    Eigen::Map<Eigen::VectorXf> dy(departures[iloc].data(), nchans);

    // Calculate Scratch_matrix = H.B.H^T + R
    Eigen::MatrixXf BHT;
    staticB.multiply(latitude[iloc], Hmatrix.transpose(), BHT);
    Eigen::MatrixXf HBHT = Hmatrix*BHT;
    Eigen::MatrixXf Scratch_matrix;
    staticR.add(channels_, HBHT, Scratch_matrix);

    // Calculate Scratch_matrix2 = Scratch_matrix^-1.dy using Cholesky decomposition
    Eigen::LLT<Eigen::MatrixXf> decomposition(Scratch_matrix);
    if (decomposition.info() == Eigen::NumericalIssue) {
      oops::Log::warning() <<
        "CloudCostFunction Scratch_matrix appears not to be positive definite" << std::endl;
      out[0][iloc] = options_.maxCost.value();
      continue;
    }
    Eigen::VectorXf Scratch_matrix2 = decomposition.solve(dy);

    // Final cost
    float Cost_final = 0.5*dy.transpose()*Scratch_matrix2;
    Cost_final /= static_cast<float>(nchans);  // normalise by number of channels
    out[0][iloc] = std::min(Cost_final, options_.maxCost.value());
  }
}

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

const ufo::Variables & CloudCostFunction::requiredVariables() const {
  return invars_;
}

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

}  // namespace ufo