import bpy
from mathutils import *
from math import *
import bmesh

from bpy.props import CollectionProperty, StringProperty, BoolProperty
from bpy_extras.io_utils import ImportHelper, ExportHelper

#data to make this an addon

bl_info = {
    "name": "GeoTools",
    "author": "Jace Priester",
    "version": (1, 0, 0),
    "blender": (2, 6, 3),
    "api": 45996,
    "location": "Ctrl-Shift-T",
    "description": "Geometry Tools - Addons for geometry operations",
    "warning": "",
    "wiki_url": "http://www.threespaceimaging.com",
    "tracker_url": "http://www.threespaceimaging.com",
    "category": "3D View"}

def BestFitPlaneThrough3Points(coords):
    com = (coords[0]+coords[1]+coords[2])/3
    
    

def BestFitPlaneThroughPoints(coords): #returns center-of-mass and plane normal as a tuple

    com = Vector((0,0,0))
    
    for co in coords:
        com += co
    com /= len(coords)
    
    if len(coords)<2: #less than 2, no solution
        print("ERROR Less than 2 points provided!")
        return (com,Vector((0,0,1)))
        
    if len(coords)==2: #treat the 2 points as the normal, so we create the mirror line
        normal=(coords[0]-coords[1]).normalized()
        return (com,normal)
        
    if len(coords)==3: #
        normal=(coords[0]-coords[1]).cross(coords[1]-coords[2]).normalized()
        return (com,normal)
    
    covar = Matrix(((0,0,0), (0,0,0), (0,0,0)))
    for co in coords:
        covar[0][0] += (co.x-com.x)**2
        covar[1][0] += (co.x-com.x)*(co.y-com.y)
        covar[2][0] += (co.x-com.x)*(co.z-com.z)
        
        covar[0][1] += (co.y-com.y)*(co.x-com.x)
        covar[1][1] += (co.y-com.y)**2
        covar[2][1] += (co.y-com.y)*(co.z-com.z)
        
        covar[0][2] += (co.z-com.z)*(co.x-com.x)
        covar[1][2] += (co.z-com.z)*(co.y-com.y)
        covar[2][2] += (co.z-com.z)**2
    
    try:    
        covar=covar.inverted()
        
        #if we got an inverse, we can proceed to solve for the normal
        
        itermax = 500
        iter = 0
        vec=Vector((1,1,1))
        vec2=(vec*covar)/(vec*covar).length
        while vec!=vec2 and iter<itermax:
            iter+=1
            vec=vec2
            vec2=(vec*covar)/(vec*covar).length
        normal = vec2.normalized()
        
        return (com,normal)
        
    except:
        #it may not have an inverse, if they're already coplanar
        #in this case we can treat it as a 3-point plane with a unique solution
        
        normal=(coords[0]-coords[1]).cross(coords[1]-coords[2]).normalized()
        return (com, normal)
        
    
    return (com,normal)
    
    
def FlattenPointsToPlane(com,normal,mesh,vertindexes):
    for vi in vertindexes:
        co=mesh.vertices[vi].co
        mesh.vertices[vi].co = co - (co-com).dot(normal)*normal
    
def MaxDistanceBetweenPoints(coords):
    max_dist=0.0
    for i in range(len(coords)):
        for j in range(len(coords)):
            if i==j:
                continue
                
            dist=(coords[i]-coords[j]).length
            if dist>max_dist:
                max_dist=dist
        
    return max_dist
    
    
def Vec3ToTuple(vec):
    return (vec[0],vec[1],vec[2])
        
