void compute_u_coefficients(int N, double mu, double q, double L, double kcrc, double complex u[]) {
    double y = M_PI/L;
    double k0r0 = kcrc * exp(-L);
    double t = -2*y*log(k0r0/2);

    if(q == 0) {
        double x = (mu+1)/2;
        double lnr, phi;
        for(int m = 0; m <= N/2; m++) {
            lngamma_4(x, m*y, &lnr, &phi);
            u[m] = polar(1.0,m*t + 2*phi);
        }
    }
    else {
        double xp = (mu+1+q)/2;
        double xm = (mu+1-q)/2;
        double lnrp, phip, lnrm, phim;
        for(int m = 0; m <= N/2; m++) {
            lngamma_4(xp, m*y, &lnrp, &phip);
            lngamma_4(xm, m*y, &lnrm, &phim);
            u[m] = polar(exp(q*log(2) + lnrp - lnrm), m*t + phip - phim);
        }
    }

    for(int m = N/2+1; m < N; m++)
        u[m] = conj(u[N-m]);
    if((N % 2) == 0)
      u[N/2] = (creal(u[N/2]) + I*0.0);
}

/* Create a valid vector for the dataset. */
static vector * citi_create_vector (struct citi_package_t * p, int i,
				    char * n, char * type) {
  vector * vec;
  vec = citi_get_vector (p, i); // fetch vector
  vec = new vector (*vec);      // copy vector
  vec->reverse ();              // reverse vector

  // convert data if necessary
  if (!strcmp (type, "MAGANGLE")) {
    for (int i = 0; i < vec->getSize (); i++) {
      nr_complex_t val = vec->get (i);
      val = polar (real (val), rad (imag (val)));
      vec->set (val, i);
    }
  }
  else if (!strcmp (type, "DBANGLE")) {
    for (int i = 0; i < vec->getSize (); i++) {
      nr_complex_t val = vec->get (i);
      val = polar (pow (10.0, real (val) / 20.0), rad (imag (val)));
      vec->set (val, i);
    }
  }

  // return named vector
  vec->setName (n);
  return vec;
}

real spherical_pattern_maximize(const function<real(Vector<real,3>)>& score, Vector<real,3>& n, real tol) {
  static const real da = 2*M_PI/5;
  static const Vector<real,2> dirs[5] = {polar(0.), polar(da), polar(2*da), polar(3*da), polar(4*da)};
  const real alpha = .5;
  real step = .2;
  real dot = score(n);
  while (step > tol) {
    real best_dot = dot;
    Vector<real,3> best_n = n;
    Vector<real,3> orth;
    orth[n.argmin()] = 1;
    Vector<real,3> a = cross(n,orth).normalized(), b = cross(n,a);
    for (int i = 0; i < 5; i++) {
      Vector<real,3> candidate = (n + step*dirs[i].x*a + step*dirs[i].y*b).normalized();
      real d = score(candidate);
      if (best_dot < d) {
        best_dot = d;
        best_n = candidate;
      }
    }
    if (dot < best_dot) {
      dot = best_dot;
      n = best_n;
    } else
      step *= alpha;
  }
  return dot;
}

cd intersect(circle a, circle b, int x = 0) { 
    double d = sqrt(norm(a.f-b.f));
    double co = (a.s*a.s+d*d-b.s*b.s)/(2*a.s*d);
    double theta = acos(co);
    
    cd tmp = (b.f-a.f)/d;
    if (x == 0) return a.f+tmp*a.s*polar(1.0,theta);
    return a.f+tmp*a.s*polar(1.0,-theta);
}

double z_freqz_db(bool hpf, double f, double a, double h, double l) {
  double wf = f * PI;
  complex<double> hf;
  if (!hpf) { hf = h + l * (1 + (1.0 + polar(a, -wf)) / (a + polar(1.0, -wf))); } else {
    a = 1.0 / a;
    hf = h + l * (1 - (1.0 + polar(a, -wf)) / (a + polar(1.0, -wf)));
  }
  double db = 10.0 * log(magsq(hf)) / log(10.0);
  return (db);
}

