#ifndef __WAVELET_H__
#define __WAVELET_H__

/**
 * @file Wavelet変換について記述したファイル
 * 
 */

#include <iostream>
#include <vector>
#include <deque>
#include <map>

#include <algorithm>
#include <iterator>

#include <cmath>

template <class PropagateType>
class WaveletCascade {
  protected:
    typedef PropagateType propagate_t;
    WaveletCascade *next;
    const int depth;
    
    struct update_res_t {
      bool valid;
      propagate_t out, in_next;
    };
    virtual update_res_t update(const propagate_t &in) = 0;
    
    struct revert_res_t {
      enum {VALID_OUTPUT, REQUIRE_SAME_LEVEL, REQUIRE_LOWER_LEVEL} state;
      propagate_t out;
    };
    virtual revert_res_t revert() = 0;
    virtual revert_res_t revert(const propagate_t &in) = 0;
    
  public:
    WaveletCascade() 
        : next(NULL), depth(0){}
    WaveletCascade(const WaveletCascade &next_elm) 
        : next(const_cast<WaveletCascade *>(&next_elm)), 
        depth(next_elm.depth + 1){}
    virtual ~WaveletCascade(){}
    template <class CallbackType>
    void propagate(const propagate_t &in, CallbackType &callback){
      update_res_t res(update(in));
      if(res.valid){
        callback(depth, res.out);
        if(next){
          next->propagate(res.in_next, callback);
        }else{
          callback(depth - 1, res.in_next);
        }
      }
    }
    
    template <class TransformedData>
    propagate_t back_propagate(TransformedData &data){
      revert_res_t res;
      res = revert();
      while(true){
        switch(res.state){
          case revert_res_t::VALID_OUTPUT:
            return res.out;
          case revert_res_t::REQUIRE_SAME_LEVEL:
            res = revert(data(depth));
            break;
          case revert_res_t::REQUIRE_LOWER_LEVEL:
            if(next){
              res = revert(next->back_propagate(data));
            }else{
              res = revert(data(depth - 1));
            }
            break;
        }
      }
    }
};

template <int Level>
struct DaubechiesCoefficients {
  static const int level;
  static const double h[Level];
  static const double g[Level];
};

template <int Level>
const int DaubechiesCoefficients<Level>::level = Level;

template <>
const double DaubechiesCoefficients<4>::h[] = {
    0.48296291314453414337487159986,
    0.83651630373780790557529378092,
    0.22414386804201338102597276224,
    -0.12940952255126038117444941881};

template <>
const double DaubechiesCoefficients<4>::g[] = {
    -0.12940952255126038117444941881,
    -0.22414386804201338102597276224,
    0.83651630373780790557529378092,
    -0.48296291314453414337487159986};

template <>
const double DaubechiesCoefficients<6>::h[] = {
    0.33267055295008261599851158914,
    0.80689150931109257649449360409,
    0.45987750211849157009515194215,
    -0.13501102001025458869638990670,
    -0.08544127388202666169281916918,
    0.03522629188570953660274066472};

template <>
const double DaubechiesCoefficients<6>::g[] = {
    0.03522629188570953660274066472,
    0.08544127388202666169281916918,
    -0.13501102001025458869638990670,
    -0.45987750211849157009515194215,
    0.80689150931109257649449360409,
    -0.33267055295008261599851158914};

template <>
const double DaubechiesCoefficients<8>::h[] = {
    0.23037781330889650086329118304,
    0.71484657055291564708992195527,
    0.63088076792985890788171633830,
    -0.02798376941685985421141374718,
    -0.18703481171909308407957067279,
    0.03084138183556076362721936253,
    0.03288301166688519973540751355,
    -0.01059740178506903210488320852};

template <>
const double DaubechiesCoefficients<8>::g[] = {
    -0.01059740178506903210488320852,
    -0.03288301166688519973540751355,
    0.03084138183556076362721936253,
    0.18703481171909308407957067279,
    -0.02798376941685985421141374718,
    -0.63088076792985890788171633830,
    0.71484657055291564708992195527,
    -0.23037781330889650086329118304};

