#!/usr/bin/env python
import os, sys, json
from numpy import sin
from numpy import arcsin as asin
import numpy as np

def forward(D0,L1,L2,L3,L4,L5):
  #define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
  #define(r='L6',scl='L7')
  #define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
  #define(X='C6',Y='C7',U='C3',V='C4',W='C5')
  L6=2000./500000
  L7=5000/1.41421356237
  L11=L7*sin(L1*1.74532925199e-05)
  L12=L7*sin(L2*1.74532925199e-05)
  L13=L7*sin(L3*1.74532925199e-05)
  L14=L7*sin(L4*1.74532925199e-05)
  L15=L7*sin(L5*1.74532925199e-05)

  print("fwd_inp(%f)  %f %f %f %f %f"%(D0,L1,L2,L3,L4,L5))
  print("fwd_inp(%f)  %f %f %f %f %f"%(D0,L11,L12,L13,L14,L15))


  #//C6=D^-1*S
  C6=+.5*L11   -.5*L12   +(L6+.25)*L13   +(L6-.75)*L14   +(.25-L6)*1.41421356237*L15
  C7=+.5*L11   +.5*L12       +.25*L13       +.25*L14       +.25*1.41421356237*L15
  C3=                       +1.*L13        +1.*L14        -1.*1.41421356237*L15
  C4=-1.*L11   +1.*L12       +1.5*L13        -.5*L14        -.5*1.41421356237*L15
  C5=-1.*L11   -1.*L12        +.5*L13        +.5*L14        +.5*1.41421356237*L15

  print("fwd_res     %f %f %f %f %f"%(C6,C7,C3,C4,C5))

  #Dinv=[[+.5,  -.5,  +(L6+.25),  +(L6-.75),  +(.25-L6)*1.41421356237],
  #      [+.5,  +.5,       +.25,       +.25,       +.25*1.41421356237],
  #      [  0,    0,        +1.,        +1.,        -1.*1.41421356237],
  #      [-1.,  +1.,       +1.5,        -.5,        -.5*1.41421356237],
  #      [-1.,  -1.,        +.5,        +.5,        +.5*1.41421356237]]
  #Dinv=np.matrix(Dinv)
  #print(Dinv)
  #print(Dinv**-1)


  #D0=$000000f8; //C3=$8 C4=$10 C5=$20 C6=$40 C7=$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
  #close
  return (D0,C6,C7,C3,C4,C5)

def inverse(D0,C6,C7,C3,C4,C5):
#//  define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
#//  define(r='L6',scl='L7')
#//  define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
#//  define(X='C6',Y='C7',U='C3',V='C4',W='C5')

  #//if(D0>0)
  #//  send 1"Velocity calculation NOT SUPPORTED\n"

  print("inv_inp(%f)  %f %f %f %f %f"%(D0,C6,C7,C3,C4,C5))

  L6=2000./500000

  L11=+.5*C6        +.5*C7   +(.25-.5*L6)*C3   -.25*C4          -.25*C5
  L12=-.5*C6        +.5*C7   +(.5*L6-.25)*C3   +.25*C4          -.25*C5
  L13=+.5*C6        +.5*C7   +(.25-.5*L6)*C3   +.25*C4          +.25*C5
  L14=-.5*C6        +.5*C7   +(.25+.5*L6)*C3   -.25*C4          +.25*C5
  L15=       .5*1.41421356237*C7   -.25*1.41421356237*C3           +.25*1.41421356237*C5

  L7=1.41421356237/5000
  L1=asin(L11*L7)/1.74532925199e-05
  L2=asin(L12*L7)/1.74532925199e-05
  L3=asin(L13*L7)/1.74532925199e-05
  L4=asin(L14*L7)/1.74532925199e-05
  L5=asin(L15*L7)/1.74532925199e-05

  print("inv_res     %f %f %f %f %f"%(L1,L2,L3,L4,L5))

  #D=[[+.5,              +.5,         +(.25-.5*L6),   -.25,               -.25],
  #   [-.5,              +.5,         +(.5*L6-.25),   +.25,               -.25],
  #   [+.5,              +.5,         +(.25-.5*L6),   +.25,               +.25],
  #   [-.5,              +.5,         +(.25+.5*L6),   -.25,               +.25],
  #   [  0, .5*1.41421356237,   -.25*1.41421356237,      0, +.25*1.41421356237]]

  #D=np.matrix(D)
  #print(D)
  #print(D**-1)


  return (D0,L1,L2,L3,L4,L5)

#close



f=forward(0,0,0,0,0,0)
print('')
i=inverse(*f)
print('\n')
f=forward(0,10000,20000,30000,40000,50000)
print('')
i=inverse(*f)

#SOMETHING IS WRONG IN THE COORD TRF !!!



#inverse(0,0,0,0,0,0)