##^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^Lance Simms, Stanford University 2009
##FitSinusoid
##
##PURPOSE:
##  Provides fitting and error functions for sinusoids
##INPUTS
##  x - The time points
##  y - The intensity in ADU
##      p[0] = Amplitude
##      p[1] = Frequency 
##      p[2] = Phase
##OPTIONAL INPUTS:
##  Col - 0 - Do the col_deriv=0
##        1 - Do the col deriv=1, apparently this works alot better
##
execfile('/afs/slac/u/ki/lances/python/python_HxRG/HxRG_Setup.py')
import pdb

def FitSinusoid(p, x, y):

  SinusoidFunc = lambda p, x    : p[0]*sin(2*pi*p[1]*x-p[2])+p[3]
  SinusoidErr  = lambda p, x, y : y-SinusoidFunc(p,x)
  p0 = p
  
  #Now do the fit      
  p1, Success = optimize.leastsq(SinusoidErr, p0[:], \
                args = (x, y), full_output=0,\
                maxfev=10000000.,\
                Dfun = SinusoidJacobian, col_deriv=0) 
                #xtol = 1.e-12, gtol=1e-15, ftol=1e-15)
  return p1

def SinusoidJacobian(p, x, y, Col=0):
  #'The Jacobian of the errfunc is -Jacobian of the func'
  A, f, phi, off= p
  if Col == 1:
    J = zeros([4, len(x)], dtype=float32 )

    ##dI/dA
    J[0,:] = -sin(2*pi*f*x-phi)

    ##dS/df
    J[1,:] = -A*(2*pi*x)*cos(2*pi*f*x-phi)

    ##dS/dphi
    J[2,:] = A*cos(2*pi*f*x-phi) 

    ##dS/doff
    J[3,:] = -1

  elif Col == 0:
    J = zeros([len(x), 4], dtype=float32 )

    ##dI/dA
    J[:,0] = -sin(2*pi*f*x-phi)

    ##dS/df
    J[:,1] = -A*(2*pi*x)*cos(2*pi*f*x-phi)

    ##dS/dphi
    J[:,2] = A*cos(2*pi*f*x-phi)

    ##dS/doff
    J[:,3] = -1

  return J