#include "fbgfx.bi"

'math**************************************************************************
'******************************************************************************
'******************************************************************************

const as double doublepi  =atn (1) * 8
const as double pi        =atn (1) * 4
const as double halfpi    =atn (1) * 2
const as double quarterpi =atn (1)
'******************************************************************************
type vector
  x as double
  y as double
end type

'******************************************************************************
'bringt ein Bogenmaß in den Bereich von 0 bis 2PI
'! BYREF es wir die übergebene Variable verändert
sub RadiansInRange (byref Rad as double)
  if Rad>=doublepi then Rad=Rad-doublepi
  if Rad<0 then Rad=doublepi+Rad
end sub

'liefert Entfernung 2er Ortsvektoren nach Pythagoras
function GetDistance(origin as vector, dest as vector) As double
  dim as double dx,dy
  dx=dest.x - origin.x
  dy=dest.y - origin.y
  return sqr(dx*dx + dy*dy)
end function

'liefert das Quadrat der Entfernung 2er Ortsvektoren nach Pythagoras, erspart SQR
function GetDistanceP2(origin as vector, dest as vector) As double
  dim as double dx,dy
  dx=dest.x - origin.x
  dy=dest.y - origin.y
  return dx*dx + dy*dy
end function

'Richtung(als Bogenmaß) des Punktes dest vom Standpunkt origin aus betrachtet
function GetRad overload(origin as vector,dest as vector) as double
  dim as double Rad
  dim as vector delta
  delta.x= dest.x - origin.x
  delta.y= dest.y - origin.y
  Rad=atan2(delta.y,delta.x)
  if sgn(Rad)=-1 then Rad= doublepi + Rad
  return Rad
end function

'wandelt einen Richtungsvektor zu Bogenmaß
  function GetRad (v as vector) as double
  dim as double Rad=atan2(v.y,v.x)
  if sgn(Rad)=-1 then Rad= doublepi + Rad
  return Rad
end function

'wandelt Bogenmaß zu Richtungsvektor
'distance stellt ein Skalar dar, der optional benutzt werden kann
function GetVector overload(Rad as double,distance as double=1) as vector
  dim as vector v
  RadiansInRange(Rad)
  v.x=cos(Rad)*distance
  v.y=sin(Rad)*distance
  return v
end function


'"normalisiert" Richtungsvektor, skalierbar
function GetVector (origin as vector,distance as double=1) as vector
  dim as double l
  l=sqr(origin.x*origin.x + origin.y*origin.y)
  origin.x *=distance/l
  origin.y *=distance/l
  return origin
end function


'Richtung zu dest als Vector,skalierbar
function GetVector (origin as vector,dest as vector,distance as double=1) as vector
  dim as double l
  dim as vector delta
  delta.x= dest.x - origin.x
  delta.y= dest.y - origin.y
  l=sqr(delta.x*delta.x + delta.y*delta.y)
  delta.x *=distance/l
  delta.y *=distance/l
  return delta
end function


'liefert die nach links zeigende Normale als Vektor mit Länge 1
function GetLeftNormal (origin as vector,dest as vector) as vector
  dim as double l
  dim as vector delta,normal
  delta.x= dest.x - origin.x
  delta.y= dest.y - origin.y
  l=sqr(delta.x*delta.x + delta.y*delta.y)
  delta.x /=l
  delta.y /=l
  normal.x=-delta.y
  normal.y=delta.x
  return normal
end function


'liefert die nach rechts zeigende Normale als Vektor mit Länge 1
function GetRightNormal (origin as vector,dest as vector) as vector
  dim as double l
  dim as vector delta,normal
  delta.x= dest.x - origin.x
  delta.y= dest.y - origin.y
  l=sqr(delta.x*delta.x + delta.y*delta.y)
  delta.x /=l
  delta.y /=l
  normal.x=delta.y
  normal.y=-delta.x
  return normal
end function


function IsLeft(linestart as vector, lineend as vector, cpoint as vector) as integer
  if ((linestart.x - cpoint.x) * (lineend.y - cpoint.y) - _
      (linestart.y - cpoint.y) * (lineend.x - cpoint.x))>0 then return 1
  return 0
end function

'******************************************************************************
'neues Zeugs

'liefert die nach links zeigende Normale als Vector
function GetLeft (v as vector) as vector
  dim as vector n
  n.x=-v.y
  n.y=v.x
  return n
end function

'liefert die nach rechts zeigende Normale als Vector
function GetRight (v as vector) as vector
  dim as vector n
  n.x=v.y
  n.y=-v.x
  return n
end function

'liefert den umgekehrten Vektor
function GetReverse (v as vector) as vector
  dim as vector n
  n.x=-v.x
  n.y=-v.y
  return n
end function