template <>
const double DaubechiesCoefficients<10>::h[] = {
    0.16010239797419291448072374802,
    0.60382926979718967054011930653,
    0.72430852843777292772807124410,
    0.13842814590132073150539714634,
    -0.24229488706638203186257137947,
    -0.03224486958463837464847975506,
    0.07757149384004571352313048939,
    -0.00624149021279827427419051911,
    -0.01258075199908199946850973993,
    0.00333572528547377127799818342};

template <>
const double DaubechiesCoefficients<10>::g[] = {
    0.00333572528547377127799818342,
    0.01258075199908199946850973993,
    -0.00624149021279827427419051911,
    -0.07757149384004571352313048939,
    -0.03224486958463837464847975506,
    0.24229488706638203186257137947,
    0.13842814590132073150539714634,
    -0.72430852843777292772807124410,
    0.60382926979718967054011930653,
    -0.16010239797419291448072374802};

template <>
const double DaubechiesCoefficients<12>::h[] = {
    0.11154074335010946362132391724,
    0.49462389039845308567720417688,
    0.75113390802109535067893449844,
    0.31525035170919762908598965481,
    -0.22626469396543982007631450066,
    -0.12976686756726193556228960588,
    0.09750160558732304910234355254,
    0.02752286553030572862554083950,
    -0.03158203931748602956507908070,
    0.00055384220116149613925191840,
    0.00477725751094551063963597525,
    -0.00107730108530847956485262161};

template <>
const double DaubechiesCoefficients<12>::g[] = {
    -0.00107730108530847956485262161,
    -0.00477725751094551063963597525,
    0.00055384220116149613925191840,
    0.03158203931748602956507908070,
    0.02752286553030572862554083950,
    -0.09750160558732304910234355254,
    -0.12976686756726193556228960588,
    0.22626469396543982007631450066,
    0.31525035170919762908598965481,
    -0.75113390802109535067893449844,
    0.49462389039845308567720417688,
    -0.11154074335010946362132391724};

template <>
const double DaubechiesCoefficients<14>::h[] = {
    0.07785205408500917901996352196,
    0.39653931948191730653900039094,
    0.72913209084623511991694307034,
    0.46978228740519312247159116097,
    -0.14390600392856497540506836221,
    -0.22403618499387498263814042023,
    0.07130921926683026475087657050,
    0.08061260915108307191292248036,
    -0.03802993693501441357959206160,
    -0.01657454163066688065410767489,
    0.01255099855609984061298988603,
    0.00042957797292136652113212912,
    -0.00180164070404749091526826291,
    0.00035371379997452024844629584};

template <>
const double DaubechiesCoefficients<14>::g[] = {
    0.00035371379997452024844629584,
    0.00180164070404749091526826291,
    0.00042957797292136652113212912,
    -0.01255099855609984061298988603,
    -0.01657454163066688065410767489,
    0.03802993693501441357959206160,
    0.08061260915108307191292248036,
    -0.07130921926683026475087657050,
    -0.22403618499387498263814042023,
    0.14390600392856497540506836221,
    0.46978228740519312247159116097,
    -0.72913209084623511991694307034,
    0.39653931948191730653900039094,
    -0.07785205408500917901996352196};

template <>
const double DaubechiesCoefficients<16>::h[] = {
    0.05441584224310400995500940520,
    0.31287159091429997065916237551,
    0.67563073629728980680780076705,
    0.58535468365420671277126552005,
    -0.01582910525634930566738054788,
    -0.28401554296154692651620313237,
    0.00047248457391328277036059001,
    0.12874742662047845885702928751,
    -0.01736930100180754616961614887,
    -0.04408825393079475150676372324,
    0.01398102791739828164872293057,
    0.00874609404740577671638274325,
    -0.00487035299345157431042218156,
    -0.00039174037337694704629808036,
    0.00067544940645056936636954757,
    -0.00011747678412476953373062823};

