/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2012 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

-----------------------------------------------------------------------------
*/

proto connectWiimote = fun [Timer Wiimote] I;;

struct Wiimote =[
  WII_inst        : PInstance,
  WII_Obj         : WiiObj,
  WII_iMode       : I,
  WII_iUserIr     : I,
  WII_lSmooth     : [[F F F F F] r1],
  WII_lastSmooth  : [F F F F F],
  WII_tWiiVec     : [F F F],
  WII_tWiiAng     : [F F F],
  WII_trmConnect  : Timer,
  WII_lastBtn     : I,
  WII_lastNunBtn  : I
  
  ] mkWiimote;;

var iSmooth = 20;;
var fSmoothCalib = 0.002;;
var fRollCalib = 2.0;;


fun deleteOb(inst, wiistr) =
  if wiistr.WII_trmConnect == nil then nil else
    _deltimer wiistr.WII_trmConnect;
  
	setPluginInstanceCbPreRender inst nil;
  if wiistr.WII_Obj == nil then nil else
    _CloseWiiDevice wiistr.WII_Obj;  
  0;;


fun getSmoothIr(wiistr)=
  let 0 -> i in
  let [0.0 0.0 0.0 0.0 0.0] -> moy in
  (
    while i < iSmooth do
    (
      let nth_list wiistr.WII_lSmooth i -> [irx1 iry1 irx2 iry2 roll] in
      let moy -> [ox1 oy1 ox2 oy2 oroll] in
        set moy = [(ox1 +. irx1) (oy1 +. iry1) (ox2 +. irx2) (oy2 +. iry2) (oroll +. roll)];
      
      set i = i + 1;
    );
    let moy -> [ox1 oy1 ox2 oy2 oroll] in
    let [(ox1 /. (itof iSmooth)) (oy1 /. (itof iSmooth)) (ox2 /. (itof iSmooth)) (oy2 /. (itof iSmooth)) (oroll /. (itof iSmooth))] -> [nx1 ny1 nx2 ny2 nroll] in
    let wiistr.WII_lastSmooth -> [lx1 ly1 lx2 ly2 lroll] in
      [(if ((absf (nx1 -. lx1)) >. fSmoothCalib) then nx1 else lx1)
       (if ((absf (ny1 -. ly1)) >. fSmoothCalib) then ny1 else ly1)
       (if ((absf (nx2 -. lx2)) >. fSmoothCalib) then nx2 else lx2)
       (if ((absf (ny2 -. ly2)) >. fSmoothCalib) then ny2 else ly2)
       (if ((absf (nroll -. lroll)) >. fRollCalib) then nroll else lroll)
      ];
  );;


fun setSmooth(wiistr, val)=
  let wiistr.WII_lSmooth -> [_ next] in
    set wiistr.WII_lSmooth = lcat next val::nil;
  0;;


fun cbIrMouse(wiimote, wiistr, irvec)=
  let irvec -> [irx1 iry1 irx2 iry2] in
  if wiistr.WII_lSmooth == nil then
  (  
    let 0 -> i in
    while i < iSmooth do
    (
      set wiistr.WII_lSmooth = [irx1 iry1 irx2 iry2 (itof 0)]::wiistr.WII_lSmooth;
      set i = i + 1;
    );
    0;
  )
  else
  (
    setSmooth wiistr [irx1 iry1 irx2 iry2 (itof 0)];
    
    let (V3DgetSessionView c3dXsession) -> viewstr in
    let _GETscreenSize -> [scrnw scrnh] in
    let getSmoothIr wiistr -> [irx1 iry1 irx2 iry2 sroll] in
    let absf (irx2 -. irx1) -> zoom in
    let if irx1 == nil || irx2 == nil then 0.0 else zoom -> zoom in  
    
    let ((irx2 +. irx1) /. 2.0) -> vdotx in    
    let ((iry2 +. iry1) /. 2.0) -> vdoty in 
    /*
    let cos sroll -> cosroll in
    let sin sroll -> sinroll in
    
    let vdotx -. (zoom /. 2.0) -> tx in
    let vdoty -. (zoom /. 2.0) -> ty in
    
    let (tx *. cosroll) -. (ty *. sinroll) -> rdx in
    let (tx *. sinroll) +. (ty *. cosroll) -> rdy in
    
    let rdx +. (zoom /. 2.0) -> vdotx in
    let rdy +. (zoom /. 2.0) -> vdoty in
    */
    let ftoi (vdotx *. ((itof scrnw) +. ((zoom +. 0.2) *. (itof scrnw)) )) -> irposx in
    let ftoi (vdoty *. ((itof scrnh) +. ((zoom +. 0.2) *. (itof scrnh)) )) -> irposy in
  
    let irposx - (ftoi (((zoom +. 0.2) /. 2.0) *. (itof scrnw))) -> irposx in
    let irposy - (ftoi (((zoom +. 0.2) /. 2.0) *. (itof scrnh))) -> irposy in  
    if irx1 == nil || irx2 == nil then nil else
    (
      if !wiistr.WII_iUserIr then nil else
        _SETscreenPos irposx irposy;
        
      SendPluginEvent wiistr.WII_inst "IR pos" strbuild ((itoa irposx)::(itoa irposy)::nil)::nil nil;
      set wiistr.WII_lastSmooth = [irx1 iry1 irx2 iry2 sroll];
    );  
    0;
  );
  0;;


