import math
import itertools
from operator import add
from functools import reduce

import numpy as np
from scipy import stats as st
import matplotlib.pyplot as plt

class Vertex:
    def __init__(self,x,y):
        self.x=x
        self.y=y

    def dot(self,b):
        return self.x*b.x + self.y*b.y

    def angle_between(b,a):
        return np.arctan2(b.y,b.x) - np.arctan2(a.y,a.x)
        #theta =  math.acos(a.dot(b)/(a.length()*b.length()))
        #if (a.x < b.x) ^ (a.y > b.y):
        #    return theta
        #else:
        #    return -theta

    def angle(self):
        if self.x < 0:
            if self.y < 0:
                return (-math.pi) + (math.atan(self.y/self.x))
            else:
                return math.pi-math.atan(self.y/self.x)
        elif self.x == 0:
            return np.sign(self.y)*math.pi/2
        else:
            return math.atan(self.y/self.x)


        if self.x == 0:
            return 0
        return math.atan(self.y/self.x)

    def __add__(a,b):
        x = a.x+b.x
        y = a.y+b.y
        return Vertex(x,y)

    def __mul__(a,b):
        try:
            return Vertex(a.x*b, a.y*b)
        except:
            raise NotImplementedError

    def __truediv__(a,b):
        try:
            return Vertex(a.x/b, a.y/b)
        except:
            raise NotImplementedError

    def __sub__(a,b):
        x = a.x-b.x
        y = a.y-b.y
        return Vertex(x,y)

    def __str__(self):
        return "({0},{1})".format(self.x,self.y)

    def length(self):
        return math.sqrt(self.x**2+self.y**2)

    def toarray(self):
        return np.array([self.x,self.y])

def which(v):
    return np.argmax([x is None for x in v])

