#!/usr/bin/python3
from math import sin, cos, pi, sqrt
import plotly.graph_objs as go
from plotly.subplots import make_subplots

FP_acc = 2<<16
pi_FP = 1* FP_acc



def abs_FP(re, im):
  return int(sqrt(re*re + im*im))
  
def sqrt_FP(val):
  #print(val)
  return int(sqrt(val)) 



def abs_FP(re, im):
#  return sqrt(re*re + im*im)
  return int(sqrt(re*re + im*im)/FP_acc)

trigon_debug = 0

def sin_FP(phi_fp):
  if (trigon_debug):
    print("sin_FP========")
    print("phi:", phi_fp)
  if phi_fp < 0:
    if (trigon_debug):
      print("phi < 0. recursive inversion...")
    return -1 *sin_FP(-1*phi_fp)

  while phi_fp >= 2*pi_FP:
    if (trigon_debug):
      print("phi is bigger than 2Pi. Decreasing...")
    phi_fp -= 2*pi_FP
    if (trigon_debug):
      print("phi:", phi_fp)
  
  if phi_fp >= pi_FP:
    if (trigon_debug):
      print("phi > pi_FP. recursive inversion...")
      print(phi_fp, pi_FP)
    return -1*sin_FP(phi_fp - pi_FP)
  
  if phi_fp == pi_FP/2:
    return 1*FP_acc
  if phi_fp == 0:
    return 0
  
  if phi_fp > pi_FP/2:
    if (trigon_debug):
      print("phi > pi_FP/2. recursive inversion...")
    return sin_FP(pi_FP - phi_fp)
  
  #now phi should be inside [0, Pi/2). checking...
  if phi_fp < 0:
    raise ValueError('error in sin_FP. after all checks phi < 0')
  if phi_fp >= pi_FP/2:
    raise ValueError('error in sin_FP. after all checks phi > pi_FP/2')
    
  #now phi is inside [0, Pi/2). So, cos(phi) > 1 always
  if (trigon_debug):
    print("phi:", phi_fp)
  return sin_FP_constrained(phi_fp)


sin_05_debug = 0

def sin_FP_constrained(phi_fp):
  phi_trh = pi_FP/16
  if (trigon_debug):
    print("sin_FP_constrained===========")
    print("phi:", phi_fp)
    print("check is phi inside [0, Pi/2)")
  if phi_fp < 0:
    raise ValueError('error in sin_FP. after all checks phi < 0')
  if phi_fp >= pi_FP/2:
    raise ValueError('error in sin_FP. after all checks phi > pi_FP/2')
  if (trigon_debug):
    print("Ok")
  if (phi_fp > phi_trh):
    if (sin_05_debug):
      print("phi_fp:", phi_fp," >",phi_trh,"...   recusion")
      print("phi_fp/2:", phi_fp/2)
    sin_phi_05 = sin_FP_constrained(phi_fp/2)
    sin_phi = int((2*sin_phi_05 * sqrt_FP(FP_acc**2 - sin_phi_05*sin_phi_05))/FP_acc)
    if (sin_05_debug):
      print("sin_phi_05:", sin_phi_05)
      print("sin_phi:",sin_phi)
    return sin_phi
  else:
    res = int((phi_fp * 3.141592653589793238462643383279502884197169399375105820974944 * FP_acc)/FP_acc - ((phi_fp * 3.141592653589793238462643383279502884197169399375105820974944 * FP_acc)/FP_acc)**3/(6*FP_acc**2))
#    res = int((phi_fp * 1.0 * FP_acc)/FP_acc - ((phi_fp * 1.0 * FP_acc)/FP_acc)**3/(6*FP_acc**2))
    if (trigon_debug):
      print("calculating sin(x) as x:",phi_fp, res)

    return res
  

  
  
  
#  return sin(pi*(phi_fp/FP_acc)/(pi_FP/FP_acc))

  
def cos_FP(phi_fp):
  return sin_FP(phi_fp - pi_FP/2)
  
  
def sin_tester():
  N = 4000
  angs = [(i - N/2)/1000 for i in range(N)]
  res_f = []
  res_FP = []
  res_cos_FP = []
  for phi in angs:
    res_f.append(sin(phi*pi))
#    print(phi, phi*FP_acc*pi_FP/FP_acc)
    val_fp = sin_FP(phi*FP_acc*pi_FP/FP_acc)
    val_cos_fp = cos_FP(phi*FP_acc*pi_FP/FP_acc)
    print("angle, sin, cos:",phi, val_fp, val_cos_fp)
    res_FP.append(val_fp)
    res_cos_FP.append(val_cos_fp)
  chart = make_subplots(rows=2, cols=1)
  
  chart.add_trace(go.Scatter(x = angs, y=res_f, name="sin_float", mode="markers+lines"), row=1, col=1)
  chart.add_trace(go.Scatter(x = angs, y=[val/FP_acc for val in res_FP], name="sin_FP", mode="markers+lines"), row=1, col=1)
  chart.add_trace(go.Scatter(x = angs, y=[val/FP_acc for val in res_cos_FP], name="cos_FP", mode="markers+lines"), row=1, col=1)
  chart.add_trace(go.Scatter(x = angs, y=[a - b/FP_acc for a,b in zip(res_f, res_FP)], name="error", mode="markers+lines"), row=2, col=1)
  chart.update_xaxes(matches="x1", row=2, col=1)

  chart.show()
  


if __name__ == "__main__":
  sin_tester()