u

import numpy as np

class ElemRot :

class listMatrix3x3 :

   @property

   def M(self) :

      return self._m

   @M.setter

   def M(self,this) :

      self._m=this

      self._dtype=this.dtype

   @property

   def shape(self) :

      return self._m.shape

   @shape.setter

   def shape(self,this) :

      self._m.shape=this

   @property

   def size(self) :

      return self._m.size

   @property

   def isSingle(self) :

      return len(self.size)==3

   #

   def __init__(self,*arg,dtype=float,value=0) :

      self._dtype=dtype

      if len(arg)==0 :

         self._m=np.array([])

      else :

         if np.isscalar(arg[0]) :

            self._m=self.zeros(arg[0],dtype)+value

         else :

            self._m=np.array(arg[0])

   #

   def __len__(self) :

      return self._m.shape[0]

   #

   def __getitem__(self,this) :

      return self._m[this]

   #

   def __setitem__(self,this,that) :

      self._m[this]=that

   #

   def zeros(self,size,dtype) :

      return np.zeros([size,3,3],dtype=dtype)

   #

   def __clear__(self) :

      self._m=None

   #

   def __str__(self) :

      return self._m.__str__()

   #

   def dot(self,this) :

      if np.isscalar(this) :

         out=self.__class__(self._m.dot(this))

         return out

      out=[]

      for k2 in self._m :

         for k1 in this :

            out.append(k2.dot(k1))

      return self.__class__(out)

   #

   def apply_to(self,this_lm) :

      if len(this_lm)==len(self) :

         out=listMatrix3x3(len(self))

         for ik in range(len(self)) :

            out[ik]=self[ik].dot(this_lm[ik])

         return out

      elif len(this_lm)==1 and len(self)>1 :

         out=listMatrix3x3(len(this_lm))

         for ik in range(len(this_lm)) :

            out[ik]=self[0].dot(this_lm[ik])

         return out

      elif len(this_lm)>1 and len(self)==1 :

         out=listMatrix3x3(len(self))

         for ik in range(len(self)) :

            out[ik]=self[ik].dot(this_lm[0])

         return out

      else :

         raise Exception("error: uncompatible dimesions between arrays","")

   ##

   #def __mul__(self,this) :

      #if np.isscalar(this) :

         #out=self.__class__(this*self._m)

         #return out

      ##

      ##if self.__class__==this.__class__:

         ##out=[]

         ##for k2 in self._m :

            ##for k1 in this.M :

               ##out.append(k2.dot(k1))

         ##return self.__class__(out)

      ###

      ##ooo=str(self.__class__).split('.')[-1].split("'")[0]

      ##raise Exception("error: a "+ooo+" object can be multiplied by a scalar or another "+ooo+" object ","")

      ##

      ##if this.__class__==self._m.__class__:

         ##print("__mul__ 1")

         ##out=self.__class__([k.dot(this) for k in self._m])

         ##return out

   ##

   #def __rmul__(self,this) :

      #if np.isscalar(this) :

         #return self.__class__(this*self._m)

      ###

      ##if self.__class__==this.__class__:

         ##out=[]

         ##for k1 in this.M :

            ##for k2 in self._m :

               ##out.append(k2.dot(k1))

         ##return self.__class__(out)

      ###

      ##ooo=str(self.__class__).split('.')[-1].split("'")[0]

      ##raise Exception("error: a "+ooo+" object can be multiplied by a scalar or another "+ooo+" object ","")

      ##

      ##if this.__class__==self._m.__class__:

         ##out=self.__class__([this.dot(k) for k in self._m])

         ##return out

      #

   def copy(self) :

      import copy

      return copy.deepcopy(self)

   def slice(self,*arg) :

      i1=arg[0]

      if len(arg)==2 :

         i2=arg[1]

      else :

         i2=i1+1

      out=listMatrix3x3()

      out._m=self._m[i1:i2]

      return out

#m=np.array([[1,0,0],[1,1,0],[1,1,1]])

#lm1=listMatrix3x3([m])

#lm=listMatrix3x3([k*m for k in range(1,100)])

