//
// MATLAB Compiler: 3.0
// Date: Sat Nov 10 11:37:03 2007
// Arguments: "-B" "macro_default" "-O" "all" "-O" "fold_scalar_mxarrays:on"
// "-O" "fold_non_scalar_mxarrays:on" "-O" "optimize_integer_for_loops:on" "-O"
// "array_indexing:on" "-O" "optimize_conditionals:on" "-B" "sglcpp" "-p" "-W"
// "main" "-L" "Cpp" "-t" "-T" "link:exe" "-h" "libmmfile.mlib" "-W" "mainhg"
// "libmwsglm.mlib" "dandao.m" 
//
#include "dandao.hpp"
#include "libmatlbm.hpp"
static mwArray _mxarray0_ = mclInitializeDouble(.017453292519943295);
static mwArray _mxarray1_ = mclInitializeDouble(-1.0);
static mwArray _mxarray2_ = mclInitializeDouble(2.0);

void InitializeModule_dandao() {
}

void TerminateModule_dandao() {
}

static mwArray Mdandao(mwArray * ty1,
                       mwArray * tz1,
                       mwArray * mx1,
                       mwArray * my1,
                       mwArray * mz1,
                       mwArray * mp1,
                       mwArray * mh1,
                       mwArray * r,
                       int nargout_,
                       mwArray tx,
                       mwArray ty,
                       mwArray tz,
                       mwArray tp,
                       mwArray th,
                       mwArray mx,
                       mwArray my,
                       mwArray mz,
                       mwArray vm,
                       mwArray vt,
                       mwArray dt,
                       mwArray m);

_mexLocalFunctionTable _local_function_table_dandao
  = { 0, (mexFunctionTableEntry *)NULL };

//
// The function "dandao" contains the normal interface for the "dandao"
// M-function from file "d:\matlab6p5\work\dll2\dandao.m" (lines 1-36). This
// function processes any input arguments and passes them to the implementation
// version of the function, appearing above.
//
mwArray dandao(mwArray * ty1,
               mwArray * tz1,
               mwArray * mx1,
               mwArray * my1,
               mwArray * mz1,
               mwArray * mp1,
               mwArray * mh1,
               mwArray * r,
               mwArray tx,
               mwArray ty,
               mwArray tz,
               mwArray tp,
               mwArray th,
               mwArray mx,
               mwArray my,
               mwArray mz,
               mwArray vm,
               mwArray vt,
               mwArray dt,
               mwArray m) {
    int nargout = 1;
    mwArray tx1 = mwArray::UNDEFINED;
    mwArray ty1__ = mwArray::UNDEFINED;
    mwArray tz1__ = mwArray::UNDEFINED;
    mwArray mx1__ = mwArray::UNDEFINED;
    mwArray my1__ = mwArray::UNDEFINED;
    mwArray mz1__ = mwArray::UNDEFINED;
    mwArray mp1__ = mwArray::UNDEFINED;
    mwArray mh1__ = mwArray::UNDEFINED;
    mwArray r__ = mwArray::UNDEFINED;
    if (ty1 != NULL) {
        ++nargout;
    }
    if (tz1 != NULL) {
        ++nargout;
    }
    if (mx1 != NULL) {
        ++nargout;
    }
    if (my1 != NULL) {
        ++nargout;
    }
    if (mz1 != NULL) {
        ++nargout;
    }
    if (mp1 != NULL) {
        ++nargout;
    }
    if (mh1 != NULL) {
        ++nargout;
    }
    if (r != NULL) {
        ++nargout;
    }
    tx1
      = Mdandao(
          &ty1__,
          &tz1__,
          &mx1__,
          &my1__,
          &mz1__,
          &mp1__,
          &mh1__,
          &r__,
          nargout,
          tx,
          ty,
          tz,
          tp,
          th,
          mx,
          my,
          mz,
          vm,
          vt,
          dt,
          m);
    if (ty1 != NULL) {
        *ty1 = ty1__;
    }
    if (tz1 != NULL) {
        *tz1 = tz1__;
    }
    if (mx1 != NULL) {
        *mx1 = mx1__;
    }
    if (my1 != NULL) {
        *my1 = my1__;
    }
    if (mz1 != NULL) {
        *mz1 = mz1__;
    }
    if (mp1 != NULL) {
        *mp1 = mp1__;
    }
    if (mh1 != NULL) {
        *mh1 = mh1__;
    }
    if (r != NULL) {
        *r = r__;
    }
    return tx1;
}

