/*
-----------------------------------------------------------------------------
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

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

/* Authors: Bastien Bourineau */
/* Last update: 21/10/2009 */
/* Version: 1.0 */

struct TobjGoto = [
                    OBJG_inst       : PInstance,
                    OBJG_h3d        : SO3_OBJECT,
                    OBJG_desth3d    : SO3_OBJECT,
                    OBJG_iSrcMode   : I,
                    OBJG_iDestMode  : I,
                    OBJG_father     : SO3_OBJECT,
                    OBJG_TCB        : [F F F],
                    OBJG_inipos     : [[F F F] [F F F F]],
                    OBJG_iniGlobalpos: [[F F F] [F F F F]],
                    OBJG_dummy      : SO3_OBJECT,
                    OBJG_srcpos     : [[F F F] [F F F F] [F F F] F],
                    OBJG_dstpos     : [[F F F] [F F F F] [F F F] F],
                    OBJG_posindex   : I,
                    OBJG_angcoef    : F,
                    OBJG_coef       : F,
                    OBJG_angstep    : F,
                    OBJG_step       : F,
                    OBJG_bezier     : F,
                    OBJG_fps        : I,
                    OBJG_speed      : F,
                    OBJG_oldindex   : I,
                    OBJG_state      : I
                    
                  ] mkTobjGoto;;


// 2t3 - 3t2 + 1
fun H1(coef)=
  (2.0 *. (pow coef 3.0)) -. (3.0 *. (pow coef 2.0)) +. 1.0;;


// -2t3 + 3t2
fun H2(coef)=
  (-. 2.0 *. (pow coef 3.0)) +. (3.0 *. (pow coef 2.0));;


// t3 - 2t2 +t
fun H3(coef)=
  (pow coef 3.0) -. (2.0 *. (pow coef 2.0)) +. coef;;


// t3 - t2
fun H4(coef)=
  (pow coef 3.0) -. (pow coef 2.0);;


fun getInterpolateCurve(vec1, vec2, ang1, ang2, coef)=
  let vec1 -> [x1 y1 z1] in
  let vec2 -> [x2 y2 z2] in
  let ang1 -> [ax1 ay1 az1] in
  let ang2 -> [ax2 ay2 az2] in

  let ((H1 coef) *. x1) +. ((H2 coef) *. x2) +. ((H3 coef) *. ax1) +. ((H4 coef) *. ax2) -> x in
  let ((H1 coef) *. y1) +. ((H2 coef) *. y2) +. ((H3 coef) *. ay1) +. ((H4 coef) *. ay2) -> y in
  let ((H1 coef) *. z1) +. ((H2 coef) *. z2) +. ((H3 coef) *. az1) +. ((H4 coef) *. az2) -> z in
  [x y z]
  ;;


fun getPosTime(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  ti;;


fun getPosTension(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  t;;


fun getPosContinuity(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  c;;


fun getPosBias(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  b;;


fun getPosX(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  x;;


fun getPosY(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  y;;


fun getPosZ(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  z;;


fun getPosXYZ(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  [x y z];;
  

fun getInterpolateTCB(lpos, t)=
	let sizelist lpos -> size in
	let size - 1 -> sizeminus in
	let 0 -> i in
	let nil -> outvec in
	(
  	while i < size && outvec == nil do
  	(
  	  let nth_list lpos i -> nextpos in
      if t <. (getPosTime nextpos) then
      (
        let if i == 0 then nth_list lpos (sizeminus - 1) else nth_list lpos (i - 1) -> curpos in
        let if i == 0 then nth_list lpos (sizeminus - 2) else if i == 1 then nth_list lpos (sizeminus - 1) else nth_list lpos (i - 2) -> prevpos in
        let if i == sizeminus then nth_list lpos 0 else nth_list lpos (i + 1) -> nextnextpos in  
        
        let ((t -. (getPosTime curpos)) /. ((getPosTime nextpos) -. (getPosTime curpos))) -> coef in
        
        //Update values from TCB
        let (1.0 -. (getPosTension curpos)) -> OneMinusTension in
        let (1.0 -. (getPosContinuity curpos)) -> OneMinusContinuity in
        let (1.0 +. (getPosContinuity curpos)) -> OnePlusContinuity in
        let (1.0 -. (getPosBias curpos)) -> OneMinusBias in
        let (1.0 +. (getPosBias curpos)) -> OnePlusBias in
        
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosX nextpos) -. (getPosX curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosX curpos) -. (getPosX prevpos) ))) -> tancurposx in
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosY nextpos) -. (getPosY curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosY curpos) -. (getPosY prevpos) ))) -> tancurposy in
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosZ nextpos) -. (getPosZ curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosZ curpos) -. (getPosZ prevpos) ))) -> tancurposz in
  
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosX nextpos) -. (getPosX curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosX nextnextpos) -. (getPosX nextpos) ))) -> tannextposx in
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosY nextpos) -. (getPosY curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosY nextnextpos) -. (getPosY nextpos) ))) -> tannextposy in
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosZ nextpos) -. (getPosZ curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosZ nextnextpos) -. (getPosZ nextpos) ))) -> tannextposz in
        
        let getInterpolateCurve (getPosXYZ curpos) (getPosXYZ nextpos) [tancurposx tancurposy tancurposz] [tannextposx tannextposy tannextposz] coef -> [x y z] in
          set outvec = [i [x y z]];
        
        0;
      )
      else nil;
      
      set i = i + 1;
    );
    outvec;
  );;