class ElemRot(listMatrix3x3) :

   @property

   def axis(self) :

      return self._axis

      return self._angle

   @angle.setter

   def angle(self,this) :

      self._angle=angle

      self._c=np.cos(angle)

      self._s=np.sin(angle)

      self._m=np.zeros([3,3,len(self)])

      self._m[0,0]=np.ones(len(self))

      self._m[1,1]=np.ones(len(self))

      self._m[2,2]=np.ones(len(self))

      self._angle=np.array([this]) if np.isscalar(this) else this

      self._c=np.cos(self._angle)

      self._s=np.sin(self._angle)

      self._m=self.zeros(len(self._angle),float)

      self._m[:,0,0]=1

      self._m[:,1,1]=1

      self._m[:,2,2]=1

      #

      if self.axis=='x' :

         self._m[1,1]=self._c

         self._m[1,2]=-self._s

         self._m[2,1]=self._s

         self._m[2,2]=self._c

         self._m[:,1,1]=self._c

         self._m[:,1,2]=-self._s

         self._m[:,2,1]=self._s

         self._m[:,2,2]=self._c

      elif self.axis=='y' :

         self._m[0,0]=self._c

         self._m[0,2]=self._s

         self._m[2,0]=-self._s

         self._m[2,2]=self._c

         self._m[:,0,0]=self._c

         self._m[:,0,2]=self._s

         self._m[:,2,0]=-self._s

         self._m[:,2,2]=self._c

      elif self.axis=='z' :

         self._m[0,0]=self._c

         self._m[0,1]=-self._s

         self._m[1,0]=self._s

         self._m[1,1]=self._c

   @property

   def M(self) :

      return self._m

   def __init__(self,axis,angle) :

         self._m[:,0,0]=self._c

         self._m[:,0,1]=-self._s

         self._m[:,1,0]=self._s

         self._m[:,1,1]=self._c

      else :

         raise Exception("error: undefined rotation axis","")

   def __init__(self,axis,Angle,deg=True) :

      listMatrix3x3.__init__(self)

      self._axis=None

      self._angle=None

      if axis==0 or str(axis).lower()=='x' :

         self._axis='y'

      elif axis==2 or str(axis).lower()=='z' :

         self._axis='z'

      else :

      elif type(axis)==type("") :

         self._axis=str(axis)

      if not angle is None :

         self.angle=angle

   def __len__(self) :

      return self._m.shape[2]

   def __getitem__(self,i,j) :

      return self._m[i,j]

   def __setitem__(self,i,j,this) :

      self._m[i,j]=this

   def copy(self) :

      import copy

      return copy.deepcopy(self)

   def __rmul__(self,this) :

      if this.__class__==self.__class__ :

         out=np.zeros([3,3,max(len(self),len(this))])

         for i in range(3) :

            for j in range(3) :

               for l in range(3) :

                  out[i,j]+=self[i,l]*this[l,j]

         oo=self.copy()

         oo._axis='undef'

         oo._m=out

         return oo

      else :

         raise Exception("error: undefined rotation axis","")

      if not Angle is None :

         if deg :

            self.angle=np.deg2rad(Angle)

         else :

            self.angle=Angle

#erot=ElemRot('x',None)

#erot1=ElemRot('x',np.arange(0,10))

#m=np.array([[1,0,0],[1,1,0],[1,1,1]])

#lm1=listMatrix3x3([m])

#lm=listMatrix3x3([k*m for k in range(1,100)])

#stop

class PRM0 :

   @property

      self.omegaVax=0.

      self.cphi0=0.

      self.ctheta0=0.

      self.commit()

   #

   def commit(self) :

      if self.isAllSet :

         out=self.rmult(self._matr['pan'],self._matr['roll'])

         out=self.rmult(self._matr['tilt'],out)

         self._matr['M1']=self.rmult(self._matr['ctheta0'],out)

         self._matr['M2']=self.rmult(self._matr['cphi0'],self._matr['tiltFork'])

         self._matr['M3']=self.rmult(self._matr['omegaVax'],self._matr['zVax'])

         out=self._matr['pan'].apply_to(self._matr['roll'])

         out=self._matr['tilt'].apply_to(out)

         self._matr['M1']=self._matr['ctheta0'].apply_to(out)

         self._matr['M2']=self._matr['cphi0'].apply_to(self._matr['tiltFork'])

         self._matr['M3']=self._matr['omegaVax'].apply_to(self._matr['zVax'])

         self._isComplete=True

      else :

         self._isComplete=False

      """ 

      if not self.isComplete :

         raise Error('Configuration not completed or not committed','')

      self._d['CTHETA_PHI']=CTHETA_PHI

      #

      self._matr['CTRL_THETA']=self.MatrY(np.deg2rad(CTHETA_PHI[:,0]))

      self._matr['CTRL_PHI']=self.MatrZ(np.deg2rad(CTHETA_PHI[:,1]))

      #

      out=self.rmult(self._matr['CTRL_THETA'],self._matr['M1'])

      out=self.rmult(self._matr['M2'],out)

      out=self.rmult(self._matr['CTRL_PHI'],out)

      out=self.rmult(self._matr['M3'],out)

      out=self._matr['CTRL_THETA'].apply_to(self._matr['M1'])

      out=self._matr['M2'].apply_to(out)

      out=self._matr['CTRL_PHI'].apply_to(out)

      out=self._matr['M3'].apply_to(out)

      #

      #stores Rout 

      self._matr['Rout']=out

      #

      #set output has NPOS,3,3 shape if required

      if axis0isNPOS==True :

         out=np.moveaxis(out,0,2)

      #if axis0isNPOS==True :

         #out=np.moveaxis(out,0,2)

      #

      return out

   #

   def MatrX(self,angle) :

      out=np.zeros([3,3,len(angle)])

      out[0,0]+=1

      c=np.cos(angle)

      s=np.sin(angle)

      out[1,1]=c

      out[1,2]=-s

      out[2,1]=s

      out[2,2]=c

      return out

   def MatrX(self,Angle,deg=True) :

      return ElemRot('x',Angle,deg=deg)

   #

   def MatrY(self,angle) :

      out=np.zeros([3,3,len(angle)])

      c=np.cos(angle)

      s=np.sin(angle)

      out[1,1]+=1

      out[0,0]=c

      out[0,2]=s

      out[2,0]=-s

      out[2,2]=c

      return out

      return ElemRot('y',Angle,deg=deg)

   #

   def MatrZ(self,angle) :

      out=np.zeros([3,3,len(angle)])

      c=np.cos(angle)

      s=np.sin(angle)

      out[2,2]+=1

      out[0,0]=c

      out[0,1]=-s

      out[1,0]=s

      out[1,1]=c

      return out

      return ElemRot('z',Angle,deg=deg)

   #

   def rmult(self,this,that) :

      out=np.zeros([3,3,max(this.shape[2],that.shape[2])])