fun cbBalanceBoard(wiimote, wiistr, wiidata)=
  let wiidata -> [tl tr bl br total] in
  let if total <=. 0.1 then 0.0 else (((((-.tl) +. (-.bl)) /. 2.0) +. ((tr +. br) /. 2.0)) /. 2.0)/* /. total*/ -> x in
  let if total <=. 0.1 then 0.0 else ((((tl +. (-.bl)) /. 2.0) +. ((tr +. (-.br)) /. 2.0)) /. 2.0)/* /. total*/ -> y in
  (
    SendPluginEvent wiistr.WII_inst "BalanceBoard" (ftoa total) nil;
    SendPluginEvent wiistr.WII_inst "BalanceBoard center" strbuild ((ftoa x)::(ftoa y)::nil)::nil nil;
  );
  0;;


fun cbWiimoteAccel(wiimote, wiistr, accelv, orientv, ang)=
	let accelv -> [vx vy vz] in
	let orientv -> [ax ay az] in
  (
    SendPluginEvent wiistr.WII_inst "Wiimote accel" strbuild ((ftoa vx)::(ftoa vy)::(ftoa vz)::nil)::nil nil;
    SendPluginEvent wiistr.WII_inst "Wiimote orientation" strbuild ((ftoa ax)::(ftoa ay)::(ftoa az)::nil)::nil nil;
  );
  0;;