fun cbMoveObj(inst, sessionstr, etime, strobj, cbend)=
  let ((itof etime) /. 1000.0) /. (1000.0 /. (itof strobj.OBJG_fps)) -> rtick in
  (
    if (strobj.OBJG_coef >=. 1.0) then
    (
      if strobj.OBJG_state == 2 || strobj.OBJG_state == 0 then nil else
      (
        // event
        set strobj.OBJG_state = 2;
        
        //force last frame
        let strobj.OBJG_dstpos -> [dstvec dstang _ _] in
        (
          SO3ObjectSetPosition strobj.OBJG_dummy dstvec;
          SO3ObjectSetOrientation strobj.OBJG_dummy dstang;
        );
        exec cbend with [strobj];
        0;
      );
    )
    else
    (
      let rtick *. strobj.OBJG_step -> nstep in    
        set strobj.OBJG_coef = if (strobj.OBJG_coef +. nstep) >. 1.0 then 1.0 else strobj.OBJG_coef +. nstep;
            
      let getInterpolateTCB strobj.OBJG_srcpos::strobj.OBJG_dstpos::nil strobj.OBJG_coef -> [index tvec] in
      
      let nth_list strobj.OBJG_srcpos::strobj.OBJG_dstpos::nil (index - 1) -> [spos sang _ _] in
      let nth_list strobj.OBJG_srcpos::strobj.OBJG_dstpos::nil index -> [dpos dang _ _] in      
      (
        if strobj.OBJG_oldindex != index then
        (
           set strobj.OBJG_angcoef = 0.0;
           set strobj.OBJG_oldindex = index;
        )
        else nil;
        
        let rtick *. strobj.OBJG_angstep -> nangstep in
          set strobj.OBJG_angcoef = if (strobj.OBJG_angcoef +. nangstep) >. 1.0 then 1.0 else strobj.OBJG_angcoef +. nangstep;
        
        let SO3MathsQuatInterpolate sang dang strobj.OBJG_angcoef 1 -> tang in
        (
          SO3ObjectSetPosition strobj.OBJG_dummy tvec;
          SO3ObjectSetOrientation strobj.OBJG_dummy tang;
          0;
        );
      );
    );
  );
  0;;


fun getTCBLength(strobj)=
  let 0.0 -> ndata in
  let 0.0 -> i in
  let 1.0 /. ((itof strobj.OBJG_fps) /. strobj.OBJG_speed) -> stp in
  let nil -> prevpos in
  (
    while i <=. 1.0 do
    (
      let getInterpolateTCB strobj.OBJG_srcpos::strobj.OBJG_dstpos::nil i -> [index tvec] in
      (
        if prevpos == nil || tvec == nil then nil else
          let prevpos -> [xa ya za] in
          let tvec -> [xb yb zb] in
          let sqrt ( (sqr(xa -. xb)) +. (sqr(ya -. yb)) +. (sqr(za -. zb)) ) -> dist in
            set ndata = ndata +. dist;
        
        if tvec == nil then nil else
          set prevpos = tvec;
      );
      set i = i +. stp;
    );
    ndata;
  );;