void phaseshifter::initSP (void) {
  nr_double_t p = rad (getPropertyDouble ("phi"));
  nr_double_t z = getPropertyDouble ("Zref");
  nr_double_t r = (z0 - z) / (z0 + z);
  nr_complex_t d = 1.0 - polar (r * r, 2 * p);
  nr_complex_t s11 = r * (polar (1.0, 2 * p) - 1.0) / d;
  nr_complex_t s21 = (1.0 - r * r) * polar (1.0, p) / d;
  allocMatrixS ();
  setS (NODE_1, NODE_1, s11);
  setS (NODE_2, NODE_2, s11);
  setS (NODE_1, NODE_2, s21);
  setS (NODE_2, NODE_1, s21);
}

//散乱波の計算
void Solver::scatteredWave(complex<double> *p){
	double rad = wave_angle*M_PI/180;	//ラジアン変換
	double _cos = cos(rad), _sin = sin(rad);	//毎回計算すると時間かかりそうだから,代入しておく

	for(int i=mField->getNpml(); i<mField->getNpx(); i++){
		for(int j=mField->getNpml(); j<mField->getNpy(); j++){
			if( N_S(i,j) == 1.0 ) continue;		//屈折率が1なら散乱は起きない
			double ikx = k_s*(i*_cos + j*_sin);
			p[index(i,j, +1)] += ray_coef*(1/_pow(N_S(i,j), 2)-1)
				                    *(polar(1.0, ikx-w_s*(time+DT_S))+polar(1.0, ikx-w_s*(time-DT_S))-2.0*polar(1.0, ikx-w_s*time)); 
		}
	}
}

static void
test_glide_alt(const fixed h, const fixed W, const fixed Wangle,
               std::ostream &hfile)
{
  GlidePolar polar(fixed_zero);
  polar.SetMC(fixed_one);

  AircraftState ac;
  ac.wind.norm = fabs(W);
  if (negative(W)) {
    ac.wind.bearing = Angle::degrees(fixed(180)+Wangle);
  } else {
    ac.wind.bearing = Angle::degrees(Wangle);
  }
  ac.altitude = h;

  GeoVector vect(fixed(400.0));
  GlideState gs(vect, fixed_zero, ac.altitude, ac.wind);
  GlideResult gr = MacCready::solve(polar, gs);
  hfile << (double)h << " " 
        << (double)gr.altitude_difference << " "
        << (double)gr.time_elapsed << " " 
        << (double)gr.v_opt << " " 
        << (double)W << " "
        << (double)Wangle << " "
        << "\n";
}

static void
basic_polar(const fixed mc)
{
  char bname[100];
  sprintf(bname,"results/res-polar-%02d-best.txt",(int)(mc*10));
  std::ofstream pfile("results/res-polar.txt");
  std::ofstream mfile(bname);

  GlidePolar polar(mc);
  for (fixed V= Vmin; V<= polar.GetVMax(); V+= fixed(0.25)) {
    pfile << (double)mc << " " 
          << (double)V << " " 
          << -(double)polar.SinkRate(V) << " " 
          << (double)(V/polar.SinkRate(V))
          << "\n";
  }

  mfile << (double)mc 
        << " " << 0
        << " " << (double)mc
        << " " << (double)polar.GetBestLD() 
        << "\n";
  mfile << (double)mc 
        << " " << (double)polar.GetVBestLD() 
        << " " << -(double)polar.GetSBestLD() 
        << " " << (double)polar.GetBestLD() 
        << "\n";
}

static void
test_glide_cb(const fixed h, const fixed W, const fixed Wangle,
              std::ostream &hfile)
{
  GlidePolar polar(fixed_one);

  AircraftState ac;
  ac.wind.norm = fabs(W);
  if (negative(W)) {
    ac.wind.bearing = Angle::degrees(fixed(180)+Wangle);
  } else {
    ac.wind.bearing = Angle::degrees(Wangle);
  }
  ac.altitude = h;

  GeoVector vect(fixed(400.0));
  GlideState gs (vect, fixed_zero, ac.altitude, ac.wind);
  GlideResult gr = MacCready::solve(polar, gs);

  gr.CalcDeferred(ac);

  hfile << (double)W << " "
        << (double)Wangle << " "
        << (double)gr.vector.Bearing.value_degrees() << " "
        << (double)gr.cruise_track_bearing.value_degrees() << " "
        << "\n";
}