//
// The function "mlxDandao" contains the feval interface for the "dandao"
// M-function from file "d:\matlab6p5\work\dll2\dandao.m" (lines 1-36). The
// feval function calls the implementation version of dandao through this
// function. This function processes any input arguments and passes them to the
// implementation version of the function, appearing above.
//
void mlxDandao(int nlhs, mxArray * plhs[], int nrhs, mxArray * prhs[]) {
    MW_BEGIN_MLX();
    {
        mwArray mprhs[12];
        mwArray mplhs[9];
        int i;
        mclCppUndefineArrays(9, mplhs);
        if (nlhs > 9) {
            error(
              mwVarargin(
                mwArray(
                  "Run-time Error: File: dandao Line: 1 Column: 1"
                  " The function \"dandao\" was called with more "
                  "than the declared number of outputs (9).")));
        }
        if (nrhs > 12) {
            error(
              mwVarargin(
                mwArray(
                  "Run-time Error: File: dandao Line: 1 Column: 1"
                  " The function \"dandao\" was called with more "
                  "than the declared number of inputs (12).")));
        }
        for (i = 0; i < 12 && i < nrhs; ++i) {
            mprhs[i] = mwArray(prhs[i], 0);
        }
        for (; i < 12; ++i) {
            mprhs[i].MakeDIN();
        }
        mplhs[0]
          = Mdandao(
              &mplhs[1],
              &mplhs[2],
              &mplhs[3],
              &mplhs[4],
              &mplhs[5],
              &mplhs[6],
              &mplhs[7],
              &mplhs[8],
              nlhs,
              mprhs[0],
              mprhs[1],
              mprhs[2],
              mprhs[3],
              mprhs[4],
              mprhs[5],
              mprhs[6],
              mprhs[7],
              mprhs[8],
              mprhs[9],
              mprhs[10],
              mprhs[11]);
        plhs[0] = mplhs[0].FreezeData();
        for (i = 1; i < 9 && i < nlhs; ++i) {
            plhs[i] = mplhs[i].FreezeData();
        }
    }
    MW_END_MLX();
}