'liefert den Winkel den 3 Punkte bilden, positiver Drehsinn(=gegen Uhrzeiger) beachten, als Bogenmaß
function GetRad(a as vector, b as vector, c as vector) as double
  dim as double rad
  rad=GetRad(b,c)-GetRad(b,a)
  RadiansInRange(rad)
  return rad
end function

'Verschiebt Vektor a um Vektor b
'BYREF Vektor a wird verändert!!!
sub ShiftVector(byref a as vector, b as vector)
  a.x+=b.x
  a.y+=b.y
end sub

'game objects******************************************************************
'******************************************************************************
'******************************************************************************
'Koordinatenursprung auf dem Screen:
const as integer CoordX=400
const as integer CoordY=300

'******************************************************************************
type drive
public:
  DrivePos      as vector 'globale Position des Antriebs
  Radius        as single 'BewegungsRadius der Kupplung
  Speed         as double 'Winkelgeschwindigkeit der Kupplung als Bogenmaß/Sekunde
  Connector     as vector 'globale Position Kupplung
  declare sub InitDrive (startpos as double=0)
  declare sub MoveDrive (frametime as double)
  declare sub DrawDrive (img as FB.Image ptr = 0)
private:
  localpos as double   'lokale Position der Kupplung in Bogenmaß
end type

sub drive.InitDrive (startpos as double=0)
  localpos=startpos'interne lokale Position der Kupplung merken in Bogenmaß
  Connector=GetVector(localpos,Radius)'lokale Position als Vektor setzen
  ShiftVector(Connector,DrivePos)'verschieben zu global, denn DrivePos ist global
end sub

sub drive.MoveDrive (frametime as double)
  localpos+=Speed*frametime'Geschwindigkeit "herunterbrechen" auf Winkel/Framezeit und intern aufsummen
  RadiansInRange localpos' Wert in 0-2Pi Bereich bringen
  Connector=GetVector(localpos,Radius)'lokal Position als Vektor neu setzen
  ShiftVector(Connector,DrivePos)'verschieben zu global, denn DrivePos ist global
end sub

sub drive.DrawDrive (img as FB.Image ptr = 0)
  circle img, (CoordX+DrivePos.x,CoordY-DrivePos.y),Radius,&HFFDD88
  circle img, (CoordX+Connector.x,CoordY-Connector.y),5,&HFF0000
end sub

'******************************************************************************
type scissors
/'
  D       C
   \     /  
  d \   /c
     \ /    
    Joint
     / \                    linksseitig
   a/   \b                     ^
   /     \                     |
  A       B  (input)    A----------->B
  
die Kupplungen/Knoten C,D,E befinden sich immer linksseitig der Richtung von A->B, so die Idee :)
'/
public:
  ConnectorA  as vector        'globale Position Kupplung A (Input)
  ConnectorB  as vector        'globale Position Kupplung B (Input)
  Joint       as vector        'globale Position Knoten, ergibt sich aus den Positionen A und B und den Längen a und b
  ConnectorC  as vector        'globale Position Kupplung C, ergibt sich aus der Richtung A->Joint und Länge c
  ConnectorD  as vector        'globale Position Kupplung D, ergibt sich aus der Richtung B->Joint und Länge d
  LengthA as integer
  LengthB as integer
  LengthC as integer
  LengthD as integer
  declare sub SetScissors (a as vector, b as vector)
  declare sub DrawScissors (img as FB.Image ptr = 0)
private:
  LengthAB    as double
  Direction   as double
  AngleA      as double
end type

sub scissors.SetScissors (a as vector, b as vector)
  '               C gamma                  A,B,C            =Ecken
  '               *                        a,b,c            =Seiten
  '              / \                       alpha,beta,gamma =Winkel
  '            b/   \a
  '            /     \
  '           /       \
  '  alpha A *---------* B beta
  '               c
  'lt. Gemini:
  'alpha = acos((b*b + c*c - a*a) / (2*b*c))
  'beta = acos((a*a + c*c - b*b) / (2*a*c))
  'gamma = acos((a*a + b*b - c*c) / (2*a*b))
  ConnectorA=a
  ConnectorB=b
  LengthAB=GetDistance(ConnectorA,ConnectorB)'Länge AB
  Direction=GetRad(ConnectorA,ConnectorB)'Richtung A->B als Bogenmaß
  'nun wird versucht die Position des Joint zu finden, wir kennen nun alle 3 Längen der Seiten des Dreiecks A-B-Joint
  'Winkel am ConnectorA in Bogenmaß alpha = acos((b*b + c*c - a*a) / (2*b*c)) aufsummen:
  Direction += acos( (LengthA*LengthA + LengthAB*LengthAB - LengthB*LengthB) / (2*LengthA*LengthAB) )
  'dies sollte nun die globale Richtung des Joint von ConnectorA aus gesehen sein
  'den konvertieren wir zu einem RichtungsVektor mit Länge
  Joint=GetVector(Direction,LengthA)
  'und verschieben in relativ zu ConnectorA
  ShiftVector(Joint, ConnectorA)
  'ConnectorC:
  Direction=GetRad(ConnectorA,Joint)'Richtung A->Joint
  ConnectorC=GetVector(Direction,LengthC)
  ShiftVector(ConnectorC, Joint)
  'ConnectorD:
  Direction=GetRad(ConnectorB,Joint)'Richtung B->Joint
  ConnectorD=GetVector(Direction,LengthD)
  ShiftVector(ConnectorD, Joint)