template <>
const double DaubechiesCoefficients<16>::g[] = {
    -0.00011747678412476953373062823,
    -0.00067544940645056936636954757,
    -0.00039174037337694704629808036,
    0.00487035299345157431042218156,
    0.00874609404740577671638274325,
    -0.01398102791739828164872293057,
    -0.04408825393079475150676372324,
    0.01736930100180754616961614887,
    0.12874742662047845885702928751,
    -0.00047248457391328277036059001,
    -0.28401554296154692651620313237,
    0.01582910525634930566738054788,
    0.58535468365420671277126552005,
    -0.67563073629728980680780076705,
    0.31287159091429997065916237551,
    -0.05441584224310400995500940520};

template <>
const double DaubechiesCoefficients<18>::h[] = {
    0.03807794736387834658869765888,
    0.24383467461259035373204158165,
    0.60482312369011111190307686743,
    0.65728807805130053807821263905,
    0.13319738582500757619095494590,
    -0.29327378327917490880640319524,
    -0.09684078322297646051350813354,
    0.14854074933810638013507271751,
    0.03072568147933337921231740072,
    -0.06763282906132997367564227483,
    0.00025094711483145195758718975,
    0.02236166212367909720537378270,
    -0.00472320475775139727792570785,
    -0.00428150368246342983449679500,
    0.00184764688305622647661912949,
    0.00023038576352319596720521639,
    -0.00025196318894271013697498868,
    0.00003934732031627159948068988};

template <>
const double DaubechiesCoefficients<18>::g[] = {
    0.00003934732031627159948068988,
    0.00025196318894271013697498868,
    0.00023038576352319596720521639,
    -0.00184764688305622647661912949,
    -0.00428150368246342983449679500,
    0.00472320475775139727792570785,
    0.02236166212367909720537378270,
    -0.00025094711483145195758718975,
    -0.06763282906132997367564227483,
    -0.03072568147933337921231740072,
    0.14854074933810638013507271751,
    0.09684078322297646051350813354,
    -0.29327378327917490880640319524,
    -0.13319738582500757619095494590,
    0.65728807805130053807821263905,
    -0.60482312369011111190307686743,
    0.24383467461259035373204158165,
    -0.03807794736387834658869765888};

template <>
const double DaubechiesCoefficients<20>::h[] = {
    0.02667005790055555358661744877,
    0.18817680007769148902089297368,
    0.52720118893172558648174482796,
    0.68845903945360356574187178255,
    0.28117234366057746074872699845,
    -0.24984642432731537941610189792,
    -0.19594627437737704350429925432,
    0.12736934033579326008267723320,
    0.09305736460357235116035228984,
    -0.07139414716639708714533609308,
    -0.02945753682187581285828323760,
    0.03321267405934100173976365318,
    0.00360655356695616965542329142,
    -0.01073317548333057504431811411,
    0.00139535174705290116578931845,
    0.00199240529518505611715874224,
    -0.00068585669495971162656137098,
    -0.00011646685512928545095148097,
    0.00009358867032006959133405013,
    -0.00001326420289452124481243668};

template <>
const double DaubechiesCoefficients<20>::g[] = {
    -0.00001326420289452124481243668,
    -0.00009358867032006959133405013,
    -0.00011646685512928545095148097,
    0.00068585669495971162656137098,
    0.00199240529518505611715874224,
    -0.00139535174705290116578931845,
    -0.01073317548333057504431811411,
    -0.00360655356695616965542329142,
    0.03321267405934100173976365318,
    0.02945753682187581285828323760,
    -0.07139414716639708714533609308,
    -0.09305736460357235116035228984,
    0.12736934033579326008267723320,
    0.19594627437737704350429925432,
    -0.24984642432731537941610189792,
    -0.28117234366057746074872699845,
    0.68845903945360356574187178255,
    -0.52720118893172558648174482796,
    0.18817680007769148902089297368,
    -0.02667005790055555358661744877};

