#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# |                                                                       |
# |  Copyright (c) 2022 by Paul Scherrer Institute (http://www.psi.ch)    |
# |              Based on Zac great first implementation                  |
# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
# *-----------------------------------------------------------------------*
'''
coordinate systems, optical center, xray axis, pixel sizes etc.
'''
import logging
import numpy as np
_log = logging.getLogger(__name__)

class geometry:

  def __init__(self):
    pass
  def find_optical_center(self,p):
    # p is an array of
    # at zoom out: (p1x,p1y),(p2x,p2y),(p3x,p3y),...
    # at zoom in : (p1x,p1y),(p2x,p2y),(p3x,p3y),...
    #
    # the pixel positions are given in chip pixel coordinates (0/0= top/right)
    # and ignore roi and binning
    # the returned (cx,cy) is the pixel koordinate that does not change with zoom
    # this coordinate represents also the origin of other coordinates
    pass

  def zoom2pixsz(self,zoom):
    # this returns the pixel size at a given zoom level
    # the returned value is a 2x2 matrix:
    # [pxx pxy]
    # [pyx pyy] which is interpolated out of a lookup table
    #
    # [pxx pxy] [nx]
    # [pyx pyy]*[ny] results in a vector in meter of a vector [nx,ny] pixels in x and y direction
    pass

  def set_zoom2pixsz(self,meas):
    #calculates _lut_z2p out of measurements
    # the _lut_z2p is dictionaty a lookuptable
    # zoom {1,200,400,600,800,1000}
    #[pxx pxy]
    #[pyx pyy]
    self._lut_z2p=_lut_z2p={
      'zoom': np.array((1,200,400,600,800,1000),dtype=np.float32),
      'pixsz': np.array(
        #((pxx,pxy),(pyx,pyy)), # zoom n
        ((( 1,0),(0, 1)), # zoom 1
        (( 2,0),(0, 2)), # zoom 200
        (( 4,0),(0, 4)), # zoom 400
        (( 6,0),(0, 6)), # zoom 600
        (( 8,0),(0, 8)), # zoom 800
        ((10,0),(0,10))), # zoom 1000
        dtype=np.float32)}

    n=len(meas)
    zoom =np.ndarray(shape=n,dtype=np.float32)
    pixsz=np.ndarray(shape=(n,2,2),dtype=np.float32)

    for i,(k,v) in enumerate(meas):
      pass
    self._lut_z2p={ 'zoom': zoom, 'pixsz': pixsz}

  def autofocus(self):
    # cam   camera object
    # mot   motor object
    # rng   region (min max relative to current position) to seek
    # n     number of images to take in region
    # roi   region of interrest to calculate sharpness
    # mode  mode to calculate sharpness (sum/max-min/hist?  of edge detection in roi)
    pass

  def pix2pos(self,p,zoom=None):
    # returns the position m(x,y) in meter relative to the optical center at a given zoom level of the pixel p(x,y)
    # if zoom=None, the last zoom value is used
    pass

  def pos2pix(self,p,zoom=None):
    # returns the pixel p(x,y) of the position m(x,y) in meter relative to the optical center at a given zoom level
    # if zoom=None, the last zoom value is used
    pass

  def optctr2xray(self):
    # returns the vector m(x,y) of the optical center to the xray
    pass



if __name__ == "__main__":
  import argparse
  logging.basicConfig(level=logging.DEBUG,format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s ')

  parser = argparse.ArgumentParser()
  parser.add_argument('-m', '--mode', help='mode')
  parser.add_argument("-t", "--test", help="test sequence", action="store_true")

  args = parser.parse_args()
  _log.info('Arguments:{}'.format(args.__dict__))


  # recorded data:
  #                   x    y        x    y
  #Zoom 1    x+1.2   97  543 ->  1100  507
  #          y+1.0  607  941 ->   575  106
  #Zoom 200  x+0.6   93  504 ->   853  510
  #          y+0.4  475  897 ->   472  157
  #Zoom 400  x+0.5   88  615 ->  1094  579
  #          y+0.4  705  991 ->   673  190
  #Zoom 600  x+0.3   32  460 ->   103  416
  #          y+0.25 551  937 ->   520  106
  #Zoom 800  x+0.18  65  524 ->  1050  484
  #          y+0.14 632  946 ->   602  168
  #Zoom 1000 x+0.1  121  632 ->  1044  592
  #          y+0.08 593  883 ->   570  145


  measure={
     1:{'x': (+1.2  , (  97, 543),(1100, 507)),
        'y': (+1.0  , ( 607, 941),( 575, 106))},
   200:{'x': (+0.6  , (  93, 504),( 853, 510)),
        'y': (+0.4  , ( 475, 897),( 472, 157))},
   400:{'x': (+0.5  , (  88, 615),(1094, 579)),
        'y': (+0.4  , ( 705, 991),( 673, 190))},
   600:{'x': (+0.3  , (  32, 460),( 103, 416)),
        'y': (+0.25 , ( 551, 937),( 520, 106))},
   800:{'x': (+0.18 , (  65, 524),(1050, 484)),
        'y': (+0.14 , ( 632, 946),( 602, 168))},
  1000:{'x': (+0.1  , ( 121, 632),(1044, 592)),
        'y': (+0.08 , ( 593, 883),( 570, 145))},
  }

  obj=geometry()
  obj.calc_zoom2pixsz(measure)