//
// The function "Mdandao" is the implementation version of the "dandao"
// M-function from file "d:\matlab6p5\work\dll2\dandao.m" (lines 1-36). It
// contains the actual compiled code for that M-function. It is a static
// function and must only be called from one of the interface functions,
// appearing below.
//
//
// function [tx1,ty1,tz1,mx1,my1,mz1,mp1,mh1,r]=dandao(tx,ty,tz,tp,th,mx,my,mz,vm,vt,dt,m)
//
static mwArray Mdandao(mwArray * ty1,
                       mwArray * tz1,
                       mwArray * mx1,
                       mwArray * my1,
                       mwArray * mz1,
                       mwArray * mp1,
                       mwArray * mh1,
                       mwArray * r,
                       int nargout_,
                       mwArray tx,
                       mwArray ty,
                       mwArray tz,
                       mwArray tp,
                       mwArray th,
                       mwArray mx,
                       mwArray my,
                       mwArray mz,
                       mwArray vm,
                       mwArray vt,
                       mwArray dt,
                       mwArray m) {
    mwLocalFunctionTable save_local_function_table_
      = &_local_function_table_dandao;
    mwArray tx1 = mwArray::UNDEFINED;
    mwArray dpsim = mwArray::UNDEFINED;
    mwArray dthetam = mwArray::UNDEFINED;
    mwArray dphicz = mwArray::UNDEFINED;
    mwArray dphisp = mwArray::UNDEFINED;
    mwArray phicz = mwArray::UNDEFINED;
    mwArray phisp = mwArray::UNDEFINED;
    mwArray mh = mwArray::UNDEFINED;
    mwArray mp = mwArray::UNDEFINED;
    mwArray psim = mwArray::UNDEFINED;
    mwArray thetam = mwArray::UNDEFINED;
    mwArray ft = mwArray::UNDEFINED;
    mwArray st = mwArray::UNDEFINED;
    mwArray ae = mwArray::UNDEFINED;
    //
    // 
    // ae=pi/180;
    //
    ae = _mxarray0_;
    //
    // st=(-1)*tp*ae;
    //
    st = _mxarray1_ * mwVa(tp, "tp") * mwVv(ae, "ae");
    //
    // ft=(-1)*th*ae;
    //
    ft = _mxarray1_ * mwVa(th, "th") * mwVv(ae, "ae");
    //
    // thetam=atan((tz-mz)./(tx-mx));
    //
    thetam
      = atan(
          rdivide(
            mwVa(tz, "tz") - mwVa(mz, "mz"), mwVa(tx, "tx") - mwVa(mx, "mx")));
    //
    // psim=atan((ty-my)./(tx-mx));
    //
    psim
      = atan(
          rdivide(
            mwVa(ty, "ty") - mwVa(my, "my"), mwVa(tx, "tx") - mwVa(mx, "mx")));
    //
    // mp=thetam;
    //
    mp = mwVv(thetam, "thetam");
    //
    // mh=psim;
    //
    mh = mwVv(psim, "psim");
    //
    // % ptr(:,k)=[ptr(1,k-1)+vt*dt*cos(st)*cos(ft);ptr(2,k-1)+vt*dt*cos(st)*sin(ft);ptr(3,k-1)+vt*dt*sin(st)];  
    // tx1=tx+vt*dt*cos(st)*cos(ft);
    //
    tx1
      = mwVa(tx, "tx")
        + mwVa(vt, "vt") * mwVa(dt, "dt") * cos(mwVv(st, "st"))
          * cos(mwVv(ft, "ft"));
    //
    // ty1=ty+vt*dt*cos(st)*sin(ft);
    //
    *ty1
      = mwVa(ty, "ty")
        + mwVa(vt, "vt") * mwVa(dt, "dt") * cos(mwVv(st, "st"))
          * sin(mwVv(ft, "ft"));
    //
    // tz1=tz++vt*dt*sin(st);
    //
    *tz1
      = mwVa(tz, "tz")
        + +(mwVa(vt, "vt")) * mwVa(dt, "dt") * sin(mwVv(st, "st"));
    //
    // %Mx(k)=pmr(1,k-1)+vm*dt*cos(thetam)*cos(psim);
    // %  Mz(k)=pmr(2,k-1)+vm*dt*cos(thetam)*sin(psim);
    // % My(k)=pmr(3,k-1)+vm*dt*sin(thetam);
    // mx1=mx+vm*dt*cos(thetam)*cos(psim);
    //
    *mx1
      = mwVa(mx, "mx")
        + mwVa(vm, "vm") * mwVa(dt, "dt") * cos(mwVv(thetam, "thetam"))
          * cos(mwVv(psim, "psim"));
    //
    // my1=my+vm*dt*cos(thetam)*sin(psim);
    //
    *my1
      = mwVa(my, "my")
        + mwVa(vm, "vm") * mwVa(dt, "dt") * cos(mwVv(thetam, "thetam"))
          * sin(mwVv(psim, "psim"));
    //
    // mz1=mz+vm*dt*sin(thetam);
    //
    *mz1
      = mwVa(mz, "mz")
        + mwVa(vm, "vm") * mwVa(dt, "dt") * sin(mwVv(thetam, "thetam"));
    //
    // %   pmr(:,k)=[Mx(k);Mz(k);My(k)];
    // %phisp=atan((ptr(3,k)-pmr(3,k))/(ptr(1,k)-pmr(1,k)));
    // % phicz=atan((ptr(2,k)-pmr(2,k))/(ptr(1,k)-pmr(1,k)));
    // phisp=atan((tz1-mz1)/(tx1-mx1));
    //
    phisp
      = atan(
          (mwVv(*tz1, "tz1") - mwVv(*mz1, "mz1"))
          / (mwVv(tx1, "tx1") - mwVv(*mx1, "mx1")));
    //
    // phicz=atan((ty1-my1)/(tx1-mx1));
    //
    phicz
      = atan(
          (mwVv(*ty1, "ty1") - mwVv(*my1, "my1"))
          / (mwVv(tx1, "tx1") - mwVv(*mx1, "mx1")));
    //
    // dphisp=(phisp-thetam)/dt;
    //
    dphisp = (mwVv(phisp, "phisp") - mwVv(thetam, "thetam")) / mwVa(dt, "dt");
    //
    // dphicz=(phicz-psim)/dt;   
    //
    dphicz = (mwVv(phicz, "phicz") - mwVv(psim, "psim")) / mwVa(dt, "dt");
    //
    // dthetam=m*dphisp;
    //
    dthetam = mwVa(m, "m") * mwVv(dphisp, "dphisp");
    //
    // dpsim=m*dphicz; 
    //
    dpsim = mwVa(m, "m") * mwVv(dphicz, "dphicz");
    //
    // thetam=thetam+dthetam*dt;
    //
    thetam = mwVv(thetam, "thetam") + mwVv(dthetam, "dthetam") * mwVa(dt, "dt");
    //
    // psim=psim+dpsim*dt;
    //
    psim = mwVv(psim, "psim") + mwVv(dpsim, "dpsim") * mwVa(dt, "dt");
    //
    // mp1=thetam;
    //
    *mp1 = mwVv(thetam, "thetam");
    //
    // mh1=psim;
    //
    *mh1 = mwVv(psim, "psim");
    //
    // r=sqrt((tx1-mx1)^2+(ty1-my1)^2+(tz1-mz1)^2);
    //
    *r
      = sqrt(
          (mwVv(tx1, "tx1") - mwVv(*mx1, "mx1") ^ _mxarray2_)
          + (mwVv(*ty1, "ty1") - mwVv(*my1, "my1") ^ _mxarray2_)
          + (mwVv(*tz1, "tz1") - mwVv(*mz1, "mz1") ^ _mxarray2_));
    mwValidateOutput(tx1, 1, nargout_, "tx1", "dandao");
    mwValidateOutput(*ty1, 2, nargout_, "ty1", "dandao");
    mwValidateOutput(*tz1, 3, nargout_, "tz1", "dandao");
    mwValidateOutput(*mx1, 4, nargout_, "mx1", "dandao");
    mwValidateOutput(*my1, 5, nargout_, "my1", "dandao");
    mwValidateOutput(*mz1, 6, nargout_, "mz1", "dandao");
    mwValidateOutput(*mp1, 7, nargout_, "mp1", "dandao");
    mwValidateOutput(*mh1, 8, nargout_, "mh1", "dandao");
    mwValidateOutput(*r, 9, nargout_, "r", "dandao");
    return tx1;
    //
    // 
    // %r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2);
    // 
    //
}