fun cbWiimoteButton(wiimote, wiistr, but)=
  let wiistr.WII_tWiiVec -> v in
  let wiistr.WII_tWiiAng -> a in
  let _GetWiiExtensionType wiistr.WII_Obj -> exttype in
  (
    if ((wiistr.WII_lastBtn & 256) && !(but & 256)) then
      SendPluginEvent wiistr.WII_inst "Left unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 512) && !(but & 512)) then
      SendPluginEvent wiistr.WII_inst "Right unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 64) && !(but & 64)) then
      SendPluginEvent wiistr.WII_inst "Up unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 128) && !(but & 128)) then
      SendPluginEvent wiistr.WII_inst "Down unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 1024) && !(but & 1024)) then
      SendPluginEvent wiistr.WII_inst "Home unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 1) && !(but & 1)) then
      SendPluginEvent wiistr.WII_inst "One unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 2) && !(but & 2)) then
      SendPluginEvent wiistr.WII_inst "Two unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 8) && !(but & 8)) then
      SendPluginEvent wiistr.WII_inst "B unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 4) && !(but & 4)) then
      SendPluginEvent wiistr.WII_inst "A unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 16) && !(but & 16)) then
      SendPluginEvent wiistr.WII_inst "Plus unpressed" nil nil
    else nil;
    
    if ((wiistr.WII_lastBtn & 32) && !(but & 32)) then
      SendPluginEvent wiistr.WII_inst "Minus unpressed" nil nil
    else nil;
    
    if but & 256 && exttype == 1 then // left
    (
      SendPluginEvent wiistr.WII_inst "Left" nil nil;
      mutate v<-[(-.0.5) _ _]
    )
    else if but & 512 && exttype == 1 then // right
    (
      SendPluginEvent wiistr.WII_inst "Right" nil nil;
      mutate v<-[0.5 _ _]
    )
    else if !(but & 512) && !(but & 256) && (exttype == 1) then // disable right
    (
      mutate v<-[0.0 _ _]
    )
    else nil;
    
    if but & 64 && exttype == 1 then // up
    (
      SendPluginEvent wiistr.WII_inst "Up" nil nil;
      mutate a<-[_ 0.5 _]
    )
    else if but & 128 && exttype == 1 then // down
    (
      SendPluginEvent wiistr.WII_inst "Down" nil nil;
      mutate a<-[_ (-.0.5) _]
    )
    else if !(but & 64) && !(but & 128) && (exttype == 1) then // disable right
    (
      mutate a<-[_ 0.0 _]
    )
    else nil;
    
    if but & 64 && exttype != 1 then // up
    (
      SendPluginEvent wiistr.WII_inst "Up" nil nil;
      mutate v<-[_ _ 1.0]
    )
    else if but & 128 && exttype != 1 then // down
    (
      SendPluginEvent wiistr.WII_inst "Down" nil nil;
      mutate v<-[_ _ (-.1.0)]
    )
    else if !(but & 64) && !(but & 128) && (exttype != 1) then // disable up
    (
      mutate v<-[_ _ 0.0]
    )
    else nil;
    
    if but & 256 && exttype != 1 then // left
    (
      SendPluginEvent wiistr.WII_inst "Left" nil nil;
      mutate a<-[1.0 _ _]
    )
    else if but & 512 && exttype != 1 then // right
    (
      SendPluginEvent wiistr.WII_inst "Right" nil nil;
      mutate a<-[(-.1.0) _ _]
    )
    else if !(but & 512) && !(but & 256) && (exttype != 1) then // disable right
    (
      mutate a<-[0.0 _ _]
    )
    else nil;
         
    if but & 1024 then // home
    (
      SendPluginEvent wiistr.WII_inst "Home" nil nil;
    )
    else nil;
        
    if but & 1 then // 1
    (
      SendPluginEvent wiistr.WII_inst "One" nil nil;
    )
    else nil;      
 
    if but & 2 then // 2
    (
      SendPluginEvent wiistr.WII_inst "Two" nil nil;
    )                 
    else nil;      

    if but & 8 then // B
    (
      SendPluginEvent wiistr.WII_inst "B" nil nil;
      
      if !wiistr.WII_iUserIr then nil else
      (
      	// click droit
      	_EmulateMouseClick 2 1;
      	_EmulateMouseClick 2 0;
     	);
    )
    else nil;
    
    if but & 16 then // plus
    (
      SendPluginEvent wiistr.WII_inst "Plus" nil nil;
    )
    else nil;
    
    if but & 32 then // minus
    (
      SendPluginEvent wiistr.WII_inst "Minus" nil nil;
    )
    else nil;
      
    if but & 4 then // A
    (
      SendPluginEvent wiistr.WII_inst "A" nil nil;
    
      if !wiistr.WII_iUserIr then nil else
      (
        _EmulateMouseClick 1 1;
        _EmulateMouseClick 1 0;
      );
      0;
    )
    else nil;
  );
  
  set wiistr.WII_lastBtn = but;
  0;;


fun cbStartRumble(inst, from, action, param, reply, wiistr)=
  _SetWiiVibrate wiistr.WII_Obj 1;
  0;;


fun cbStopRumble(inst, from, action, param, reply, wiistr)=
   _SetWiiVibrate wiistr.WII_Obj 0;
  0;;


fun cbSetLeds(inst, from, action, param, reply, wiistr)=
  let strextr param -> l in
  let hd l -> p in
  let atoi nth_list p 0 -> l1 in
  let atoi nth_list p 1 -> l2 in
  let atoi nth_list p 2 -> l3 in
  let atoi nth_list p 3 -> l4 in
    _SetWiiLeds wiistr.WII_Obj l1 l2 l3 l4;
  0;;


fun cbWiimoteLost(wiimote, wiistr)=
  SendPluginEvent wiistr.WII_inst "Disconnected" nil nil;
  connectWiimote nil wiistr;
  0;;


fun cbNunchukButton(wiimote, wiistr, but)=
  if ((wiistr.WII_lastNunBtn & 1) && !(but & 1)) then
    SendPluginEvent wiistr.WII_inst "C unpressed" nil nil
  else nil;

  if ((wiistr.WII_lastNunBtn & 2) && !(but & 2)) then
    SendPluginEvent wiistr.WII_inst "Z unpressed" nil nil
  else nil;

  if but & 1 then // C
  (
    SendPluginEvent wiistr.WII_inst "C" nil nil;
  )
  else if but & 2 then // Z
  (
    SendPluginEvent wiistr.WII_inst "Z" nil nil;
  )
  else nil;
  
  set wiistr.WII_lastNunBtn = but;
	0;;


