from vector import multivector

import random

class solid:
	timescale = 1
	
	def __init__(self, dim=3, mass=1, constrain_position=False, constrain_rotation=False):
		self.dim = dim
		self.mass = mass
		self.inertia = None # Override this!
		self.npoints = None # Override this!
		self.cache = {}
		self.displace = multivector(dim)
		self.velocity = multivector(dim)
		
		# Is this object constrained somehow?
		self.constrain_position = constrain_position
		self.constrain_rotation = constrain_rotation
	
	def update(self, dt):
	#	linear1 = self.displace[1]
	#	linear2 = self.velocity[1] * dt * self.timescale
	#	angular1 = self.displace[2]
	#	angular2 = self.velocity[2] * dt * self.timescale
	#	linear = linear1 + linear2
	#	angular = angular1 + angular2
	#	self.displace = linear + angular
		self.displace += self.velocity * dt * self.timescale
	
	def angular_inertia(self, plane):
		return self.fancy_inertia(plane) if self.inertia is None else self.inertia
	
	def fancy_inertia(self, plane):
		out = 0
		m = self.mass / self.npoints
		i = plane.normalize()
		for r in self.rotated_points():
			out += m * (r*i).magnsqr() # Contract radius onto the plane, then take squared magnitude, then scale by mass
		return out
	
	def linear_impulse(self, impulse):
		self.velocity += impulse / self.mass
	
	def angular_impulse(self, impulse, arm):
		if impulse.is_null():
			return
		torque = impulse % arm
		self.velocity += torque / self.angular_inertia(torque) # torque = impulse /\ lever-arm
#		print(self.velocity)
	
	def impulse(self, impulse, loc):
		if not self.constrain_position: self.linear_impulse(impulse)
		if not self.constrain_rotation: self.angular_impulse(impulse, (loc-self.displace).vector())
		self.cache.clear()
	
	def cache_rotations(self):
		self.cache['linear'] = self.displace[1]
		self.cache['angular'] = angular = self.displace[2]
		self.cache['rotor1'] = rotor = angular.exp()
		self.cache['rotor2'] = ~rotor
	
	def check_cache(self):
		if not self.cache:
			self.cache_rotations()
	
	def local_to_global(self, point):
		self.check_cache()
		return self.cache['rotor1'] @ point @ self.cache['rotor2'] + self.cache['linear'] # R@v@~R+x returns v rotated by R and displaced by x
	
	def point(self, n):
		return self.local_to_global(self.internal_point(n))
	
	def rotated_points(self):
		self.check_cache()
		out = [
			self.cache['rotor1'] @ self.internal_point(i) @ self.cache['rotor2'] + self.cache['linear']
			for i in range(self.npoints)
		]
		self.cache.clear()
		return out
	
	def random_point(self):
		return self.local_to_global(self.internal_random_point())
	
	def internal_point(self, n):
		raise NotImplementedError()
	
	def internal_random_point(self):
		raise NotImplementedError()
	
	def get_edges(self):
		raise NotImplementedError()
	
	def get_faces(self):
		raise NotImplementedError()