#!/usr/bin/env python
#
# obj2abs.py : Project points along axis z to recreate a 2D depth map
# from the 3D mesh. Use FRGC file format (.abs).
# This function use the face triangle with z interpolation (slower than using only the points)
#
#
# Copyright (C) 2010  Clement Creusot
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

import sys
import os
import string
import getopt


def print_help():
    print "Usage: "+sys.argv[0]+" x y filein.obj [fileout.abs]"
    print " option : -s strict x,y size (dont enforce square pixels)"
    print "          -c FILE print reverse correspondance pixel->vertexId in FILE"
    sys.exit()

def print_error(*str):
    print "ERROR: ",
    for i in str:
        print i,
    print
    sys.exit()


#-------------------------------------------------------------------------
import time
import math
startTime = 0
lastTime = 0
discarded = 0
def PrintProgressionBar(reached, total, comments, bar_width=40, out=sys.stderr):
    global startTime, lastTime, discarded

    if(reached==0):
        startTime = 0
        lastTime = 0
        discarded = 0
    rest = ""
    if cmp(comments[0:9],"remaining")==0:
        rest = comments[9:]
        if startTime ==0:
            startTime = time.time()
            lastTime = startTime
            comments = "unknown"
        else:
            #if the mean is too low to be reliable
            #we use the last period and start to compute a new mean
            if (reached == discarded):
                mean = (time.time() - startTime)
            else:
                mean = (time.time() - startTime)/(reached-discarded)
            last = (time.time() - lastTime)
            if last > 5*mean:
                seconds = last*(total-reached)
                startTime = time.time()
                discarded = reached
            else:
                seconds = mean *(total-reached)
            
            comments = time.strftime("%H:%M:%S", time.gmtime(seconds))
            lastTime = time.time()

    run_symbol=['|','\\','-','/']

    percent = float(reached)/float(total) * 100
    done = int(math.floor(float(reached)/float(total) * bar_width))
    percent_str = "%(p)3.2f" % {"p":percent}
    percent_str = percent_str.rjust(6, ' ')

    sentence = "|" + "="*done + " "*(bar_width-done) + run_symbol[(int(total-reached))%4] + " ("+percent_str+"%) " + comments    

    sentence += " "+rest[-(79-len(sentence)):]

    out.write("\r"+sentence.ljust(79,' '))
    if reached==total:
        out.write("\n")
    out.flush()


#-------------------------------------------------------------------------

strictSize = False
reverseCorrespondence = False
corFile = ""
optlist, args = getopt.getopt(sys.argv[1:], 'hsc:')
for opt,value in optlist:
    if opt in ("-h", "--help"):
        print_help()
    elif opt in ("-s"):
        strictSize = True
    elif opt in ("-c"):
        reverseCorrespondence = True
        corFile = value
    else:
        print "Unhandled option"
        print_help()


if (len(args) < 3):
    print_help()

# 2D image description
height = int(args[0]);
width = int(args[1]);
useFaces = True

objfilename = args[2];
absfilename = args[2]+".abs"
if (len(args) == 4):
    absfilename = args[3];

subdiv  = 10 # divide x and y dimension into 10 regions for local face search
faces = []
pointStore = []
for i in range(subdiv+1):
    tab = []
    tab2 = []
    for i in range(subdiv+1):    
        tab.append([])
        tab2.append([])
    faces.append(tab)
    pointStore.append(tab2)

def GetPosition(x,y):
    global dx,dy,height,width,mini,maxi

    if x < mini[0] or x > maxi[0]:
        print_error("x out of bounds",x)
    if y < mini[1] or y > maxi[1]:
        print_error("y out of bounds",y)
    # 
    #  .---. .---. 
    #  |   | |   | 
    #^ |   | |   |
    #j o---' o---'
    #  i >
    i = int((x-mini[0])/dx) 
    j = int((maxi[1]-y)/dy)
    return j*width+i,i,j



