#!/usr/bin/env python

import sys
import math
import time
import random

import numpy
import transformations
import cv2.cv as cv

def clamp(a, x, b):
    return numpy.maximum(a, numpy.minimum(x, b))

def norm(v):
    mag = numpy.sqrt(sum([e * e for e in v]))
    return v / mag

class Vec3:
    def __init__(self, x, y, z):
        self.v = (x, y, z)
    def x(self):
        return self.v[0]
    def y(self):
        return self.v[1]
    def z(self):
        return self.v[2]
    def __repr__(self):
        return "<Vec3 (%s,%s,%s)>" % tuple([repr(c) for c in self.v])
    def __add__(self, other):
        return Vec3(*[self.v[i] + other.v[i] for i in range(3)])
    def __sub__(self, other):
        return Vec3(*[self.v[i] - other.v[i] for i in range(3)])
    def __mul__(self, other):
        if isinstance(other, Vec3):
            return Vec3(*[self.v[i] * other.v[i] for i in range(3)])
        else:
            return Vec3(*[self.v[i] * other for i in range(3)])
    def mag2(self):
        return sum([e * e for e in self.v])
    def __abs__(self):
        return numpy.sqrt(sum([e * e for e in self.v]))
    def norm(self):
        return self * (1.0 / abs(self))
    def dot(self, other):
        return sum([self.v[i] * other.v[i] for i in range(3)])
    def cross(self, other):
        (ax, ay, az) = self.v
        (bx, by, bz) = other.v
        return Vec3(ay * bz - by * az, az * bx - bz * ax, ax * by - bx * ay)


class Ray:

    def __init__(self, o, d):
        self.o = o
        self.d = d

    def project(self, d):
        return self.o + self.d * d

class Camera:

    def __init__(self, F):
        R = Vec3(1., 0., 0.)
        U = Vec3(0, 1., 0)
        self.center = Vec3(0, 0, 0)
        self.pcenter = Vec3(0, 0, F)
        self.up = U
        self.right = R

    def genray(self, x, y):
        """ -1 <= y <= 1 """
        r = numpy.sqrt(x * x + y * y)
        if 0:
            rprime = r + (0.17 * r**2)
        else:
            rprime = (10 * numpy.sqrt(17 * r + 25) - 50) / 17
        print "scale", rprime / r
        x *= rprime / r
        y *= rprime / r
        o = self.center
        r = (self.pcenter + (self.right * x) + (self.up * y)) - o
        return Ray(o, r.norm())

class Sphere:

    def __init__(self, center, radius):
        self.center = center
        self.radius = radius

    def hit(self, r):
        # a = mag2(r.d)
        a = 1.
        v = r.o - self.center
        b = 2 * r.d.dot(v)
        c = self.center.mag2() + r.o.mag2() + -2 * self.center.dot(r.o) - (self.radius ** 2)
        det = (b * b) - (4 * c)
        pred = 0 < det

        sq = numpy.sqrt(abs(det))
        h0 = (-b - sq) / (2)
        h1 = (-b + sq) / (2)

        h = numpy.minimum(h0, h1)

        pred = pred & (h > 0)
        normal = (r.project(h) - self.center) * (1.0 / self.radius)
        return (pred, numpy.where(pred, h, 999999.), normal)

def pt2plane(p, plane):
    return p.dot(plane) * (1. / abs(plane))

class Plane:

    def __init__(self, p, n, right):
        self.D = -pt2plane(p, n)
        self.Pn = n
        self.right = right
        self.rightD = -pt2plane(p, right)
        self.up = n.cross(right)
        self.upD = -pt2plane(p, self.up)

    def hit(self, r):
        Vd = self.Pn.dot(r.d)
        V0 = -(self.Pn.dot(r.o) + self.D)
        h = V0 / Vd
        pred = (0 <= h)

        return (pred, numpy.where(pred, h, 999999.), self.Pn)

    def localxy(self, loc):
        x = (loc.dot(self.right) + self.rightD)
        y = (loc.dot(self.up) + self.upD)
        return (x, y)

