import numpy
from epoint import *


## Projective transformation of a point with a matrix A.
#  
#  Takes a point as a horizontal vector and multiplies it transposed with A from left.
#  
#  @return transformed point as a numpy.array
def transformPoint(point,A):
  return (A*numpy.matrix(point).transpose()).getA1()


class Grid:
  ## Creates a Grid from the provided Corners object.
  #  
  #  Finds the vanishing points of the board lines (corner points define perspectively transformed parallel lines). The vanishing points define the image horizon.
  #  
  #  The horizon can be used to construct a matrix for affine rectification of the image (restoring parallel lines parallelism). We transform the corner points by this matrix,
  #  interpolate them to get proper intersections' coordinates and then transform these back to get their placement at the original image.
  #  
  #  The result is stored in grid.intersections, a boardSize*boardSize list with [row][column] coordinates.
  #  
  #  @param corners a properly initialized Corners object. !! Needs a check for the proper initialization.
  def __init__(self,corners):
    # ad
    # bc
    a,b,c,d=[c.toProjective() for c in corners.corners]
    
    p1=numpy.cross(a,b)
    p2=numpy.cross(c,d)
    vanish1=numpy.cross(p1,p2)
    # !! 32 bit int can overflow. keeping it reasonably small. might want to use a cleaner solution
    vanish1=EPoint.fromProjective(vanish1).toProjective() # !! EPoint fails with point in infinity
    
    p3=numpy.cross(a,d)
    p4=numpy.cross(b,c)
    vanish2=numpy.cross(p3,p4)
    vanish2=EPoint.fromProjective(vanish2).toProjective()
    
    horizon=numpy.cross(vanish1,vanish2)

    horizon=EPoint.fromProjective(horizon).toProjective()
    
    rectiMatrix=numpy.matrix([horizon,[0,1,0],[0,0,1]])
    rectiMatrixInv=numpy.linalg.inv(rectiMatrix)
    
    
    affineCorners=[EPoint.fromProjective(transformPoint(x,rectiMatrix)) for x in (a,b,c,d)]
    x=[affineCorners[0]-affineCorners[3],affineCorners[1]-affineCorners[2],affineCorners[0]-affineCorners[1],affineCorners[3]-affineCorners[2]]
    
    self.intersections=[]
    boardSize=19
    for r in range(boardSize):
      self.intersections.append([None]*boardSize)
      rowStart=(affineCorners[0]*(boardSize-1-r)+affineCorners[1]*r) / (boardSize-1)
      rowEnd=(affineCorners[3]*(boardSize-1-r)+affineCorners[2]*r) / (boardSize-1)
      
      for c in range(boardSize):
        affineIntersection=(rowStart*(boardSize-1-c)+rowEnd*c) / (boardSize-1)
        self.intersections[r][c]=EPoint.fromProjective(transformPoint(affineIntersection.toProjective(),rectiMatrixInv))



# from corners import Corners
# corn=Corners()

# corn.add(106,86)
# corn.add(57,321)
# corn.add(416,320)
# corn.add(365,86)
# corn.add(365,88)

# x=Grid(corn)