//////////////////////////////////////////////////////////////////////////////
//
// Phasor.cpp, (c) 2006-2017, Ryan A. Colyer
//
// See Phasor.h for documentation and notes.
//
//////////////////////////////////////////////////////////////////////////////
//
// Boost Software License - Version 1.0 - August 17th, 2003
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
//////////////////////////////////////////////////////////////////////////////

#include "Phasor.h"
#include <math.h>

namespace PhasorApp {
  Phasor::Phasor() {
    Clear();
  }


  void Phasor::Clear() {
    bigG = 0;
    bigS = 0;
    bigG2 = 0;
    bigS2 = 0;
    counts = 0;
  }


  float Phasor::GetM() const {
    return sqrt(bigG*bigG+bigS*bigS) / counts;
  }


  float Phasor::GetPhi() const {
    if (bigS == 0 && bigG == 0) {
      return 0;
    }
    else {
      return atan2(bigS, bigG);
    }
  }


  float Phasor::GetM2() const {
    return sqrt(bigG2*bigG2+bigS2*bigS2) / counts;
  }


  float Phasor::GetPhi2() const {
    if (bigS2 == 0 && bigG2 == 0) {
      return 0;
    }
    else {
      return atan2(bigS2, bigG2);
    }
  }


  void Phasor::SetGS(float g, float s) {
    bigG = g;
    bigS = s;
    counts = 1;
  }


  void Phasor::SetMPhi(float m, float phi) {
    SetGS(m * cos(phi), m * sin(phi));
  }


  void Phasor::SetTau(float tau, float freq) {
    float tau_freq;
    float m, phi;

    tau_freq = (tau * 2 * 3.14159265358979 * freq);

    m = sqrt(1.0 / (tau_freq * tau_freq + 1));
    phi = atan(tau_freq);

    SetMPhi(m, phi);
  }


  float Phasor::GetTauP(float freq) const {
    float omega;
    float phi, tp;

    omega = (2 * 3.14159265358979 * freq);
    phi = GetPhi();

    tp = tan(phi) / omega;

    return tp;
  }


  float Phasor::GetTauM(float freq) const {
    float omega;
    float m, tm;

    omega = (2 * 3.14159265358979 * freq);
    m = GetM();

    if (m <= 1) {
      tm = sqrt((1/(m*m))-1) / omega;
    }
    else {
      tm = 0-(sqrt(1-(1/(m*m))) / omega);
    }

    return tm;
  }


  void Phasor::Reference(float m_fact, float phi_shift) {
    float m, phi;

    m = sqrt(bigG*bigG+bigS*bigS);
    phi = atan2(bigS, bigG);

    m *= m_fact;
    phi += phi_shift;

    bigG = m * cos(phi);
    bigS = m * sin(phi);
  }


  void Phasor::ReferenceBG(float m_fact, float phi_shift, float background) {
    float m, phi;

    m = sqrt(bigG*bigG+bigS*bigS);
    phi = atan2(bigS, bigG);

    m *= m_fact;
    phi += phi_shift;

    m *= counts / (counts - background);

    bigG = m * cos(phi);
    bigS = m * sin(phi);
  }


  // If this phasor is the reference sample, reference to the target value.
  float Phasor::IsReferenceMod(Phasor target) const {
    return (target.GetM() / GetM());
  }


  // If this phasor is the reference sample, reference to the target value.
  float Phasor::IsReferencePhi(Phasor target) const {
    return (target.GetPhi() - GetPhi());
  }


  // If this phasor is the reference sample, reference to the target value.
  float Phasor::IsReferenceModBG(Phasor target, float background) const {
    float m = GetM() * (counts / (counts - background));
    return (target.GetM() / m);
  }


  void Phasor::PhaseReference(float phi_shift) {
    float m, phi;

    phi = atan2(bigS, bigG);
    m = sqrt(bigG*bigG+bigS*bigS);

    phi += phi_shift;

    bigG = m * cos(phi);
    bigS = m * sin(phi);
  }



  // Only valid for unreferenced phasor.
  float Phasor::SigmaPhi() const {
    return (sqrt(1-GetM2()*cos(2*GetPhi()-GetPhi2()))
            / (GetM()*sqrt(2*counts)));
  }


  // Only valid for unreferenced phasor.
  // m_fact = 1/m_IR from paper.
  float Phasor::SigmaM(float m_fact) const {
    return (m_fact*sqrt(1-2*GetM()*GetM()+GetM2()*cos(2*GetPhi()-GetPhi2()))
            / (sqrt(2*counts)));
  }


  // Only valid for unreferenced phasor.
  // phi_shift = -phi_IR from paper.
  float Phasor::SigmaTauP(float freq_times_harmonic, float phi_shift) const {
    float cosval;
    cosval = cos(GetPhi()+phi_shift);
    return (SigmaPhi() / (TWO_PI * freq_times_harmonic * cosval*cosval));
  }


  // Only valid for unreferenced phasor.
  // m_fact = 1/m_IR from paper.
  float Phasor::SigmaTauM(float freq_times_harmonic, float m_fact) const {
    float m, msqr;
    m = GetM();
    msqr = m*m;
    return (SigmaM(m_fact)
            / (TWO_PI * freq_times_harmonic * msqr * sqrt(1-msqr)));
  }
}