class Triangle:
    def __init__(self, *, vertices, angles=[None,None,None], fuzzy=True):
        self.angles = np.array(angles)
        self.a1,self.a2,self.a3=self.angles
        self.v1,self.v2,self.v3=np.array(vertices)
        self.flag = which([self.v1,self.v2,self.v3])
        if self.flag == 0:
            self.vertices = [self.v1,self.v2,self.v3]
        elif self.flag == 1:
            self.vertices = [self.v2,self.v1,self.v3]
            self.angles = [self.a2,self.a1,self.a3]
        elif self.flag == 2:
            self.vertices = [self.v3,self.v2,self.v1]
            self.angles = [self.a3,self.a2,self.a1]
        self.vertices = np.array(self.vertices)


        # if we have two angles fill in the third
        if self.get_num_angles() == 2:
            self.fill_angle()

        if self.get_num_vertices() == 3:
            # compute the angles between them
            self.fill_angles_from_vertices()
        elif self.get_num_vertices() == 2:
            if self.get_num_angles() > 0:
                # compute the remaining angles and vertices
                self.fill_missing_vertex()
                self.fill_angles_from_vertices
            else:
                raise Exception('Without all the vertices we need at least one angle.')
        else:
            if not fuzzy:
                raise Exception('Need at least two vertices to define a triangle.')
            else:
                for v in self.vertices:
                    if v is not None:
                        return v
        self.lengths()

    def vertices(self):
        if self.flag == 0:
            return self.vertices
        elif self.flag == 1:
            return np.array([self.v2,self.v1,self.v3])
        elif self.flag == 2:
            return np.array([self.v3,self.v2,self.v1])

    def angless(self):
        if self.flag == 0:
            return self.angles
        elif self.flag == 1:
            return np.array([self.a2,self.a1,self.a3])
        elif self.flag == 2:
            return np.array([self.a3,self.a2,self.a1])

    def center(self):
        return reduce(add,self.vertices)/3

    def get_num_angles(self):
        return np.sum(np.array([a != None for a in self.angles]))

    def get_num_vertices(self):
        return np.sum(np.array([v != None for v in self.vertices]))

    def fill_angles_from_vertices(self):
        if self.get_num_angles() == 3:
            return None
        which = np.array([a != None for a in self.angles])
        which_not = np.invert(which)
        index = np.argmax(which_not)
        which_not = np.setdiff1d(np.arange(0,3),[index])

        side1 = self.vertices[index] - self.vertices[which_not[0]]
        side2 = self.vertices[index] - self.vertices[which_not[1]]
        self.angles[index] = side1.angle_between(side2)
        self.a1,self.a2,self.a3 = self.angles
        self.fill_angles_from_vertices()

    def fill_missing_vertex(self):
        # find missing vertex
        which = np.array([a != None for a in self.vertices])
        which_not = np.invert(which)
        index = np.argmax(which_not)
        which_not = np.setdiff1d(np.arange(0,3),[index])

        va = self.vertices[which_not[0]]
        ab = self.angles[which_not[0]]
        vb = self.vertices[which_not[1]]
        aa = self.angles[which_not[1]]
        ac = self.angles[index]

        # sin rule
        C = vb - va
        vC = (vb - va).length()
        vA = (vC/math.sin(ac)) * math.sin(aa)

        adj = C.angle() # rotation so angle is relative to side C
        A = Vertex(vA*math.cos((ab)+adj), vA*math.sin(ab+adj))
        if self.flag == 0:
            A = A
        elif self.flag == 1:
            A = Vertex(-A.x,-A.y)
        elif self.flag == 2:
            A = Vertex(-A.y,-A.x)
        self.vertices[index] = va + A
        self.v1,self.v2,self.v3 = self.vertices
        return None

    def fill_angle(self):
        which = np.array([a != None for a in self.angles])
        if np.all(which):
            return
        elif sum(which) < 2:
            raise Exception('Need at least two angles.')
        else:
            if np.sum(self.angles[which]) > math.pi:
                raise Exception('Angles must add up to < π radians')
            self.angles[np.invert(which)] = math.pi - np.sum(self.angles[which])
            self.a1,self.a2,self.a3 = self.angles
            return None

    def lengths(self):
        self.l1 = (self.v2 - self.v3).length()
        self.l2 = (self.v3 - self.v1).length()
        self.l3 = (self.v1 - self.v2).length()

    def __str__(self):
        string = '''
        Angles: {a1}, {a2}, {a3}
        Vertices: {v1}, {v2}, {v3}
        Lengths: {l1}, {l2}, {l3}
        '''.format(
            a1=math.degrees(self.a1), a2=math.degrees(self.a2), a3=math.degrees(self.a3),
            v1=self.v1, v2=self.v2, v3=self.v3,
            l1=self.l1, l2=self.l2, l3=self.l3
        )
        return string

    def plot(t,marker='.',fig=None):
        if fig is None:
            fig,ax = plt.subplots(1,1,figsize=(5,5))
        else:
            ax = fig.axes[0]
        ax.scatter(t.v1.x,t.v1.y,label='v1',marker=marker)
        ax.scatter(t.v2.x,t.v2.y,label='v2',marker=marker)
        try:
            ax.scatter(t.v3.x,t.v3.y,label='v3',marker=marker)
        except:
            None
        try:
            None
            #center = t.center()
            #ax.scatter(center.x,center.y,label='center',marker='x')
        except:
            None

        mmax = max(ax.get_xlim()[1],ax.get_ylim()[1])
        plt.xlim(0,mmax)
        plt.ylim(0,mmax)
        plt.legend(loc=3)
        return fig

def ArrayVertex(A):
    if (A[0] < 0) or (A[1] < 0):
        return None
    return Vertex(A[0],A[1])

def vertices_from_arrays(led1,led2,led3):
    return [(ArrayVertex(led1[t,:]),ArrayVertex(led2[t,:]),ArrayVertex(led3[t,:]))
            for t in np.arange(np.shape(led1)[0])]

def triangles_from_vertex_arrays(led1,led2,led3):
    return [Triangle(vertices=v) for v in vertices_from_arrays(led1,led2,led3)]

class TriangleSet:
    def __init__(self,led1,led2,led3,central_tendancy='mean'):
        self.triangles = triangles_from_vertex_arrays(led1,led2,led3)
        self.central_tendancy=central_tendancy

    def all_angles(self):
        return [t.angless() for t in self.triangles]

    def mean_angle(self):
        if self.central_tendancy=='mean':
            return np.mean(self.all_angles(),axis=0)
        elif self.central_tendancy=='median':
            return np.median(self.all_angles(),axis=0)
        elif self.central_tendancy=='r_mean':
            r = 1.5
            return np.array([np.mean(st.sigmaclip([(t[i])
                                                   for t in self.all_angles()],r,r)[0])
                             for i in range(3)])

    def apply(self,vertices):
        #try:
        return Triangle(vertices=vertices, angles=self.mean_angle())
        #except:
        #    None

    def apply_to_set(self,led1,led2,led3):
        return [self.apply(v) for v in vertices_from_arrays(led1,led2,led3)]