class MakePointsCoplanar(bpy.types.Operator):
    '''Make Points Coplanar'''
    bl_idname = "mesh.make_points_coplanar"
    bl_label = "Make Points Coplanar"
    bl_options = {'UNDO'}
    
    mode = bpy.props.IntProperty(name = "Mode",
                    description = "switch between operating modes",
                    default = 2)
        
    def execute(self, context):
        
        restore_editmode=False

        if bpy.context.mode=='EDIT_MESH':
            restore_editmode=True
            bpy.ops.object.mode_set(mode='OBJECT')
            

        coords=list()
        vertindexes=list()

        obj = bpy.context.active_object
        i=0
        for v in obj.data.vertices:
            if v.select:
                coords.append(v.co)
                vertindexes.append(i)
                
            i+=1
            
        (com,normal)=BestFitPlaneThroughPoints(coords)

        if self.mode==0: #fit points to plane
            print("Flatten points to plane (mode 0)")
            FlattenPointsToPlane(com,normal,obj.data,vertindexes)
        elif self.mode==1: #create a new plane describing the planar fit (in edit mode)
        
            print("Create best fit plane in edit mode (mode 1)")
            
            v=Vector((0,0,1))+com #put the vector straight up toward the Z axis, start at the COM, then project to the plane
            v = v - (v-com).dot(normal)*normal
            radius = MaxDistanceBetweenPoints(coords) / 2.0
            v = com + (v-com) * radius / (v-com).length #normalize it to a reasonable length (based on the distance across the planar coords
            
            q2=Quaternion(normal,2.0*pi/3.0)
            q3=Quaternion(normal,2.0*pi/3.0*2.0)
            
            bm=bmesh.new()
            bm.from_mesh(obj.data)
            bm.verts.new(v)
            bm.verts.new(com + q2*(v-com))
            bm.verts.new(com + q3*(v-com))
            bm.faces.new([bm.verts[-1],bm.verts[-2],bm.verts[-3]])
            bm.to_mesh(obj.data)
            
        elif self.mode==2: #create a new plane describing the planar fit (in object mode as a new object)

            print("Create best fit plane as new object (mode 2)")
            
            zvec=Vector((0,0,1))
            radius = MaxDistanceBetweenPoints(coords) / 2.0
            v1 = Vector((0,radius,0))
            
            q2 = Quaternion(zvec,2.0*pi/3.0)
            q3 = Quaternion(zvec,2.0*pi/3.0*2.0)
            
            v2 = q2*v1
            v3 = q3*v1
            
            planar_mesh=bpy.data.meshes.new(name="Planar")
            planar_mesh.from_pydata([Vec3ToTuple(v1), Vec3ToTuple(v2), Vec3ToTuple(v3)], [], [[0,1,2]])
            
            planar_obj=bpy.data.objects.new(name="Planar",object_data=planar_mesh)
            bpy.context.scene.objects.link(planar_obj)
            
            planar_obj.location = obj.matrix_world * com #transfer the center of mass to world coords
            #now we need to rotate the planar object so it is aligned with the normal
            normal_world=obj.matrix_world*normal - obj.matrix_world*Vector((0,0,0))
            rotation_axis = zvec.cross(normal_world).normalized()
            rotation_angle = acos(normal_world.dot(zvec))
            planar_obj.rotation_mode='AXIS_ANGLE'
            planar_obj.rotation_axis_angle=[rotation_angle, rotation_axis[0], rotation_axis[1], rotation_axis[2]]
            
        elif self.mode==3: #change the view, aligning to the best fit plane
            pass

            
        if restore_editmode:
            bpy.ops.object.mode_set(mode='EDIT')
        
        return {'FINISHED'}
        
def extra_menu_func1(self, context):
    self.layout.operator(MakePointsCoplanar.bl_idname, text="Make Points Coplanar").mode = 0
    
def extra_menu_func2(self, context):    
    self.layout.operator(MakePointsCoplanar.bl_idname, text="Create Best Fit Plane (Edit Mode)").mode = 1
    
def extra_menu_func3(self, context):    
    self.layout.operator(MakePointsCoplanar.bl_idname, text="Create Best Fit Plane (As Object)").mode = 2
    
def register():
    bpy.utils.register_class(MakePointsCoplanar)
    bpy.types.VIEW3D_MT_edit_mesh_specials.append(extra_menu_func1)
    bpy.types.VIEW3D_MT_edit_mesh_specials.append(extra_menu_func2)
    bpy.types.VIEW3D_MT_edit_mesh_specials.append(extra_menu_func3)
    
    
def unregister():
    bpy.types.VIEW3D_MT_edit_mesh_specials.remove(extra_menu_func1)
    bpy.types.VIEW3D_MT_edit_mesh_specials.remove(extra_menu_func2)
    bpy.types.VIEW3D_MT_edit_mesh_specials.remove(extra_menu_func3)
    bpy.utils.unregister_class(MakePointsCoplanar)