# lena = numpy.fromstring(cv.LoadImage("../samples/c/lena.jpg", 0).tostring(), numpy.uint8) / 255.0

def texture(xy):
    x,y = xy
    xa = numpy.floor(x * 512)
    ya = numpy.floor(y * 512)
    a = (512 * ya) + xa
    safe = (0 <= x) & (0 <= y) & (x < 1) & (y < 1)
    if 0:
        a = numpy.where(safe, a, 0).astype(numpy.int)
        return numpy.where(safe, numpy.take(lena, a), 0.0)
    else:
        xi = numpy.floor(x * 11).astype(numpy.int)
        yi = numpy.floor(y * 11).astype(numpy.int)
        inside = (1 <= xi) & (xi < 10) & (2 <= yi) & (yi < 9)
        checker = (xi & 1) ^ (yi & 1)
        final = numpy.where(inside, checker, 1.0)
        return numpy.where(safe, final, 0.5)

def under(vv, m):
    return Vec3(*(numpy.dot(m, vv.v + (1,))[:3]))

class Renderer:

    def __init__(self, w, h, oversample):
        self.w = w
        self.h = h

        random.seed(1)
        x = numpy.arange(self.w*self.h) % self.w
        y = numpy.floor(numpy.arange(self.w*self.h) / self.w)
        h2 = h / 2.0
        w2 = w / 2.0
        self.r = [ None ] * oversample
        for o in range(oversample):
            stoch_x = numpy.random.rand(self.w * self.h)
            stoch_y = numpy.random.rand(self.w * self.h)
            nx = (x + stoch_x - 0.5 - w2) / h2
            ny = (y + stoch_y - 0.5 - h2) / h2
            self.r[o] = cam.genray(nx, ny)

        self.rnds = [random.random() for i in range(10)]

    def frame(self, i):

        rnds = self.rnds
        roll = math.sin(i * .01 * rnds[0] + rnds[1])
        pitch = math.sin(i * .01 * rnds[2] + rnds[3])
        yaw = math.pi * math.sin(i * .01 * rnds[4] + rnds[5])
        x = math.sin(i * 0.01 * rnds[6])
        y = math.sin(i * 0.01 * rnds[7])

        x,y,z = -0.5,0.5,1
        roll,pitch,yaw = (0,0,0)

        z = 4 + 3 * math.sin(i * 0.1 * rnds[8])
        print z

        rz = transformations.euler_matrix(roll, pitch, yaw)
        p = Plane(Vec3(x, y, z), under(Vec3(0,0,-1), rz), under(Vec3(1, 0, 0), rz))

        acc = 0
        for r in self.r:
            (pred, h, norm) = p.hit(r)
            l = numpy.where(pred, texture(p.localxy(r.project(h))), 0.0)
            acc += l
        acc *= (1.0 / len(self.r))

        # print "took", time.time() - st

        img = cv.CreateMat(self.h, self.w, cv.CV_8UC1)
        cv.SetData(img, (clamp(0, acc, 1) * 255).astype(numpy.uint8).tostring(), self.w)
        return img

#########################################################################

num_x_ints = 8
num_y_ints = 6
num_pts = num_x_ints * num_y_ints

def get_corners(mono, refine = False):
    (ok, corners) = cv.FindChessboardCorners(mono, (num_x_ints, num_y_ints), cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_NORMALIZE_IMAGE)
    if refine and ok:
        corners = cv.FindCornerSubPix(mono, corners, (5,5), (-1,-1), ( cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER, 30, 0.1 ))
    return (ok, corners)

def mk_object_points(nimages, squaresize = 1):
    opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1)
    for i in range(nimages):
        for j in range(num_pts):
            opts[i * num_pts + j, 0] = (j / num_x_ints) * squaresize
            opts[i * num_pts + j, 1] = (j % num_x_ints) * squaresize
            opts[i * num_pts + j, 2] = 0
    return opts