static void
test_glide_stf(const fixed h, const fixed W, const fixed Wangle, const fixed S,
               std::ostream &hfile)
{
  GlidePolar polar(fixed_zero);
  polar.SetMC(fixed_one);

  AircraftState ac;
  ac.wind.norm = fabs(W);
  if (negative(W)) {
    ac.wind.bearing = Angle::degrees(fixed(180)+Wangle);
  } else {
    ac.wind.bearing = Angle::degrees(Wangle);
  }
  ac.altitude = h;
  ac.netto_vario = S;

  GeoVector vect(fixed(400.0));
  GlideState gs(vect, fixed_zero, ac.altitude, ac.wind);
  GlideResult gr = MacCready::solve(polar, gs);

  fixed Vstf = polar.SpeedToFly(ac, gr, false);

  hfile << (double)h << " " 
        << (double)gr.altitude_difference << " "
        << (double)gr.v_opt << " " 
        << (double)Vstf << " " 
        << (double)W << " "
        << (double)Wangle << " "
        << (double)ac.netto_vario << " "
        << "\n";
}

// Fast Fourier transform du signal f(deb:pas:fin) (Matlab notation).
// s=-1 pour DFT directe, s=+1 pour DFT inverse.
// Buffer est utilise comme tableau temporaire, sa longueur doit etre au moins
// celle de f.
void fft_main(complex<float> f[], int deb, int pas, int fin, float s,
              complex<float> buffer[]) {
    int n = (fin - deb) / pas + 1;
    if (n == 1) {
        return;
    }

    assert(n % 2 == 0);

    fft_main(f, deb, 2 * pas, fin - pas, s, buffer);
    fft_main(f, deb + pas, 2 * pas, fin, s, buffer);

    for (int i = 0; i < n; i++) {
        buffer[i] = f[deb + i * pas];
    }

    complex<float> t = 1;
    complex<float> w = polar(1.0f, float(s * 2 * M_PI / n));

    for (int i = 0; i < n / 2; i++) {
        f[deb + i * pas] = buffer[2 * i] + t * buffer[2 * i + 1];
        f[deb + (i + n / 2) * pas] = buffer[2 * i] - t * buffer[2 * i + 1];

        t *= w;
    }
}

static void
test_glide_cb(const fixed h, const fixed W, const fixed Wangle,
              std::ostream &hfile)
{
  GlideSettings settings;
  settings.SetDefaults();

  GlidePolar polar(fixed(1));

  AircraftState ac;
  ac.wind.norm = fabs(W);
  if (negative(W)) {
    ac.wind.bearing = Angle::Degrees(fixed(180)+Wangle);
  } else {
    ac.wind.bearing = Angle::Degrees(Wangle);
  }
  ac.altitude = h;

  GeoVector vect(fixed(400.0), Angle::Zero());
  GlideState gs (vect, fixed(0), ac.altitude, ac.wind);
  GlideResult gr = MacCready::Solve(settings, polar, gs);

  gr.CalcDeferred();

  hfile << (double)W << " "
        << (double)Wangle << " "
        << (double)gr.vector.bearing.Degrees() << " "
        << (double)gr.cruise_track_bearing.Degrees() << " "
        << "\n";
}

vector< cplx > fft(const vector< cplx > &smp, bool rev) {
	int n = sz(smp);
	vector< cplx > a(n), w(n);

	for (int i = 0; i < n; ++i) {
		int ni = 0;
		for (int j = 1, x = i; j < n; j <<= 1, x >>= 1)
			ni <<= 1, ni |= x & 1;
		a[ni] = smp[i];
	}

	for (int i = 0; i < n; ++i)
		w[i] = polar(1.0, (rev ? -1 : 1) * 2.0 * i * kPi / n);

	for (int k = 1; k < n; k <<= 1) {
		vector< cplx > y(n);
		for (int i = 0, s = n / (2 * k); i < n; i += 2 * k)
			for (int j = 0, p = i; j < k; ++j, ++p) {
				y[p] = a[p] + w[j * s] * a[p + k];
				y[p + k] = a[p] - w[j * s] * a[p + k];
			}
		a = y;
	}

	if (rev) {
		for (int i = 0; i < n; ++i)
			a[i] /= n;
	}

	return a;
}

pair<circle, circle> tangent_circles(const line& l, const line& m, double r) {
    double th = arg(m.b - m.a) - arg(l.b - l.a);
    double ph = (arg(m.b - m.a) + arg(l.b - l.a)) / 2.0;
    point p = crosspointLL(l, m);
    point d = polar(r / sin(th / 2.0), ph);
    return mp(circle(p - d, r), circle(p + d, r));
}

