#! /usr/bin/python
#############################################################################
## 
#  \file qphase.py
#  \brief Phase correction algorithm of quadrature signal.
#  \version 0.1
#  \author Dimitri Denk
#  \date 21.12.2016
#
# This file demonstrates phase correction algorithm and calculation of
# correction coefficients.
#
# Last modification: 12.01.2017
# 
# Copyright (c) 2016-2017
# Dimitri Denk
#
# Permission to use, copy, modify, distribute and sell this software
# and its documentation for any purpose is hereby granted without fee,
# provided that the above copyright notice appear in all copies and
# that both that copyright notice and this permission notice appear
# in supporting documentation. Author makes no representations about 
# the suitability of this software for any purpose. 
# It is provided "as is" without express or implied warranty.
#
#############################################################################

import math
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.figure import SubplotParams
import matplotlib.cm

# additional phase shift between components of quadrature signal
phase = -8.

# angle measurement range
theta = np.linspace(-1, 1, 100)
# component A of quadrature signal
a = np.sin(theta * np.pi) * .8
# component B of quadrature signal with additional phase shift
b = np.cos(theta * np.pi + phase * np.pi/ 180) * .8

# Calculation of correction coefficients
k1 = np.sin((phase / 2) * np.pi / 180)
k2 = 1. / (1 - k1**2)**.5
print "phase shift", phase, "k1", k1, "k2", k2

# symmetric correction of both signals
a1 = (a + k1 * b) * k2
b1 = (b + k1 * a) * k2

# plotting of results
sp = SubplotParams(left=0.15, bottom=0.1, right=0.9, top=0.92, wspace=0., hspace=0.)
fig = plt.figure(frameon=False, subplotpars=sp, figsize=(5., 5.))
ax1 = fig.add_subplot(1, 1, 1)

ax1.set_title("Quadrature signal")
ax1.set_aspect(1)
ax1.grid(True)
ax1.plot(a, b, 'r', label='Original', linewidth=5., alpha=0.2)
ax1.plot(a1, b1, 'g', label='Corrected', linewidth=2.)
ax1.legend()
ax1.set_xlim(-1., 1.)
ax1.set_ylim(-1., 1.)
ax1.set_xlabel("A")
ax1.set_ylabel("B")

#fig.savefig("quadrature_signal.png", format='png', dpi=75)

plt.show()

#EOF