#!/bin/env python
# coding: utf-8

# Calcul du dépointage à partir d'images du collimateur
# Frederic.Meynadier@aerov.jussieu.fr Janvier 2008


import params

import pyfits
from optparse import OptionParser
import sys

from numpy import *
from math import *

#from fitgaussian import *

VERSION='0.0'

label = sys.argv[1]

(ref,img,liste_points,sh_guess) = params.pset(label)


sh_guess = array(sh_guess)

size=128
subsize=size/2

for point in liste_points :
    limites = [size,2048-size]
    target = array(point) + sh_guess
    print target
    if (target[0] < limites[0] or target[0] > limites[1]\
        or target[1] < limites[0] or target[1] > limites[1]):
        print "Point ",point," hors cadre dans l'image destination"
        sys.exit(1)

cor_list = []
for coords in liste_points :

    tgt_ref=array(coords)
    tgt_img = tgt_ref + sh_guess




    ## parser = OptionParser()
    ## parser.add_option("-i","--input",dest="InputFilename",
    ##                   help="Input filename", metavar="RAWIMAGE")
    ## parser.add_option("-o","--output",dest="OutputFilename",
    ##                   help="Output Filename", metavar="FITSIMAGE")

    #(options, args) = parser.parse_args ()


    # Ouverture de l'image de référence
    refhdulist = pyfits.open(ref)
    refdata = array(refhdulist[0].data[tgt_ref[1]-size/2:tgt_ref[1]+size/2,\
                                       tgt_ref[0]-size/2:tgt_ref[0]+size/2])
    refhdulist[0].data = refdata
    refhdulist.writeto('ref'+str(len(cor_list))+'.fits',clobber=True)


    # Ouverture de l'image à mesurer

    imghdulist = pyfits.open(img)
    imgdata = imghdulist[0].data[tgt_img[1]-subsize/2:tgt_img[1]+subsize/2,\
                                 tgt_img[0]-subsize/2:tgt_img[0]+subsize/2]

    imghdulist[0].data = imgdata
    imghdulist.writeto('img'+str(len(cor_list))+'.fits',clobber=True)

    # Etablissement de la carte de corrélation

    corsize = size-subsize+1

    cordata = zeros([corsize,corsize])

    motif = sqrt(sum(sum(imgdata*imgdata)))

    for i in range(corsize):
        for j in range(corsize):
            refpart = refdata[i:i+subsize,j:j+subsize]
            cordata[i,j] = \
                 sum(sum( imgdata * refpart))\
                 / (motif * sqrt(sum(sum(refpart*refpart))))


    cor_list.append(cordata)

    refhdulist[0].data = cordata
    prihdr = refhdulist[0].header

    prihdr.update('CRPIX1',(corsize-1)/2,'crpix1')
    prihdr.update('CRPIX2',(corsize-1)/2,'crpix2')
    prihdr.update('CRVAL1',sh_guess[0],'crval1')
    prihdr.update('CRVAL2',sh_guess[1],'crval2')
    prihdr.update('CDELT1',-1.0,'crdelt1')
    prihdr.update('CDELT2',-1.0,'crdelt2')
    prihdr.update('CROTA1',0.0,'crota1')
    prihdr.update('CROTA2',0.0,'crota2')
    prihdr.update('CTYPE1',' ','ctype1')
    prihdr.update('CTYPE2',' ','ctype2')


    refhdulist.writeto('cor'+str(len(cor_list)-1)+'.fits',clobber=True)        



cor_prod = cor_list[0]

for i in range(1,len(cor_list)):
    cor_prod *= cor_list[i]


refhdulist[0].data = cor_prod
prihdr = refhdulist[0].header

crpix1 = (corsize-1)/2
crpix2 = (corsize-1)/2
crval1 = sh_guess[0]+1
crval2 = sh_guess[1]+1
cdelt1 = -1.0
cdelt2 = -1.0

prihdr.update('CRPIX1',crpix1,'crpix1')
prihdr.update('CRPIX2',crpix2,'crpix2')
prihdr.update('CRVAL1',crval1,'crval1')
prihdr.update('CRVAL2',crval2,'crval2')
prihdr.update('CDELT1',cdelt1,'crdelt1')
prihdr.update('CDELT2',cdelt2,'crdelt2')
prihdr.update('CROTA1',0.0,'crota1')
prihdr.update('CROTA2',0.0,'crota2')
prihdr.update('CTYPE1',' ','ctype1')
prihdr.update('CTYPE2',' ','ctype2')


refhdulist.writeto('cor_prod.fits',clobber=True)        

refhdulist.close()
imghdulist.close()

# On cherche le maximum du tableau -> passage en 1D
lin_max = argmax(cor_prod.ravel())
x_max = lin_max % corsize
y_max = floor(lin_max / corsize)
#print "Pixel max :",(int(x_max),int(y_max))
#print "Décalage (entier) correspondant : (%d,%d)" %\
#      (int((x_max-crpix1)*cdelt1+crval1-1),
#       int((y_max-crpix2)*cdelt2+crval2-1))

# On cherche le centroïde (bloc 2*(npfit)+1**2)
npfit = 1


bloc = cor_prod[y_max-npfit:y_max+npfit+1,
                x_max-npfit:x_max+npfit+1]
bloc = bloc - min(bloc.ravel())

og = array([0.0,0.0])

for i in range(2*npfit+1):
    for j in range(2*npfit+1):
        vec = array([j+1,i+1])
        og += vec*bloc[i,j]

og /= sum(sum(bloc))
og -=1


print "ref :",ref
print "img :",img

#print "Centroïde en : %f %f" % (og[0]+x_max,og[1]+y_max)
print "Décalage centroïde : %f %f" % (((og[0]+x_max)-crpix1)*cdelt1+crval1,
                                      ((og[1]+y_max)-crpix2)*cdelt2+crval2)


#params = fitgaussian(bloc)
#fit = gaussian(*params)
#(height, x, y, width_x, width_y) = params
#print "Décalage gaussienne : %f %f" % (((x+x_max)-crpix1)*cdelt1+crval1,
#                                      ((y+y_max)-crpix2)*cdelt2+crval2)