def mk_image_points(goodcorners):
    ipts = cv.CreateMat(len(goodcorners) * num_pts, 2, cv.CV_32FC1)
    for (i, co) in enumerate(goodcorners):
        for j in range(num_pts):
            ipts[i * num_pts + j, 0] = co[j][0]
            ipts[i * num_pts + j, 1] = co[j][1]
    return ipts

def mk_point_counts(nimages):
    npts = cv.CreateMat(nimages, 1, cv.CV_32SC1)
    for i in range(nimages):
        npts[i, 0] = num_pts
    return npts

def cvmat_iterator(cvmat):
    for i in range(cvmat.rows):
        for j in range(cvmat.cols):
            yield cvmat[i,j]

cam = Camera(3.0)
rend = Renderer(640, 480, 2)
cv.NamedWindow("snap")

#images = [rend.frame(i) for i in range(0, 2000, 400)]
images = [rend.frame(i) for i in [1200]]

if 0:
    for i,img in enumerate(images):
        cv.SaveImage("final/%06d.png" % i, img)

size = cv.GetSize(images[0])
corners = [get_corners(i) for i in images]

goodcorners = [co for (im, (ok, co)) in zip(images, corners) if ok]

def checkerboard_error(xformed):
    def pt2line(a, b, c):
        x0,y0 = a
        x1,y1 = b
        x2,y2 = c
        return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
    errorsum = 0.
    for im in xformed:
        for row in range(6):
            l0 = im[8 * row]
            l1 = im[8 * row + 7]
            for col in range(1, 7):
                e = pt2line(im[8 * row + col], l0, l1)
                #print "row", row, "e", e
                errorsum += e

    return errorsum

if True:
    from scipy.optimize import fmin

    def xf(pt, poly):
        x, y = pt
        r = math.sqrt((x - 320) ** 2 + (y - 240) ** 2)
        fr = poly(r) / r
        return (320 + (x - 320) * fr, 240 + (y - 240) * fr)
    def silly(p, goodcorners):
    #    print "eval", p

        d = 1.0 # - sum(p)
        poly = numpy.poly1d(list(p) + [d, 0.])

        xformed = [[xf(pt, poly) for pt in co] for co in goodcorners]

        return checkerboard_error(xformed)

    x0 = [ 0. ]
    #print silly(x0, goodcorners)
    print "initial error", silly(x0, goodcorners)
    xopt = fmin(silly, x0, args=(goodcorners,))
    print "xopt", xopt
    print "final error", silly(xopt, goodcorners)

    d = 1.0 # - sum(xopt)
    poly = numpy.poly1d(list(xopt) + [d, 0.])
    print "final polynomial"
    print poly

    for co in goodcorners:
        scrib = cv.CreateMat(480, 640, cv.CV_8UC3)
        cv.SetZero(scrib)
        cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), [xf(pt, poly) for pt in co], True)
        cv.ShowImage("snap", scrib)
        cv.WaitKey()

    sys.exit(0)

for (i, (img, (ok, co))) in enumerate(zip(images, corners)):
    scrib = cv.CreateMat(img.rows, img.cols, cv.CV_8UC3)
    cv.CvtColor(img, scrib, cv.CV_GRAY2BGR)
    if ok:
        cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), co, True)
    cv.ShowImage("snap", scrib)
    cv.WaitKey()

print len(goodcorners)
ipts = mk_image_points(goodcorners)
opts = mk_object_points(len(goodcorners), .1)
npts = mk_point_counts(len(goodcorners))

intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
distortion = cv.CreateMat(4, 1, cv.CV_64FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# focal lengths have 1/1 ratio
intrinsics[0,0] = 1.0
intrinsics[1,1] = 1.0
cv.CalibrateCamera2(opts, ipts, npts,
           cv.GetSize(images[0]),
           intrinsics,
           distortion,
           cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
           cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
           flags = 0) # cv.CV_CALIB_ZERO_TANGENT_DIST)
print "D =", list(cvmat_iterator(distortion))
print "K =", list(cvmat_iterator(intrinsics))
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
for img in images:
    r = cv.CloneMat(img)
    cv.Remap(img, r, mapx, mapy)
    cv.ShowImage("snap", r)
    cv.WaitKey()