end sub

sub scissors.DrawScissors (img as FB.Image ptr = 0)
  circle img, (CoordX+Joint.x,CoordY-Joint.y),5,&HFFFF00
  circle img, (CoordX+ConnectorA.x,CoordY-ConnectorA.y),3,&H00AA00
  circle img, (CoordX+ConnectorB.x,CoordY-ConnectorB.y),3,&H00AA00
  circle img, (CoordX+ConnectorC.x,CoordY-ConnectorC.y),5,&HFF0000
  circle img, (CoordX+ConnectorD.x,CoordY-ConnectorD.y),5,&HFF0000
  line img, (CoordX+Joint.x,CoordY-Joint.y)-(CoordX+ConnectorA.x,CoordY-ConnectorA.y),&HFFFFFF
  line img, (CoordX+Joint.x,CoordY-Joint.y)-(CoordX+ConnectorB.x,CoordY-ConnectorB.y),&HFFFFFF
  line img, (CoordX+Joint.x,CoordY-Joint.y)-(CoordX+ConnectorC.x,CoordY-ConnectorC.y),&HFFFFFF
  line img, (CoordX+Joint.x,CoordY-Joint.y)-(CoordX+ConnectorD.x,CoordY-ConnectorD.y),&HFFFFFF
end sub


'Start'************************************************************************
'******************************************************************************
'******************************************************************************
'https://www.youtube.com/watch?v=w6hNFGbcPa4

const SCREEN_W = 800, SCREEN_H = 600
screenres SCREEN_W, SCREEN_H, 32
dim as FB.Image ptr bild1 = imagecreate(SCREEN_W, SCREEN_H), bild2 = imagecreate(SCREEN_W, SCREEN_H, 0)

dim as double oldtime,currtime,frametime
dim as drive m1,m2
dim as scissors s1,s2
dim as integer unit

'Motor 1
m1.DrivePos.x=-80
m1.DrivePos.y=-200
m1.Radius=50
m1.Speed=doublepi
m1.InitDrive 0
'Motor 2
m2.DrivePos.x=70
m2.DrivePos.y=-200
m2.Radius=30
m2.Speed=-doublepi- .25
m2.InitDrive 0

'1.Nach Video Scherengelenk 5:3 Einheiten Schenkellänge

s1.LengthA=150
s1.LengthB=150
s1.LengthC=90
s1.LengthD=90

'2.Nach Video Scherengelenk 3(:3)
s2.LengthA=90
s2.LengthB=90
s2.LengthC=0
s2.LengthD=0

dim key as string
dim as integer posX = -1, posY
currtime=timer
do
  sleep 1
  oldtime=currtime
  currtime=timer
  frametime=currtime-oldtime

  m1.MoveDrive(frametime)
  m2.MoveDrive(frametime)
  s1.SetScissors (m1.Connector,m2.Connector)
  s2.SetScissors (s1.ConnectorD,s1.ConnectorC)

  key=inkey

  ' Bild vom Gerät zeichnen (bild1, auf transparentem Hintergrund)
  line bild1, (0, 0)-(799, 599), &hff00ff, bf
  m1.DrawDrive (bild1)
  m2.DrawDrive (bild1)
  s1.DrawScissors (bild1)
  s2.DrawScissors (bild1)
  ' resultierende Zeichnung zeichnen (bild2)
  if posX < 0 then
    pset bild2, (CoordX+s2.Joint.x,CoordY-s2.Joint.y), &HFF0000
  else
    line bild2, (posX, posY)-(CoordX+s2.Joint.x,CoordY-s2.Joint.y), &HFF0000
  end if
  posX = CoordX+s2.Joint.x
  posY = CoordY-s2.Joint.y
  ' Ausgabe beider Bilder
  screenlock
  put (0, 0), bild2, pset
  put (0, 0), bild1, trans
  screenunlock

loop until key="q" or key= chr(255, 107)

put (0, 0), bild2, pset
imagedestroy bild1
imagedestroy bild2
if key <> chr(255, 107) then
  print "press any key"
  sleep
end if