mini = [ 9999999.9, 9999999.9, 9999999.9]
maxi = [-9999999.9,-9999999.9,-9999999.9]
points = []
image = {}

# read input obj file
objfile = open(objfilename, "r")
for line in objfile.readlines():
    t = string.split(line)
    if t!= []:
        if cmp(t[0], "v")==0 :
            # Read point position + determine min/max
            # t = ['v', x, y, z]
            mini[0] = min(mini[0],float(t[1]))
            mini[1] = min(mini[1],float(t[2]))
            mini[2] = min(mini[2],float(t[3]))
            maxi[0] = max(maxi[0],float(t[1]))
            maxi[1] = max(maxi[1],float(t[2]))
            maxi[2] = max(maxi[2],float(t[3]))
            points.append(map(float,t[1:]))
objfile.close()

objfile = open(objfilename, "r")
for line in objfile.readlines():
    t = string.split(line)
    if t!= []:
        if cmp(t[0], "f")==0 :
            if image == {}:
                #Initialise 
                a = (maxi[0]-mini[0])/width
                b = (maxi[1]-mini[1])/height
                dx = a
                dy = b
                if not strictSize:
                    # Keep ratio, squared pixels
                    dx = max(a,b)
                    dy = max(a,b)
                    width = int((maxi[0]-mini[0])/dx)
                    height = int((maxi[1]-mini[1])/dy)
                subI = width/(subdiv-1)
                subJ = height/(subdiv-1)
                #Prepare structure
                image = {"flag":[0]*(height*width),
                         "closestId":[0]*(height*width),
                         "x":[0.0]*(height*width),
                         "y":[0.0]*(height*width),
                         "z":[-99999999.0]*(height*width)}
         
            # Read triangle
            a = points[int(string.split(t[1],"/")[0])-1]
            b = points[int(string.split(t[2],"/")[0])-1]
            c = points[int(string.split(t[3],"/")[0])-1]
            posa,ia,ja = GetPosition(a[0],a[1])
            posb,ib,jb = GetPosition(b[0],b[1])
            posc,ic,jc = GetPosition(c[0],c[1])

            # determine areas indices for the three points
            Ia = ia/subI; Ja = ja/subJ
            Ib = ib/subI; Jb = jb/subJ
            Ic = ic/subI; Jc = jc/subJ
            # determine bounding box
            Imin = min(Ia,min(Ib,Ic))
            Imax = max(Ia,max(Ib,Ic))
            Jmin = min(Ja,min(Jb,Jc))
            Jmax = max(Ja,max(Jb,Jc))
            # Add current triangle to local areas
            IDS = map(lambda x:int(string.split(x,"/")[0])-1,t[1:4])
            for m in range(Imax-Imin+1):
                for n in range(Jmax-Jmin+1):
                    faces[m+Imin][n+Jmin].append({"points":[a,b,c],"ids":IDS})


if image == {}: # in case no facet is present
    useFaces = False
    #Initialise 
    print "Bounding Box:"
    print mini[0],maxi[0]
    print mini[1],maxi[1]
    print mini[2],maxi[2]

    a = (maxi[0]-mini[0])/width
    b = (maxi[1]-mini[1])/height
    dx = a
    dy = b
    if not strictSize:
        # Keep ratio, squared pixels
        dx = max(a,b)
        dy = max(a,b)
        width = int((maxi[0]-mini[0])/dx)
        height = int((maxi[1]-mini[1])/dy)
    subI = width/(subdiv-1)
    subJ = height/(subdiv-1)
    #Prepare structure
    image = {"flag":[0]*(height*width),
             "closestId":[0]*(height*width),
             "x":[0.0]*(height*width),
             "y":[0.0]*(height*width),
             "z":[-99999999.0]*(height*width)}

    for i,p in enumerate(points):
        posa,ia,ja = GetPosition(p[0],p[1])
        Ia = ia/subI; Ja = ja/subJ
        pointStore[Ia][Ja].append({"point":p,"id":i})