fun cbObjEnd(strobj)=
  SendPluginEvent strobj.OBJG_inst "End" nil nil;
  0;;


fun cbObjGoto(o, from, action, param, reply, strobj)=
  if (strobj.OBJG_state != 0) then nil else
  (
    if strobj.OBJG_iSrcMode == 0 then nil else
      set strobj.OBJG_h3d = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJG_iSrcMode;
      
    if strobj.OBJG_iDestMode == 0 then nil else
      set strobj.OBJG_desth3d = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJG_iDestMode;
    
    set strobj.OBJG_father = SO3ObjectGetParent strobj.OBJG_h3d;
    set strobj.OBJG_dummy = SO3SceneNodeCreate (V3DgetSession c3dXsession) strcat (getPluginInstanceName strobj.OBJG_inst) "_shell";
    set strobj.OBJG_inipos = [(SO3ObjectGetPosition strobj.OBJG_h3d) (SO3ObjectGetOrientation strobj.OBJG_h3d)];
    set strobj.OBJG_iniGlobalpos = [(SO3ObjectGetGlobalPosition strobj.OBJG_h3d) (SO3ObjectGetGlobalOrientation strobj.OBJG_h3d)];
    
    SO3ObjectLink strobj.OBJG_dummy(SO3ObjectGetSceneRoot (V3DgetSession c3dXsession));
    
    // récupère la position courante de la caméra dans le monde vecteur et angle
    let SO3ObjectGetGlobalPosition strobj.OBJG_h3d -> vec in
    let SO3ObjectGetGlobalOrientation strobj.OBJG_h3d -> ang in
    let SO3ObjectGetGlobalPosition strobj.OBJG_desth3d -> dstvec in
    let SO3ObjectGetGlobalOrientation strobj.OBJG_desth3d -> dstang in
    (
      set strobj.OBJG_srcpos = [vec ang strobj.OBJG_TCB 0.0];
      set strobj.OBJG_dstpos = [dstvec dstang [0.0 0.0 0.0] 1.0];
      SO3ObjectSetPosition strobj.OBJG_dummy vec;
      SO3ObjectSetOrientation strobj.OBJG_dummy ang;
    );
    
    SO3ObjectLink strobj.OBJG_h3d strobj.OBJG_dummy;
    SO3ObjectSetPosition strobj.OBJG_h3d [0.0 0.0 0.0];
    SO3ObjectSetOrientation strobj.OBJG_h3d [0.0 0.0 0.0 1.0];
    
    // calcul de la longeur de la courbe
    let getTCBLength strobj -> tcbdist in
    (
      if tcbdist == 0.0 then 
      (
        set strobj.OBJG_angstep = 1.0;
        set strobj.OBJG_angcoef = 1.0;
        set strobj.OBJG_step = 1.0;
        set strobj.OBJG_coef = 1.0;
      )
      else
      (
        set strobj.OBJG_coef = 0.0;
        set strobj.OBJG_angcoef = 0.0;
        set strobj.OBJG_step = (1.0 /. (tcbdist *. ((itof strobj.OBJG_fps) /. strobj.OBJG_speed)));
        set strobj.OBJG_angstep = (1.0 /. (tcbdist *. ((itof strobj.OBJG_fps) /. strobj.OBJG_speed)));
      );
    );
    
    set strobj.OBJG_state = 1;
    
    setPluginInstanceCbScenePostRender strobj.OBJG_inst mkfun4 mkfun5 @cbMoveObj @cbObjEnd strobj;
  );
  0;;