template <int Level, class PropagateType = double>
class DaubechiesCascade 
    : public WaveletCascade<PropagateType>,
    public DaubechiesCoefficients<Level> {
  protected:
    typedef WaveletCascade<PropagateType> super_t;
    typedef DaubechiesCoefficients<Level> coef_t;
  public:
    typedef typename super_t::update_res_t update_res_t;
    typedef typename super_t::revert_res_t revert_res_t;
    typedef typename super_t::propagate_t propagate_t;
    using coef_t::level;
  protected:
    using coef_t::h;
    using coef_t::g;
  public:
    propagate_t storage[Level];
    propagate_t storage_init[Level - 2];
    int storage_size;
    DaubechiesCascade() 
        : super_t(), storage_size(0) {}
    DaubechiesCascade(const DaubechiesCascade &next_elm)
        : super_t(next_elm), storage_size(0) {}
    ~DaubechiesCascade(){}
  protected:
    update_res_t update(const propagate_t &in){
      update_res_t res;
      
      storage[storage_size] = in;
      if(storage_size < sizeof(storage_init) / sizeof(storage_init[0])){
        storage_init[storage_size] = in;
      }
      if(++storage_size >= Level){
        res.valid = true;
        res.in_next = storage[0] * h[0];
        res.out = storage[0] * g[0];
        for(int i(1); i < Level; ++i){
          res.in_next += storage[i] * h[i];
          res.out += storage[i] * g[i];
        }
        storage_size -= 2;
        for(int i(0), j(2); i < storage_size; ++i, ++j){
          storage[i] = storage[j];
        }
      }else{
        res.valid = false;
      }
      
      return res;
    }
    revert_res_t revert(){
      revert_res_t res;
      if(storage_size > Level - 2){
        res.state = revert_res_t::VALID_OUTPUT;
        storage_size--;
        res.out = storage[1];
      }else{
        res.state = revert_res_t::REQUIRE_SAME_LEVEL;
        storage_size++;
      }
      return res;
    }
    revert_res_t revert(const propagate_t &in){
      revert_res_t res;
      if((storage_size % 2) == 0){
        propagate_t same_in(storage[1]); // in <= lower
        
        // 前の計算結果をシフトする
        for(int i(Level - storage_size); i < (Level - 2); i++){
          storage[i] = storage[i + 2];
        }
        
        // 新たに加わった部分を加算/新規割り当てする
        for(int i(Level - storage_size); i < Level; i++){
          propagate_t temp(in * h[i] + same_in * g[i]);
          if(i < (Level - 2)){
            storage[i] += temp;
          }else{
            storage[i] = temp;
          }
        }
        
        // 十分な量のデータが溜まっているかどうか
        if(storage_size >= Level){
          res.state = revert_res_t::VALID_OUTPUT;
          storage_size--;
          res.out = storage[0];
        }else{
          res.state = revert_res_t::REQUIRE_SAME_LEVEL;
          storage_size++;
        }
      }else{
        storage_size++;
        storage[1] = in; // Same
        res.state = revert_res_t::REQUIRE_LOWER_LEVEL;
      }
      return res;
    }
  public:
    template <class CallbackType>
    void terminate_propagate(CallbackType &callback){
      if(storage_size){
        for(int i(0); i < Level - 2; ++i){
          propagate(storage_init[i % storage_size], callback);
        }
      }
      storage_size = 0;
      if(super_t::next){
        static_cast<DaubechiesCascade *>(super_t::next)
            ->terminate_propagate(callback);
      }
    }
    template <class TransformedData>
    void initialize_back_propagate(TransformedData &data){
      /*
       * 例えばD6で5段=2**5=32の場合、以下のような変換過程(下から上へ)をたどる
       * 
       * <depth : 4>
       * a1 ... a13 a14 a15 a16 a17 ... +a29 a30 a31 a32
       * b1 ... +b13 b14 b15 b16 c1 ... +c13 c14 c15 c16 => depth@c = 4((6-2) % 16 = 4)
       * (b13 c13 b14 c14 b15 c15と処理してはじめてa29 a30が得られる)
       * 
       * <depth : 3>
       * b1 ... b5 b6 b7 b8 b9 ... +b13 b14 b15 b16
       * d1 ... +d5 d6 d7 d8 e1 ... +e5 e6 e7 e8 => depth@e = 3((6-2) % 8 = 4)
       * 
       * <depth : 2>
       * d1 d2 d3 d4 +d5 d6 d7 d8
       * +f1 f2 f3 f4 +g1 g2 g3 g4 => depth@g = 2((6-2) % 4 = 0)
       * 
       * <depth : 1>
       * +f1 f2 f3 f4
       * +h1 h2 +i1 i2 => depth@i = 1((6-2) % 2 = 0)
       * 
       * <depth : 0>
       * +h1 h2
       * j1 k1 => depth@k = 0((6-2) % 1 = 0)
       * 
       * +マークは同じレベルで一番初めに得られる出力
       * そのまま使えるレベルの値(c,e,g,i,k)は、
       * 適切にrewindででてくる順番をコントロールする
       * 
       */
      data.rewind(super_t::depth, Level - 2);
      storage_size = 0;
      if(super_t::next){
        static_cast<DaubechiesCascade *>(super_t::next)
            ->initialize_back_propagate(data);
      }
    }
};