fun cbNunchukAccel(wiimote, wiistr, accelv, orientv, pitchroll)=
	let accelv -> [vx vy vz] in
	let orientv -> [ax ay az] in
  (
    SendPluginEvent wiistr.WII_inst "Nunchuk accel" strbuild ((ftoa vx)::(ftoa vy)::(ftoa vz)::nil)::nil nil;
    SendPluginEvent wiistr.WII_inst "Nunchuk orientation" strbuild ((ftoa ax)::(ftoa ay)::(ftoa az)::nil)::nil nil;
  );
	0;;


fun cbNunchukJoy(wiimote, wiistr, pos)=
	let pos -> [x y] in
	(
		mutate wiistr.WII_tWiiAng<-[-.x _ _];
    mutate wiistr.WII_tWiiVec<-[_ _ y];
		SendPluginEvent wiistr.WII_inst "Nunchuk stick" (strbuild ((ftoa x)::(ftoa y)::nil)::nil) nil;
  );
	0;;


fun cbWiiExtConnected(wiimote, wiistr, exttype)=
  //reset control
  set wiistr.WII_tWiiVec = [0.0 0.0 0.0];
  set wiistr.WII_tWiiAng = [0.0 0.0 0.0];

  // Nunchuk
  if exttype == 1 then
  (
  	_CBnunchukButton wiimote @cbNunchukButton wiistr;
  	_CBnunchukAccel wiimote @cbNunchukAccel wiistr;
  	_CBnunchukJoy wiimote @cbNunchukJoy wiistr;
  	SendPluginEvent wiistr.WII_inst "Nunchuk connected" nil nil;
  	0;
  )
  // classic
  else if exttype == 2 then
  (
		_CBnunchukButton wiimote nil nil;
		_CBnunchukAccel wiimote nil nil;
		_CBnunchukJoy wiimote nil nil;
  	0;
  )
  // motion plus
  else if exttype == 3 then
  (
		_CBnunchukButton wiimote nil nil;
		_CBnunchukAccel wiimote nil nil;
		_CBnunchukJoy wiimote nil nil;
  	0;
  )
  else if exttype == 4 then
  (
    _CBwiiBalanceBoard wiistr.WII_Obj @cbBalanceBoard wiistr;
    SendPluginEvent wiistr.WII_inst "Connected" nil nil;
    0;
  )
  else
  (
		_CBnunchukButton wiimote nil nil;
		_CBnunchukAccel wiimote nil nil;
		_CBnunchukJoy wiimote nil nil;
		0;
  );
  0;;


fun cbWiiExtDisconnected(wiimote, wiistr, exttype)=
  set wiistr.WII_tWiiVec = [0.0 0.0 0.0];
  set wiistr.WII_tWiiAng = [0.0 0.0 0.0];
  SendPluginEvent wiistr.WII_inst "Extension disconnected" nil nil;
  //_DLGMessageBox _channel nil "Warning" strcat "Wiimote extension disconnected" (itoa exttype) 0;
  0;;


//todo event control
fun cbWiiPreRender(inst, viewstr, wiistr)=
  let wiistr.WII_tWiiVec -> [vx vy vz] in
  let wiistr.WII_tWiiAng -> [ax ay az] in
  (
    SendPluginEvent inst "Control" (strbuild ((ftoa vx)::(ftoa vy)::(ftoa -.vz)::nil)::((ftoa ay)::(ftoa ax)::(ftoa az)::nil)::nil) inst.INST_sName;   
  );
	0;;