fun objRelease(strobj)=
  if (strobj.OBJG_state == 0) then nil else
  (
    SO3ObjectLink strobj.OBJG_h3d strobj.OBJG_father;
    let strobj.OBJG_inipos -> [vec ang] in
    (
      SO3ObjectSetPosition strobj.OBJG_h3d vec;
      SO3ObjectSetOrientation strobj.OBJG_h3d ang;
    );
    
    SO3ObjectDestroy strobj.OBJG_dummy;
    set strobj.OBJG_dummy = nil;
    set strobj.OBJG_father = nil;
    set strobj.OBJG_state = 0;
    setPluginInstanceCbScenePostRender strobj.OBJG_inst nil;
  );
  0;;


fun cbObjRelease(strobj)=
  if (strobj.OBJG_state == 0) then nil else
  (
    objRelease strobj;
    SendPluginEvent strobj.OBJG_inst "Back end" nil nil;
  );
  0;;


fun cbObjBack(inst, from, action, param, reply, strobj)=
  if (strobj.OBJG_state == 0) then nil else
  (
    let strobj.OBJG_iniGlobalpos -> [gvec grot] in
      set strobj.OBJG_dstpos = [gvec grot [0.0 0.0 0.0] 1.0];
    set strobj.OBJG_srcpos = [(SO3ObjectGetGlobalPosition strobj.OBJG_dummy) (SO3ObjectGetGlobalOrientation strobj.OBJG_dummy) strobj.OBJG_TCB 0.0];

    // calcul de la longeur de la courbe
    let getTCBLength strobj -> tcbdist in
    (
      if tcbdist == 0.0 then
      (
        set strobj.OBJG_angstep = 1.0;
        set strobj.OBJG_angcoef = 1.0;
        set strobj.OBJG_step = 1.0;
        set strobj.OBJG_coef = 1.0;
      )
      else
      (
        set strobj.OBJG_coef = 0.0;
        set strobj.OBJG_angcoef = 0.0;
        set strobj.OBJG_step = (1.0 /. (tcbdist *. ((itof strobj.OBJG_fps) /. strobj.OBJG_speed)));
        set strobj.OBJG_angstep = (1.0 /. (tcbdist *. ((itof strobj.OBJG_fps) /. strobj.OBJG_speed)));
      );
    );
    
    set strobj.OBJG_state = 1;
    setPluginInstanceCbScenePostRender strobj.OBJG_inst mkfun4 mkfun5 @cbMoveObj @cbObjRelease strobj;
  );
  0;;


fun cbSetSpeed(inst, from, action, param, reply, strobj)=
  let atof param -> speed in
  if (speed == nil) || (speed <=. 0.0) then nil else
    set strobj.OBJG_speed = atof param;
  0;;


fun deleteOb(inst, strobj)=
  objRelease strobj;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "destobject") -> destname in
  let (getPluginInstanceParam inst "sourceobject") -> sourcename in
  let atoi (getPluginInstanceParam inst "fps") -> fps in
  let if (fps == nil) then 60 else fps -> fps in
  let atof (getPluginInstanceParam inst "speed") -> speed in
  let atof (getPluginInstanceParam inst "T") -> ft in
  let atof (getPluginInstanceParam inst "C") -> fc in
  let atof (getPluginInstanceParam inst "B") -> fb in
  
  // for compatibility
  let if ((atoi (getPluginInstanceParam inst "iscamera")) == 1) then "Current camera" else sourcename -> sourcename in
  
  let V3DgetObjectByName c3dXsession sourcename -> source in
  let V3DgetObjectTypeByName sourcename -> isourcemode in
  let V3DgetObjectByName c3dXsession destname -> objdest in
  let V3DgetObjectTypeByName destname -> idestmode in

  let mkTobjGoto [inst source objdest isourcemode idestmode nil nil nil nil nil nil nil 0 0.0 0.0 nil nil 0.0 nil nil nil 0] -> strobj in
  (
    set strobj.OBJG_TCB = [ft fc fb];
    set strobj.OBJG_fps = fps;
    set strobj.OBJG_speed = speed;
    
    PluginRegisterAction inst "Goto" mkfun6 @cbObjGoto strobj;
    PluginRegisterAction inst "Back" mkfun6 @cbObjBack strobj;
    PluginRegisterAction inst "Set speed" mkfun6 @cbSetSpeed strobj;

    setPluginInstanceCbDel inst mkfun2 @deleteOb strobj;
  );
  0;;


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