template <class PropagateType = double>
class HaarCascade 
    : public WaveletCascade<PropagateType> {
  protected:
    typedef WaveletCascade<PropagateType> super_t;
  public:
    typedef typename super_t::update_res_t update_res_t;
    typedef typename super_t::revert_res_t revert_res_t;
    typedef typename super_t::propagate_t propagate_t;
    static const double h[];
    static const double g[];
  public:
    propagate_t storage[2];
    int storage_size;
    static const int level;
    HaarCascade() 
        : super_t(), storage_size(0) {}
    HaarCascade(const HaarCascade &next_elm)
        : super_t(next_elm), storage_size(0) {}
    ~HaarCascade(){}
  protected:
    update_res_t update(const propagate_t &in){
      update_res_t res;
      
      storage[storage_size] = in;
      if(++storage_size >= 2){
        res.valid = true;
        res.in_next = storage[0] * h[0];
        res.out = storage[0] * g[0];
        res.in_next += storage[1] * h[1];
        res.out += storage[1] * g[1];
        storage_size = 0;
      }else{
        res.valid = false;
      }
      
      return res;
    }
    revert_res_t revert(){
      revert_res_t res;
      if(storage_size){
        res.state = revert_res_t::VALID_OUTPUT;
        storage_size = 0;
        res.out = storage[1];
      }else{
        res.state = revert_res_t::REQUIRE_SAME_LEVEL;
        storage_size = 1;
      }
      return res;
    }
    revert_res_t revert(const propagate_t &in){
      revert_res_t res;
      if(storage_size >= 2){
        storage[0] = in; // Lower
        res.out = storage[0] * h[0] + storage[1] * g[0];
        storage[1] = storage[0] * h[1] + storage[1] * g[1];
        storage_size = 1;
        res.state = revert_res_t::VALID_OUTPUT;
      }else{
        storage_size = 2;
        storage[1] = in; // Same
        res.state = revert_res_t::REQUIRE_LOWER_LEVEL;
      }
      return res;
    }
  public:
    template <class CallbackType>
    void terminate_propagate(CallbackType &callback){
      storage_size = 0;
      if(super_t::next){
        static_cast<HaarCascade *>(super_t::next)
            ->terminate_propagate(callback);
      }
    }
    template <class TransformedData>
    void initialize_back_propagate(TransformedData &data){
      storage_size = 0;
      if(super_t::next){
        static_cast<HaarCascade *>(super_t::next)
            ->initialize_back_propagate(data);
      }
    }
};

template <class PropagateType>
const int HaarCascade<PropagateType>::level = 2; 

template <class PropagateType>
const double HaarCascade<PropagateType>::h[] = {
    0.70710678118654752440084436210,
    0.70710678118654752440084436210};

template <class PropagateType>
const double HaarCascade<PropagateType>::g[] = {
    0.70710678118654752440084436210,
    -0.70710678118654752440084436210};