fun connectWiimote(trm, wiistr)=
  set wiistr.WII_tWiiVec = [0.0 0.0 0.0];
  set wiistr.WII_tWiiAng = [0.0 0.0 0.0];
  
  _deltimer wiistr.WII_trmConnect;
  set wiistr.WII_trmConnect = nil;
  
  _CloseWiiDevice wiistr.WII_Obj;
  set wiistr.WII_Obj = nil;
  
  let 1 -> i in
  (
    set wiistr.WII_Obj = _OpenWiiDevice _channel i;
    set i = i + 1;
    //test type only for balance board
    while ((wiistr.WII_Obj == nil) || (((_GetWiiExtensionType wiistr.WII_Obj) != wiistr.WII_iMode) && (wiistr.WII_iMode == 4))) && (i < 9) do
    (
      //_CloseWiiDevice wiistr.WII_Obj;
      set wiistr.WII_Obj = _OpenWiiDevice _channel i;
      set i = i + 1;
    );
  );
  
  if wiistr.WII_Obj == nil then
  (
    set wiistr.WII_trmConnect = _rfltimer _starttimer _channel 5000 @connectWiimote wiistr;
    0;
  )
  else if wiistr.WII_iMode == 4 then
  (
    _CBwiiBalanceBoard wiistr.WII_Obj @cbBalanceBoard wiistr;
    _SetWiiLeds wiistr.WII_Obj 1 0 0 0;
    
    _CBwiimoteConnectionLost wiistr.WII_Obj @cbWiimoteLost wiistr;
    setPluginInstanceCbPreRender wiistr.WII_inst mkfun3 @cbWiiPreRender wiistr;  
    _CBextensionDisconnected wiistr.WII_Obj @cbWiiExtDisconnected wiistr;
    _CBextensionConnected wiistr.WII_Obj @cbWiiExtConnected wiistr;
    SendPluginEvent wiistr.WII_inst "Connected" nil nil;
    0;
  )
  else
  (
    _CBwiimoteIr wiistr.WII_Obj @cbIrMouse wiistr;
    _CBwiimoteButton wiistr.WII_Obj @cbWiimoteButton wiistr;
    _CBwiimoteAccel wiistr.WII_Obj @cbWiimoteAccel wiistr;
    _SetWiiLeds wiistr.WII_Obj 1 0 0 0;
    
    _CBwiimoteConnectionLost wiistr.WII_Obj @cbWiimoteLost wiistr;
    setPluginInstanceCbPreRender wiistr.WII_inst mkfun3 @cbWiiPreRender wiistr;  
    _CBextensionDisconnected wiistr.WII_Obj @cbWiiExtDisconnected wiistr;
    _CBextensionConnected wiistr.WII_Obj @cbWiiExtConnected wiistr;
    SendPluginEvent wiistr.WII_inst "Connected" nil nil;
    0;
  );
  0;;


fun cbConnect(inst, from, action, param, reply, wiistr)=
  connectWiimote nil wiistr;
  0;;
  

fun cbDisconnect(inst, from, action, param, reply, wiistr)=
  set wiistr.WII_tWiiVec = [0.0 0.0 0.0];
  set wiistr.WII_tWiiAng = [0.0 0.0 0.0];
  SendPluginEvent inst "Control" (strbuild ((ftoa 0.0)::(ftoa 0.0)::(ftoa -.0.0)::nil)::((ftoa 0.0)::(ftoa 0.0)::(ftoa 0.0)::nil)::nil) inst.INST_sName;
  
  if wiistr.WII_trmConnect == nil then nil else
    _deltimer wiistr.WII_trmConnect;
  
	setPluginInstanceCbPreRender inst nil;
  if wiistr.WII_Obj == nil then nil else
    _CloseWiiDevice wiistr.WII_Obj; 
  0;;


fun newOb(inst)=
  let atoi (getPluginInstanceParam inst "ir") -> useir in
  let atoi (getPluginInstanceParam inst "mode") -> mode in
  let atoi (getPluginInstanceParam inst "init") -> init in
  let if init == nil then 1 else init -> init in
  let if useir == nil then 0 else useir -> useir in
  let mkWiimote [inst nil mode useir nil nil [0.0 0.0 0.0] [0.0 0.0 0.0] nil 0 0] -> wiistr in
  (
    if !init then nil else
      connectWiimote nil wiistr;
     
    PluginRegisterAction inst "Start rumble" mkfun6 @cbStartRumble wiistr;
    PluginRegisterAction inst "Stop rumble" mkfun6 @cbStopRumble wiistr;
    PluginRegisterAction inst "Set leds" mkfun6 @cbSetLeds wiistr;
    PluginRegisterAction inst "Connect" mkfun6 @cbConnect wiistr;
    PluginRegisterAction inst "Disconnect" mkfun6 @cbDisconnect wiistr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb wiistr;
  );
  0;;


fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;