objfile.close()



def sub_3(x,y):
    res = []
    for i in range(3):
        res.append(x[i]-y[i])
    return res

def DotProduct_2(in1, in2):
    return (in1[0])*(in2[0]) + (in1[1])*(in2[1])

def IntersectTriangle(P,A,B,C):
    v0 = sub_3(C,A)
    v1 = sub_3(B,A)
    v2 = sub_3(P,A)

    d00 = DotProduct_2(v0, v0)
    d01 = DotProduct_2(v0, v1)
    d02 = DotProduct_2(v0, v2)
    d11 = DotProduct_2(v1, v1)
    d12 = DotProduct_2(v1, v2)
    if (d00 * d11 - d01 * d01) == 0:
        return False, 0

    inv = 1. / (d00 * d11 - d01 * d01)
    u = (d11 * d02 - d01 * d12) * inv
    v = (d00 * d12 - d01 * d02) * inv    
    
    # return true if point in triangle 
    # + interpolation of the z coordinate
    z = a[2]+u*v0[2]+v*v1[2]
    return (u > 0) and (v > 0) and (u + v < 1), z


def ClosestPoint2D(P,A,B,C):
    V = [sub_3(P,A),
         sub_3(P,B),
         sub_3(P,C)]
    mini = 9999999
    argmin = -1
    for i in range(3):
        d = (V[i][0]**2+V[i][1]**2)
        if (d < mini):
            mini = d
            argmin = i
    return argmin

# for each output pixel
totalNb = width*height
for i in range(width):
    for j in range(height):
        PrintProgressionBar(i*height+j+1,totalNb,"remaining")        
        pos = j*width+i
        # Determine which area to search
        I = i/subI
        J = j/subJ
        # Get coordinates
        x = (i+0.5)*dx+mini[0]
        y = -(j+0.5)*dy+maxi[1]
        
        # Search the local area for faces.
        # if point intersect several triangles
        # keep the maximum z coordinate induced
        found = False
        closestId = -1
        if useFaces:
            z = -999999.000000
            for D in faces[I][J]:
                a,b,c = D["points"]
                inter,zp = IntersectTriangle([x,y,0],a,b,c)            
                if inter:
                    found = True
                    if zp > z:
                        z = zp;
                        if reverseCorrespondence:
                            closestId = D["ids"][ClosestPoint2D([x,y,0],a,b,c)]
        else:
            z = 0        
            L = []
            minDist = 9999999.9
            argmin = -1
            for D in pointStore[I][J]:
                p = D["point"]
                if abs(p[0]-x) < dx and abs(p[1]-y) < dy:
                    found = True
                    L.append(p[2])
                    if reverseCorrespondence:
                        d = (p[0]-x)**2+(p[1]-y)**2
                        if d < minDist:
                            minDist = d
                            argmin = D["id"]
            if found:
                z = sum(L)/float(len(L))                    
            else:
                z = -999999.000000 
            if reverseCorrespondence:
                closestId = argmin

        image["z"][pos] = z        
        image["closestId"][pos] = closestId        
        if found:
            image["flag"][pos] = 1        
            image["x"][pos] = x
            image["y"][pos] = y
        else: # empty pixel
            image["x"][pos] = -999999.000000
            image["y"][pos] = -999999.000000


# Write the abs file
absfile = open(absfilename, "w")
absfile.write(str(height)+" rows\n")
absfile.write(str(width)+" columns\n")
absfile.write("pixels (flag X Y Z):\n")
absfile.write(string.join(map(str,image["flag"])," ")+"\n")
absfile.write(string.join(map(str,image["x"])," ")+"\n")
absfile.write(string.join(map(str,image["y"])," ")+"\n")
absfile.write(string.join(map(str,image["z"])," ")+"\n")
absfile.close()
                                  
if reverseCorrespondence:
    corfile = open(corFile, "w")
    corfile.write(string.join(map(str,image["closestId"])," ")+"\n")
    corfile.close()