void
PolarMaterial::computeQpProperties()
{
  std::vector<Real> polar(3);
  polar[0]=_polar_x_val[_qp];
  polar[1]=_polar_y_val[_qp];
  polar[2]=_polar_z_val[_qp];
  _polars[_qp]=polar;
  
  std::vector<RealGradient> polar_grad(3);
  polar_grad[0]=_polar_x_grad[_qp];
  polar_grad[1]=_polar_y_grad[_qp];
  polar_grad[2]=_polar_z_grad[_qp];
  _polar_grads[_qp]=polar_grad;
  
  _alpha1[_qp]=_alpha1_i;
  _alpha11[_qp]=_alpha11_i;
  _alpha12[_qp]=_alpha12_i;
  _alpha111[_qp]=_alpha111_i;
  _alpha112[_qp]=_alpha112_i;
  _alpha123[_qp]=_alpha123_i;
  _G11[_qp]=_G11_i;
  _G12[_qp]=_G12_i;
  _G44[_qp]=_G44_i;
  _G44P[_qp]=_G44P_i;
}

void BlobSmoother::digestFrame()
{
    int lIndexMin = -1;
    float lDistanceMax = 1000000000.f;
    for(int i = 0; i < mMatchingBlobs.size(); i++)
    {
        float lDistance = mMatchingBlobs[i].centroid.squareDistance(mPos);
        if(lDistance < lDistanceMax)
        {
            lIndexMin = i;
            lDistanceMax = lDistance;
        }
    }
    
    mShouldSendUpdate = true;
    if(lIndexMin >= 0)
    {
        mPos = mMatchingBlobs[lIndexMin].centroid;
        mIntensity += 0.02;
    }
    else // no match
    {
        if(mIntensity < 0.01f)
        {
            mShouldSendUpdate = false;
        }
        mIntensity -= 0.02;
    }
    
    mIntensity = ofClamp(mIntensity, 0.f, 1.f);
    polar(mPos.x - mCenter.x, -(mPos.y - mCenter.y), mRadius, mAngle);
}

void iac::initAC (void) {
  nr_double_t a = getPropertyDouble ("I");
  nr_double_t p = getPropertyDouble ("Phase");
  nr_complex_t i = polar (a, rad (p));
  allocMatrixMNA ();
  setI (NODE_1, +i); setI (NODE_2, -i);
}

void performHighPass(const cv::Mat& image, cv::Mat& res, int rad) {
    cv::Mat grey, tmp;
    cv::cvtColor(image, grey, CV_BGR2GRAY);


    grey.convertTo(grey, CV_32F);
    grey.copyTo(res);
    res.convertTo(res, CV_8U);
    std::vector<cv::Mat> planes(2, cv::Mat());
    std::vector<cv::Mat> polar(2, cv::Mat());

    cv::dft(grey, tmp, cv::DFT_COMPLEX_OUTPUT);
    cv::split(tmp, planes);
    cv::cartToPolar(planes[0], planes[1], polar[0], polar[1]);
    visualization(polar[0], tmp);
    concatImages(res, tmp, res);

    rearrangeQuadrants(polar[0]);
    highPassFilter(polar[0], rad);
    rearrangeQuadrants(polar[0]);

    visualization(polar[0], tmp);
    tmp.convertTo(tmp, res.type());
    concatImages(res, tmp, res);

    cv::polarToCart(polar[0], polar[1], planes[0], planes[1]);
    cv::merge(planes, tmp);
    cv::dft(tmp, tmp, cv::DFT_SCALE | cv::DFT_INVERSE | cv::DFT_REAL_OUTPUT);
    tmp.convertTo(tmp, CV_8U);
    concatImages(res, tmp, res);
}

void vccs::calcAC (nr_double_t frequency) {
  nr_double_t t = getPropertyDouble ("T");
  nr_complex_t g = polar (getPropertyDouble ("G"),
			  - 2.0 * M_PI * frequency * t);
  setY (NODE_2, NODE_1, +g); setY (NODE_3, NODE_4, +g);
  setY (NODE_3, NODE_1, -g); setY (NODE_2, NODE_4, -g);
}