#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <vg.h>
#include <pf.h>

int my_motion_model(vgMotionCallbackStruct *mot_cb);

typedef struct
{
        float x, y, z;
        float h, p, r;
        float velocity;
        vgPosition *pos;
        double now;
} motion_model_data;

int
main(int argc, char **argv)
{
        vgObserver      *obs;
        vgMotion        *mot;
        vgIDev          *idev;

        motion_model_data *mot_instance_data;

        if (argc < 2)
        {
                printf("syntax: %s <config file>", argv[0]);
                exit(-1);
        }

        vgInitSys();
        vgDefineSys(argv[1]);
        vgConfigSys();

        obs = vgGetObserv(0);
        mot = vgGetObservMot(obs);

        if (!mot)
        {
                /* 创建新的运动模型(motion model)并依附到观察者(observer) */
                mot = vgNewMot();
                vgName(mot, "user defined motion model");

                vgObservMot(obs, mot);

                /* 告诉观察者(observer)使用依附于其上的运动模型(motion model) */
                vgProp(obs, VGOBS_TETHERSTATE, VGOBS_DYNAMIC);
        }

        /* 创建新的输入设备实例并依附到运动模型(motion model)上 */
        idev = vgNewIDev();
        vgName(idev, "KBMouse");
        vgIDevOpen(idev);
        vgMotIDev(mot, idev);

        /* 定义运动模型的回调 */
        vgMotRegister(VGMOT_USER1, my_motion_model);

        /* 为回调数据分配内存 */
        mot_instance_data = pfCalloc(1, sizeof(motion_model_data),
                        vgGetSharedArena());

        /* 为运动模型实例初始化回调数据 */
        vgUserData(mot, mot_instance_data);

        /* 设置运动模型属性 */
        vgProp(mot, VGMOT_MODEL, VGMOT_USER1);

        while (1)
        {
                vgSyncFrame();
                vgFrame();
        }
}


int
my_motion_model(vgMotionCallbackStruct *mot_cb)
{

        vgMotion *mot;
        vgIDev *input_device;
        static vgPosition *device_pos = NULL;
        float dt;
        float sine, cosine;
        int toggles[3];
        int middlebutton, leftbutton, rightbutton;
        float devicex, devicey, devicez, deviceh, devicep, devicer;
        bool bChange;

        motion_model_data *data;


        mot = mot_cb->mot;
        data = (motion_model_data *)mot_cb->user_data;
        input_device = vgGetMotIDev(mot);

        if (!data) {
                printf("motion model callback data not available");
                return(-1);
        }

        if (!device_pos)
                device_pos = vgNewPos();

        switch(mot_cb->reason)
        {
        case VGMOT_INIT_EVENT:
                /* 初始化运动模型数据 */

                if (!data->pos)
                        data->pos = vgNewPos();

                vgGetPos(mot, data->pos);
                vgGetPosVec(data->pos, &data->x, &data->y, &data->z,
                            &data->h, &data->p, &data->r);

                data->x = 0.0f;
                data->y = 0.0f;
                data->z = 0.0f;
                data->h = 0.0f;
                data->p = 0.0f;
                data->r = 0.0f;

                data->velocity = 100.0f;

                data->now = mot_cb->data.now;
                break;

        case VGMOT_RESET_EVENT:
                /* 运动模型重置事件处理代码 */

                vgGetPos(mot, data->pos);
                vgGetPosVec(data->pos, &data->x, &data->y, &data->z,
                            &data->h, &data->p, &data->r);

                data->p = 0.0f;
                data->r = 0.0f;
                data->now = mot_cb->data.now;
                break;

        case VGMOT_UPDATE_EVENT:
                vgGetPos(input_device, device_pos);
                vgGetPosVec(device_pos, &devicex, &devicey, &devicez,
                            &deviceh, &devicep, &devicer);

                vgIDevReadToggleData(input_device, toggles, 3);
                leftbutton = toggles[0];
                middlebutton = toggles[1];
                rightbutton = toggles[2];

                dt  = mot_cb->data.now - data->now;
                data->now = mot_cb->data.now;

                key = vgGetWinKey(vgGetWin(0));
                bChanged = false;
                devicey = 0;

                if (leftbutton)
                {
                        data->h -= 30.0f * devicex * dt;

                        if ((data->h < -360.0f) || (data->h > 360.0f))
                                data->h = fmod(data->h, 360.0f);

                        vgGetSinCos(data->h, &sine, &cosine);

                        data->x -= data->velocity * sine * devicey * dt;
                        data->y += data->velocity * cosine * devicey * dt;

                        vgPosVec(data->pos, data->x, data->y, data->z,
                                 data->h, data->p, data->r);
                }

                if (key == VGWIN_UPARROW)
                {
                        devicey = m_nDeviceY;
                        bChanged = true;
                }
                if (key == VGWIN_DOWNARROW)
                {
                        devicey = -m_nDeviceY;
                        bChanged = true;
                }
                if (key == VGWIN_LEFTARROW)
                        data->h += m_nDeviceH;
                if (key == VGWIN_RIGHTARROW)
                        data->h -= m_nDeviceH;

                if (bChanged)
                {
                        if ((data->h < -360.0f) || (data->h > 360.0f))
                                data->h = fmod(data->h, 360.0f);

                        vgGetSinCos(data->h, &sine, &cosine);

                        data->x -= data->velocity * sine * devicey;
                        data->y += data->velocity * cosine * devicey;
                }

                /* 如果不需要碰撞检测的话可以去掉下面这段代码 */
                if (vgGetIsect(0) != NULL)
                {
                        int     status;
                        float   result[3];

                        vgPos(vgGetIsect(0), data->pos);

                        vgUpdate(vgGetIsect(0));

                        status = vgGetIsectResult(vgGetIsect(0), VGIS_GETTRIPOD, result);

                        if (status != 3)
                        {
                                data->x += data->velocity * sine * devicey;
                                data->y -= data->velocity * cosine * devicey;
                                vgPosVec(data->pos, data->x, data->y, data->z,
                                         data->h, data->p, data->r);
                        }
                }

                break;

        case VGMOT_EXIT_EVENT:
                /* 清除运动模型数据 */

                vgDelPos(data->pos);
                data->pos = NULL;

                break;

        default:
                vgNotify(VG_WARN, VG_APP, "My Motion Model: Invalid Callback event",
                         mot_cb->reason );
                break;
        }

        return(0);
}