#include <new> // for placement new

template <class Cascade, int Stages>
class WaveletTransformation {
  protected:
    Cascade cascade[Stages];
  public:
    static const int stages = Stages;
    static int debug;
  
    WaveletTransformation(){
      for(int i((sizeof(cascade) / sizeof(cascade[0])) - 2);
          i >= 0; --i){
        new(&cascade[i]) Cascade(cascade[i + 1]);
      }
    }
    ~WaveletTransformation(){}
    
  protected:
    template <class T>
    class Hook {
      public:
        typedef std::vector<T> serialized_t;
        typedef std::deque<T> spool_t;
        typedef std::map<int, spool_t> transformed_t;
      protected:
        transformed_t transformed;
      public:
        Hook() : transformed() {}
        ~Hook() {}
        
        // forward 用
        void operator()(const int &depth, const T &value){
          //std::cerr << value << "(" << depth << ") ";
          transformed[depth].push_back(value);
        }
        serialized_t serialize() {
          serialized_t res;
          for(typename transformed_t::reverse_iterator it(transformed.rbegin());
              it != transformed.rend();
              ++it){
            //std::cerr << it->first << ", " << it->second.size() << std::endl;
            int base(it->first < 0 ? 0 : (1 << (it->first))), offset(0);
            if(res.size() < (base << 1)){
              res.resize(base << 1);
            }
            for(typename transformed_t::mapped_type::iterator it2(it->second.begin());
                it2 != it->second.end();
                ++it2, ++offset){
              res[base + offset] = *it2;
            }
          }
          return res;
        }
        
        // invert 用
        Hook(const serialized_t &serialized) : transformed() {
          int index(0), depth(-1), next_depth_index(1);
          for(typename serialized_t::const_iterator it(serialized.begin()); 
              it != serialized.end();
              ++it, ++index){
            if(index >= next_depth_index){
              next_depth_index <<= 1;
              depth++;
            }
            transformed[depth].push_back(*it);
          }
        }
        T &operator()(const int &depth){
          //std::cerr << depth << " ";
          transformed[depth].push_back(transformed[depth].front());
          transformed[depth].pop_front();
          return transformed[depth].back();
        }
        void rewind(const int &depth, int size){
          typename transformed_t::mapped_type &target(transformed[depth]);
          size %= target.size();
          if(size > 0){
            while(size--){
              target.push_front(target.back()); // 後ろを前に
              target.pop_back();
            }
          }else if(size < 0){
            while(size++){
              target.push_back(target.front()); // 前を後ろに
              target.pop_front();
            }
          }
        }
    };
  public:
    std::vector<typename Cascade::propagate_t> forward(
        const std::vector<typename Cascade::propagate_t> &src){
      
      Hook<typename Cascade::propagate_t> hook;
      for(int i(0); i < src.size(); i++){
        //std::cerr << i << ": ";
        cascade[0].propagate(src[i], hook);
        //std::cerr << std::endl;
      }
      cascade[0].terminate_propagate(hook);
      //std::cerr << std::endl;
      
      return hook.serialize();
    }
    
    std::vector<typename Cascade::propagate_t> invert(
        const std::vector<typename Cascade::propagate_t> &src){
      
      std::vector<typename Cascade::propagate_t> res1, res2;
      
      Hook<typename Cascade::propagate_t> hook(src);
      cascade[0].initialize_back_propagate(hook);
      for(int i(0); i < src.size(); i++){
        //std::cerr << i << ": ";
        if(i < (cascade[0].level - 2)){
          res1.push_back(cascade[0].back_propagate(hook));
        }else{
          res2.push_back(cascade[0].back_propagate(hook));
        }
        //std::cerr << std::endl;
      }
      //出力の順番を修正する
      std::copy(res1.begin(), res1.end(),
          std::back_insert_iterator<
              std::vector<typename Cascade::propagate_t> >(res2));
      
      return res2;
    }
};

#endif /* __WAVELET_H__ */