#pragma once

#include <vector>
#include <complex>
#include <opencv2/opencv.hpp>
#include <iterator>

namespace math {

  using complex = std::complex<double>;
  using signal = std::vector<double>;
  using csignal = std::vector<complex>;
  using contour = std::vector<cv::Point>;
  constexpr double pi() {return std::atan(1)*4;}

  int filter(const cv::Mat& img, cv::Mat output, int seuil) {
  	bool detect = false;
  	uchar R, G, B;
  	int rows = img.rows;
  	int cols = img.cols;
  	int dim = img.channels();
  	int indexNB;
  
  	for (int index=0,indexNB=0;index<dim*rows*cols;index+=dim,indexNB++) {
  		detect=0;
  		B = img.data[index    ];
  		G = img.data[index + 1];
  		R = img.data[index + 2];
  
  		if ((R>G) && (R>B))
  			if (((R-B)>=seuil) || ((R-G)>=seuil))
  				detect=1;
  		if (detect==1)
  			output.data[indexNB]=255;
  		else
  			output.data[indexNB]=0;
  	}
  }

  csignal cont2sig(const contour& cont) {
    csignal sig;
    for (auto p: cont) {
      sig.push_back(complex(p.x, p.y));
    }
    return sig;
  };

  complex mean(const csignal& sig) {
    complex res = 0;
    for (auto x: sig) {
      res += x;
    }
    return complex(res.real()/sig.size(), res.imag()/sig.size());
  };

  csignal diff(const csignal& input, complex mean) {
    csignal res;
    for (auto x: input) {
      res.push_back(x-mean);
    }
    return res;
  }

  csignal fft_rec(const csignal& input) {
    int size = input.size();

    if (size <= 1) {
      return input;
    } else {
      csignal odd;
      csignal even;
      auto odd_back_it = std::back_inserter(odd);
      auto even_back_it = std::back_inserter(even);
      bool insert_in_even = false;

      for (auto it = input.begin(); it != input.end(); ++it) {
        if (insert_in_even) {
          *(even_back_it++) = *it;
          insert_in_even = false;
        } else {
          *(odd_back_it++) = *it;
          insert_in_even = true;
        }
      }

      csignal odd_fft = fft_rec(odd);
      csignal even_fft = fft_rec(even);
      csignal res(size, complex());

      for (int k=0; k<size/2; ++k) {
        complex t = std::exp(complex(0, -2*pi()*k/size)) * odd_fft[k];
        res[k] = even_fft[k] + t;
        res[size/2+k] = even_fft[k] - t;
      }
      return res;
    }
  }

  csignal fft(const csignal& input, int N=0) {
    int opt_size;
    if (N < input.size()) {
      opt_size = 1 << (int)std::ceil(std::log(input.size())/std::log(2));
    } else if (N==0){
      opt_size = input.size();
    } else {
      opt_size = 1 << (int)std::ceil(std::log(N)/std::log(2));
    }
    csignal sig(input);
    for (int i=0; i<opt_size-input.size(); ++i) {
      sig.push_back(complex(0, 0));
    }
    return fft_rec(sig);
  };

  void operator*=(csignal& sig, complex& m) {
    for(auto x: sig) {
      x *= m;
    }
  }

  void operator*=(csignal& sig, complex&& m) {
    for(auto x: sig) {
      x *= m;
    }
  }

  void operator/=(csignal& sig, complex& m) {
    for(auto x: sig) {
      x /= m;
    }
  }

  void operator/=(csignal& sig, complex&& m) {
    for(auto x: sig) {
      x /= m;
    }
  }

  csignal extract(const csignal& tfd, int cmax) {
    csignal res;
    for (int k=0; k<cmax; ++k) {
      res.push_back(tfd[tfd.size() - cmax + k]);
    }
    for (int k=cmax; k<2*cmax; ++k) {
      res.push_back(tfd[k]);
    }
    return res;
  }

  contour sig2cont(const csignal& sig) {
    contour res;
    for (auto x: sig) {
      res.push_back(cv::Point(x.real(), x.imag()));
    }
    return res;
  }

  csignal desc2sig(const csignal& desc, complex mean, int N, int kmin) {
    csignal cont;
    auto desc_it = desc.begin();

    for (int m=0; m<N; ++m) {
      complex sum;
      for (int k=0; k<desc.size(); ++k) {
        sum += desc[k]*std::exp(complex(0, 2*pi()*(k+kmin)*m/N));
      }
      cont.push_back(mean + sum);
    }
    return cont;
  };

  contour simplify_contour(const contour& cont, int cmax) {
    csignal z = cont2sig(cont);
    complex zm = mean(z);
    csignal tfd = fft(diff(z, zm));
    tfd /= tfd.size();
    csignal desc = extract(tfd, cmax);

    if (std::abs(desc[desc.size()-1]) > std::abs(desc[0])) {
      std::reverse(desc.begin(), desc.end());
    }

    double phy = std::arg(desc[desc.size()-1]*desc[0])/2;
    desc *= std::exp(complex(0, -phy));
    double theta = std::arg(desc[0]);

    for (int k=0; k<desc.size(); ++k) {
      desc[k] *= std::exp(complex(0, -theta*k));
    }
    desc /= desc[0];

    csignal sig = desc2sig(desc, zm, z.size(), cmax);
    return sig2cont(sig);
  };

  int max_cont(const std::vector<contour>& contours) {
    int max = 0;
    int id = 0;
    for (int i=0; i<contours.size(); ++i) {
      if (contours[i].size() > max) {
        max = contours[i].size();
        id = i;
      }
    }
    return id;
  };
}