#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <semaphore.h>
#include <string.h>
#include <pthread.h>
#include <signal.h>
#include <errno.h>
#include <time.h>
#include <assert.h>
#include <math.h>

#include "probe/probe.h"
#include "ptz/ptz.h"
#include "proto_comm.h"
#include "proto_dbg.h"

static PtzDevs *gPtzDevs = NULL;

/*get the image wideth*/
static int img_w_get(PtzDevs *DevData) {
  
  int ret; 

  SOAP_ASSERT(DevData != NULL);
  SOAP_ASSERT(DevData->profiles != NULL);

  ret = DevData->profiles->venc.Width;

  return ret;
}

/*get the image height*/
static int img_h_get(PtzDevs *DevData) {

  int ret;

  SOAP_ASSERT(DevData != NULL);
  SOAP_ASSERT(DevData->profiles != NULL);

  ret = DevData->profiles->venc.Height;

  return ret;
}

static void cb_probe(char *DeviceXAddr) {

  int len;
  char *pos_s, *pos_e;

  if (gPtzDevs == NULL) {

    gPtzDevs = malloc(sizeof(PtzDevs));

    SOAP_ASSERT(gPtzDevs != NULL);

    memset(gPtzDevs, 0x00, sizeof(PtzDevs));

    pos_s = strstr(DeviceXAddr, ADDR_PREFIX);
    pos_e = strstr(DeviceXAddr + strlen(ADDR_PREFIX), ADDR_LAST);
    len = pos_e - DeviceXAddr - strlen(ADDR_PREFIX);
    memcpy(gPtzDevs->ip, DeviceXAddr + strlen(ADDR_PREFIX), len);
  }

  proto_GetCapabilities(DeviceXAddr, &(gPtzDevs->capa), USERNAME, PASSWORD);
  proto_GetProfiles(gPtzDevs->capa.MediaXAddr, &(gPtzDevs->profiles), USERNAME, PASSWORD);

}

/*preset tour task*/
static void *task_preset_touring(void *arg) {
  int ret;
  int i;
  float data;
  PosCur pos_get;
  float pval, tval;
  struct timespec time_exp;

  time_exp.tv_sec =  WT_TIME;
  time_exp.tv_nsec = 0;
  //int data = *(int *)arg;

  pthread_detach(pthread_self());
  if (gPtzDevs != NULL) {
    ret = sem_init(&(gPtzDevs->sem_tour), 0, 0);
    SOAP_ASSERT(ret == 0);

    timer_init(&(gPtzDevs->timer_id), ptz_timer_handler);

    gPtzDevs->dev_stat = DISABLE;
    pthread_mutex_init(&(gPtzDevs->node_mutex), NULL);
    pthread_cond_init(&(gPtzDevs->cond), NULL);

    while(1) {
      ret = sem_timedwait(&(gPtzDevs->sem_tour), &time_exp);
      if (gPtzDevs->proc_stat == DISABLE) {
        printf("now task_preset_touring is exitting!\n");

        pthread_cond_destroy(&(gPtzDevs->cond));
        pthread_mutex_destroy(&(gPtzDevs->node_mutex));
        sem_destroy(&(gPtzDevs->sem_tour));

        timer_destroy(gPtzDevs->timer_id);

        free(gPtzDevs->profiles);

        free(gPtzDevs);

        gPtzDevs = NULL;

        break;

      } else if (ret == 0) {

        if (gPtzDevs->dev_stat == ENABLE) {

          if (gPtzDevs->tour_type == TOUR_PRIV) {
            printf("now the priv touring is calling:\n");

            if (gPtzDevs->posNode != NULL) {

              while(gPtzDevs->dev_stat == ENABLE) {

                for (i = 0; i < gPtzDevs->posNode_count; i++) {

                  proto_PTZ_GetStatus(gPtzDevs, &pos_get, USERNAME, PASSWORD);
            
                  proto_PTZGetPT(gPtzDevs, (gPtzDevs->posNode + i)->x, (gPtzDevs->posNode + i)->y, &pval, &tval, USERNAME, PASSWORD);
                  
                  proto_PTZSet(gPtzDevs, pval, tval, pos_get.z, USERNAME, PASSWORD);
                  
                  proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);
                  
                  data = proto_PTZZoom_get(gPtzDevs, (gPtzDevs->posNode + i)->pos0_x, (gPtzDevs->posNode + i)->pos0_y, \
                                  (gPtzDevs->posNode + i)->pos1_x, (gPtzDevs->posNode + i)->pos1_y);
                  
                  proto_PTZSet(gPtzDevs, 0, 0, data, USERNAME, PASSWORD);
                 
                  proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);

                  timer_start(gPtzDevs->timer_id, (gPtzDevs->posNode + i)->time_sec);

                  pthread_mutex_lock(&(gPtzDevs->node_mutex));
                  while((gPtzDevs->dev_stat == ENABLE) && (gPtzDevs->wt_stat == ENABLE)) {
                    pthread_cond_wait(&(gPtzDevs->cond), &(gPtzDevs->node_mutex));
                  }
                  pthread_mutex_unlock(&(gPtzDevs->node_mutex));

                  proto_PTZSet(gPtzDevs, 0, 0, 1, USERNAME, PASSWORD);
                  proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);

                  gPtzDevs->wt_stat = ENABLE;

                  if (gPtzDevs->dev_stat == DISABLE) {
				  
                    break;
                  }
                }
              }

              timer_stop(gPtzDevs->timer_id);

            }

          } else {
          
            if (gPtzDevs->postradNode != NULL) {

              while(gPtzDevs->dev_stat == ENABLE) {

                for (i = 0; i < gPtzDevs->postradNode_count; i++) {
                  
                  proto_PTZPreset(gPtzDevs, NULL, PRESET_GOTO, &((gPtzDevs->postradNode + i)->posToken), USERNAME, PASSWORD);
        
                  timer_start(gPtzDevs->timer_id, (gPtzDevs->postradNode + i)->time_sec);

                  pthread_mutex_lock(&(gPtzDevs->node_mutex));
                  while((gPtzDevs->dev_stat == ENABLE) && (gPtzDevs->wt_stat == ENABLE)) {
                    pthread_cond_wait(&(gPtzDevs->cond), &(gPtzDevs->node_mutex));
                  }
                  pthread_mutex_unlock(&(gPtzDevs->node_mutex));

                  gPtzDevs->wt_stat = ENABLE;

                  if (gPtzDevs->dev_stat == DISABLE) {

                    printf("the comm type tour stop is triggered, now stop it!\n");

                    break;
                  }
                }
              }

              timer_stop(gPtzDevs->timer_id);
            }
          }
        }
      }
    }
  }

  return NULL;
}

/*the timer process heandler*/
void ptz_timer_handler(union sigval para) {

  if (gPtzDevs != NULL) {

    pthread_mutex_lock(&(gPtzDevs->node_mutex));

    gPtzDevs->wt_stat = DISABLE;

    pthread_cond_signal(&(gPtzDevs->cond));

    pthread_mutex_unlock(&(gPtzDevs->node_mutex));
  }

  return;
}

/*initialize the PTZ*/
int proto_PTZInit(void) {
 
  int ret;
  int flag;
  pthread_t tid;

  ret = DiscoverDevice(cb_probe);
  if (ret <= 0) {
    return -1;
  }

  ret = pthread_create(&tid, NULL, task_preset_touring, &flag);
  if (ret) {
    return -1;
  }

  return 0;
}

/*release all the resources when exitted*/
void proto_PTZClose(void) {
 
  if (gPtzDevs != NULL) {

    if (gPtzDevs->dev_stat == ENABLE) {
      proto_PTZPreset_tour_stop(gPtzDevs);
    }

    gPtzDevs->proc_stat = DISABLE;
  }

}

PtzDevs *proto_PTZGethandle(void) {
  return gPtzDevs;
}

/*wait for PTZ stopped*/
void proto_PTZWaitStopped(PtzDevs *DevData, const char *username, const char *passwd) {
  PosCur pos_get;

  proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
  while(pos_get.stat == RUNNING) {
    proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
    sleep(1); 
  }

  sleep(STOP_WT_TIME);
}

/*
 *(x0, y0): the left-up pixel;
 *(x1, y1): the right-down pixel;
 *
 *(x0, y0) and (x1, y1) must be aligned tightly with the center of the image.
*/
float proto_PTZZoom_get(PtzDevs *DevData, int x0, int y0, int x1, int y1) {
  int w;
  int h;
  float data0;
  float data1;
  float temp;
  float temp_w;
  float temp_h;
  float result;

  w = img_w_get(DevData);
  h = img_h_get(DevData);

  data0 = w * h * MAX_IMG_SCALE;
  data1 = abs(x1 - x0) * abs(y1 - y0);

  temp = sqrtf(data0 / data1);
  temp_w = (w / 2) / (abs(x1 - w / 2));
  temp_h = (h / 2) / (abs(y1 - h / 2));

  result = min(temp_w, temp_h);
  result = min(result, temp);

  if (result <= 1) {
    result = 1;
  } 

  return result;
}

int proto_PTZ_GetStatus(PtzDevs *DevData, PosCur *pos_get, const char *username, const char *passwd) {
  int result = 0;
  struct soap *soap = NULL;
  char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
  struct _tptz__GetStatus           *getStatus = NULL;
  struct _tptz__GetStatusResponse   *getStatusResponse = NULL;

  SOAP_ASSERT(DevData != NULL);
  SOAP_ASSERT(NULL != DevData->profiles);
  SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));

  getStatus = soap_new__tptz__GetStatus(soap, 1);
  getStatusResponse = soap_new__tptz__GetStatusResponse(soap, 1);

  strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
  proto_SetAuthInfo(soap, username, passwd);

  getStatus->ProfileToken = DevData->profiles->token;
  result = soap_call___tptz__GetStatus(soap, PTZXAddr, NULL, getStatus, getStatusResponse);
  SOAP_CHECK_ERROR(result, soap, "proto_PTZ_GetStatus");

EXIT:

  if ((SOAP_OK != result) || (SOAP_OK != soap->error)) {
    
    if (NULL != soap) {
      proto_soap_delete(soap);
    }

    return -1;
  }

  if (pos_get != NULL) {

    pos_get->p = pos_calc_p(getStatusResponse->PTZStatus->Position->PanTilt->x);
    pos_get->t = pos_calc_t(getStatusResponse->PTZStatus->Position->PanTilt->y);
    pos_get->z = pos_calc_z(getStatusResponse->PTZStatus->Position->Zoom->x);

    if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__IDLE) {

      pos_get->stat = IDLE;

    } else if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__MOVING) {
      
      pos_get->stat = RUNNING;

    } else if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__UNKNOWN){
      
      pos_get->stat = UNKNOWN;

    }
  }

  if (NULL != soap) {
      proto_soap_delete(soap);
  }

  return 0;
}

static float pos_calc_p(float pos) {
  float data = COEF_H_A * pos + COEF_H_B;

  return data;
}

static float pos_calc_t(float pos) {
  float data = COEF_V_A * pos + COEF_V_B;

  return data;
}

static float pos_calc_z(float pos) {
  float data = COEF_Z_A * pos + COEF_Z_B;

  return data;
}

static float pos_res_p(float pos) {
  int pos_temp;
  float data;

  pos_temp = (int)pos % MAX_VAL;
  data = (float) (pos_temp - COEF_H_B) * 1.0 / COEF_H_A;
  
  data = para_check(data);
  return data;
}

static float pos_res_t(float pos) {
  int pos_temp;
  float data;

  pos_temp = (int)pos % MAX_VAL;
  data = (float)(pos_temp - COEF_V_B) * 1.0 / COEF_V_A;

  data = para_check(data);
  return data;
}

static float pos_res_z(float pos) {
  float data = (float)(abs(pos) - COEF_Z_B) * 1.0 / COEF_Z_A;

  data = para_check(data);
  if (data <= 0) {
    data *= -1;
  }
  return data;
}

/*set the self-defined tour parameters table*/
int proto_PTZPreset_tour_set(PtzDevs *DevData, PresetToure_node *PosArr, int n) {

  int i;

  if ((PosArr == NULL) || (n < MIN_TOUR_POS) || (DevData == NULL)) {
    
    SOAP_DBGERR("invalid parameters!\n");

    return -1;
  }

  if (DevData->dev_stat == ENABLE) {
    SOAP_DBGERR("the tour is in progress, the paras can not been set in this stat!\n");

    return -1;
  }

  pthread_mutex_lock(&(DevData->node_mutex));

  if (DevData->posNode != NULL) {
    free(DevData->posNode);
  }

  DevData->posNode = (PresetToure_node *)malloc(sizeof(PresetToure_node) * n);
  if (DevData->posNode == NULL) {
    SOAP_DBGERR("in %s: out of memory!\n", __FUNCTION__);

    exit(1);
  }
  DevData->posNode_count = n;

  memcpy(DevData->posNode, PosArr, sizeof(PresetToure_node) * n);

  for (i = 0; i < n; i++) {
    if ((DevData->posNode + i)->time_sec < MIN_TOUR_TIME) {

      (DevData->posNode + i)->time_sec = MIN_TOUR_TIME;
    
    }
  }

  pthread_mutex_unlock(&(DevData->node_mutex));

  return 0;
}

/*start the tour*/
int proto_PTZPreset_tour_start(PtzDevs *DevData, enum tourType type) {

  if (DevData->dev_stat == ENABLE) {
    SOAP_DBGERR("the tour is in progress now, it can not been re-started yet!\n");

    return -1;
  }

  if (type == TOUR_PRIV) {
    if ((DevData == NULL) || (DevData->posNode == NULL)) {
      SOAP_DBGERR("invalid parameters! configure the paras firstly please!\n");

      return -1;
    }
  } else {
    if ((DevData == NULL) || (DevData->postradNode == NULL)) {
      SOAP_DBGERR("invalid parameters! configure the paras firstly please!\n");

      return -1;
    }
  }

  pthread_mutex_lock(&(DevData->node_mutex));

  DevData->dev_stat = ENABLE;
  DevData->tour_type = type;
  sem_post(&(DevData->sem_tour));

  pthread_mutex_unlock(&(DevData->node_mutex));

  return 0;
}

/*stop the tour*/
void proto_PTZPreset_tour_stop(PtzDevs *DevData) {

  if ((DevData != NULL) && (DevData->dev_stat == ENABLE)) {

    pthread_mutex_lock(&(DevData->node_mutex));

    DevData->dev_stat = DISABLE;

    pthread_cond_signal(&(DevData->cond));

    pthread_mutex_unlock(&(DevData->node_mutex));
  }
}

/*set the tradditional tour parameters table such as the one from hikon*/
int proto_PTZPreset_tradtour_set(PtzDevs *DevData, PresetToure_tradnode *PosArr, int n) {

  int i;

  if ((PosArr == NULL) || (n < 2) || (DevData == NULL)) {

    SOAP_DBGERR("invalid parameters!\n");

    return -1;
  }

  if (DevData->dev_stat == ENABLE) {
    SOAP_DBGERR("the tour is in progress, the paras can not been set in this stat!\n");

    return -1;
  }

  pthread_mutex_lock(&(DevData->node_mutex));

  if (DevData->postradNode != NULL) {
    free(DevData->postradNode);
  }

  DevData->postradNode = (PresetToure_tradnode *)malloc(sizeof(PresetToure_tradnode) * n);
  if (DevData->postradNode == NULL) {
    SOAP_DBGERR("in %s: out of memory!\n", __FUNCTION__);

    exit(1);
  }
  DevData->postradNode_count = n;

  memcpy(DevData->postradNode, PosArr, sizeof(PresetToure_tradnode) * n);

  for (i = 0; i < n; i++) {
    if ((DevData->postradNode + i)->time_sec < MIN_TOUR_TIME) {
      (DevData->postradNode + i)->time_sec = MIN_TOUR_TIME;
    }  
  }

  pthread_mutex_unlock(&(DevData->node_mutex));

  return 0;
}

/*pos_p: 
 *   the angle that will be rotated in pan
 *pos_t:
 *    the angle that will be rotated in tilt
 *pos_z:
 *    the zoom value that will be zoomed
 */
int proto_PTZSet(PtzDevs *DevData, float pos_p, float pos_t, float pos_z,  \
                        const char *username, const char *passwd) {
    int result = 0;
    float data_p, data_t, data_z;
    struct soap *soap = NULL;
    PosCur pos_get;
    char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };

    SOAP_ASSERT(DevData != NULL);
    SOAP_ASSERT(NULL != DevData->profiles);
    SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));

    proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);

    data_p = pos_res_p(pos_get.p - pos_p);
    data_t = pos_res_t(pos_get.t - pos_t);
    data_z = pos_res_z(pos_z);

    struct _tptz__AbsoluteMove           *absMove = soap_new__tptz__AbsoluteMove(soap, 1);
    struct _tptz__AbsoluteMoveResponse   *absMoveResponse = soap_new__tptz__AbsoluteMoveResponse(soap, 1);

    strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
    proto_SetAuthInfo(soap, username, passwd);
    absMove->ProfileToken = DevData->profiles->token;

    absMove->Position = soap_new_tt__PTZVector(soap, 1);
    absMove->Position->PanTilt = soap_new_tt__Vector2D(soap, 1);
    absMove->Position->Zoom = soap_new_tt__Vector1D(soap, 1);
    absMove->Speed = soap_new_tt__PTZSpeed(soap, 1);
    absMove->Speed->PanTilt = soap_new_tt__Vector2D(soap, 1);
    absMove->Speed->Zoom = soap_new_tt__Vector1D(soap, 1);

    absMove->Position->PanTilt->x = data_p;
    absMove->Position->PanTilt->y = data_t;
    absMove->Position->Zoom->x = data_z;

    result = soap_call___tptz__AbsoluteMove(soap, PTZXAddr, NULL, absMove, absMoveResponse);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZAbsoluteMove");

EXIT:
    if (NULL != soap) {
        proto_soap_delete(soap);
    }
    return 0;
}

/*stop the PTZ*/
int proto_PTZStop(PtzDevs *DevData, const char *username, const char *passwd) {
    int result = 0;
    char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
    struct soap *soap = NULL;
    enum xsd__boolean xsd_true = xsd__boolean__true_;
    enum xsd__boolean xsd_false = xsd__boolean__false_;
  
    SOAP_ASSERT(DevData != NULL);
    SOAP_ASSERT(NULL != DevData->profiles);
    SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));

    struct _tptz__Stop *tptzStop = soap_new__tptz__Stop(soap, 1);
    struct _tptz__StopResponse *tptzStopResponse = soap_new__tptz__StopResponse(soap, 1);

    strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
    proto_SetAuthInfo(soap, username, passwd);
    tptzStop->ProfileToken = DevData->profiles->token;

    tptzStop->PanTilt = &xsd_true;
    tptzStop->Zoom = &xsd_false;
    result = soap_call___tptz__Stop(soap, PTZXAddr, NULL, tptzStop, tptzStopResponse);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZStopMove");

  EXIT:
    if (NULL != soap) {
        proto_soap_delete(soap);
    }
    return result;
}

/*
* speed_p && speed_t && speed_z: 0 - MAX_ZOOM
*/
int proto_PTZControl(PtzDevs *DevData, enum PTZCMD cmd, float speed_p, float speed_t, float speed_z,
                        const char *username, const char *passwd) {
    int result = 0;
    float data_p, data_t, data_z;
    struct soap *soap = NULL;
    char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };

    SOAP_ASSERT(DevData != NULL);
    SOAP_ASSERT(NULL != DevData->profiles);
    SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
    
    struct _tptz__ContinuousMove* contMove = soap_new__tptz__ContinuousMove(soap, 1);
    struct _tptz__ContinuousMoveResponse* contMoveResponse = soap_new__tptz__ContinuousMoveResponse(soap, 1);
   
    data_p = pos_res_z(speed_p);
    data_t = pos_res_z(speed_t);
    data_z = pos_res_z(speed_z);

    strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
    proto_SetAuthInfo(soap, username, passwd);
    contMove->ProfileToken = DevData->profiles->token;
    contMove->Velocity = soap_new_tt__PTZSpeed(soap, 1);
    memset(contMove->Velocity, 0x00, sizeof(struct tt__PTZSpeed));
    contMove->Velocity->PanTilt = soap_new_tt__Vector2D(soap, 1);
    memset(contMove->Velocity->PanTilt, 0x00, sizeof(struct tt__Vector2D));
    contMove->Velocity->Zoom = soap_new_tt__Vector1D(soap, 1);
    memset(contMove->Velocity->Zoom, 0x00, sizeof(struct tt__Vector1D));

    switch (cmd)
    {
        case  PTZ_CMD_LEFT:
            contMove->Velocity->PanTilt->x = data_p * (-1);

            break;

        case  PTZ_CMD_RIGHT:
            contMove->Velocity->PanTilt->x = data_p;

            break;

        case  PTZ_CMD_UP:
            contMove->Velocity->PanTilt->y = data_t;

            break;

        case  PTZ_CMD_DOWN:
            contMove->Velocity->PanTilt->y = data_t * (-1);

            break;

        case  PTZ_CMD_LEFTUP:
            contMove->Velocity->PanTilt->x = data_p * (-1);
            contMove->Velocity->PanTilt->y = data_t;

            break;

        case PTZ_CMD_LEFTDOWN:
            contMove->Velocity->PanTilt->x = data_p * (-1);
            contMove->Velocity->PanTilt->y = data_t * (-1);

            break;

        case  PTZ_CMD_RIGHTUP:
            contMove->Velocity->PanTilt->x = data_p;
            contMove->Velocity->PanTilt->y = data_t;

            break;

        case PTZ_CMD_RIGHTDOWN:
            contMove->Velocity->PanTilt->x = data_p;
            contMove->Velocity->PanTilt->y = data_t * (-1);

            break;

        case  PTZ_CMD_ZOOM_IN:
            contMove->Velocity->Zoom->x = data_z;

            break;

        case  PTZ_CMD_ZOOM_OUT:
            contMove->Velocity->Zoom->x = data_z * (-1);

            break;

        default:
            break;
    }

    result = soap_call___tptz__ContinuousMove(soap, PTZXAddr, NULL, contMove, contMoveResponse);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZContinousMove");

 EXIT:
    if (NULL != soap) {
        proto_soap_delete(soap);
    }
    return result;
}

 __attribute__((unused)) static float ptz_fovh_get(float pos) {
  
  float data = COEF_FH_B / (COEF_FH_C + pos);

  float ret = expf(data) * COEF_FH_A;

  return ret;
}

 __attribute__((unused)) static float ptz_fovv_get(float pos) {
  
  float data = COEF_FV_B / (COEF_FV_C + pos);

  float ret = expf(data) * COEF_FV_A;

  return ret;
}

int proto_PTZGetPT(PtzDevs *DevData, int x, int y, float *p, float *t, const char *username, const char *passwd) {
  float fovx, fovy;
  float lx, ly, lz;
  float tx, ty, tz;
  PosCur pos_get;
  float pan;
  float tilt;
  int ret;
  int w, h;
  
  ret = proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
  if (ret != 0) {
    return ret;
  }

#if 0
  float temp;

  temp = pos_res_z(pos_get.z);
  fovx = ptz_fovh_get(temp);
  fovy = ptz_fovv_get(temp);
#else 
  fovx = MAX_FOVH; 
  fovy = MAX_FOVV;
#endif 

  pan = 0.0;
  tilt = 0.0;

  w = img_w_get(DevData);
  h = img_h_get(DevData);

  lx = (2 * x / w - 1) * tanf(ANG2RAD(fovx / 2));
  ly = (2 * y / h - 1) * tanf(ANG2RAD(fovy / 2));
  lz = 1;

  tx = cosf(pan) * cosf(tilt) * lx - cosf(tilt) * sinf(pan) * ly - sinf(tilt) * lz;
  ty = sinf(pan) * lx + cosf(pan) * ly;
  tz = cosf(pan) * sinf(tilt) * lx - sinf(pan) * sinf(tilt) * ly + cosf(tilt) * lz;

  pan = atan2f(tx, tz);
  tilt = atan2f(ty, tz);

  if (p != NULL) {
    *p = RAD2ANG(pan);
  }

  if (t != NULL) {
    *t = RAD2ANG(tilt);
  }

  return 0;
}

/*Set the position based on the current pos*/
int proto_PTZSetStep(PtzDevs *DevData, enum PTZCMD cmd, float pos_p, float pos_t, float pos_z, \
                        const char *username, const char *passwd) {
    int result = 0;
    struct soap *soap = NULL;
    char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };

    SOAP_ASSERT(DevData != NULL);
    SOAP_ASSERT(NULL != DevData->profiles);
    SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));

    pos_p = para_check(pos_p);
    pos_t = para_check(pos_t);
    pos_z = para_check(pos_z);

    struct _tptz__RelativeMove *relMove = soap_new__tptz__RelativeMove(soap, 1);
    struct _tptz__RelativeMoveResponse *relMoveResponse = soap_new__tptz__RelativeMoveResponse(soap, 1);

    strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
    proto_SetAuthInfo(soap, username, passwd);
    relMove->ProfileToken = DevData->profiles->token;
    relMove->Translation = (struct tt__PTZVector *)soap_malloc(soap, sizeof(struct tt__PTZVector));
    memset(relMove->Translation, 0x00, sizeof(struct tt__PTZVector));
    relMove->Translation->PanTilt = soap_new_tt__Vector2D(soap, 1);
    relMove->Translation->Zoom = soap_new_tt__Vector1D(soap, 1);

    memset(relMove->Translation->PanTilt, 0x00, sizeof(struct tt__Vector2D));
    memset(relMove->Translation->Zoom, 0x00, sizeof(struct tt__Vector1D));

    switch (cmd) {
        case  PTZ_CMD_LEFT:
            relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);

            break;

        case  PTZ_CMD_RIGHT:
            relMove->Translation->PanTilt->x = fabsf(pos_p);

            break;
        case  PTZ_CMD_UP:
            relMove->Translation->PanTilt->y = fabsf(pos_t);

            break;

        case  PTZ_CMD_DOWN:
            relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);

            break;

        case  PTZ_CMD_LEFTUP:
            relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);
            relMove->Translation->PanTilt->y = fabsf(pos_t);

            break;

        case PTZ_CMD_LEFTDOWN:
            relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);
            relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);

            break;
        case  PTZ_CMD_RIGHTUP:
            relMove->Translation->PanTilt->x = fabsf(pos_p);
            relMove->Translation->PanTilt->y = fabsf(pos_t);

            break;

        case PTZ_CMD_RIGHTDOWN:
            relMove->Translation->PanTilt->x = fabsf(pos_p);
            relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);

            break;

        case  PTZ_CMD_ZOOM_IN:
            relMove->Translation->Zoom->x = fabsf(pos_z);

            break;

        case  PTZ_CMD_ZOOM_OUT:
            relMove->Translation->Zoom->x = fabsf(pos_z) * (-1);

            break;

        default:
            break;
    }

    result = soap_call___tptz__RelativeMove(soap, PTZXAddr, NULL, relMove, relMoveResponse);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZRelativeMove");

 EXIT:
    if (NULL != soap) {
        proto_soap_delete(soap);
    }
    
    return result;
}

/*set the focus-on functionality*/
int proto_PTZ_ImagingSet(PtzDevs *DevData, enum PTZCMD cmd, float speed, const char *username, const char *passwd) {
  int result = 0;
  float speed_val;
  struct soap *soap = NULL;
  struct tt__FocusMove  *stFocusMove = NULL;
  struct tt__ContinuousFocus *stContFocus = NULL;
  struct _timg__Move *stMoveReq = NULL;
  struct _timg__MoveResponse *stMoveRes = NULL;
  struct _timg__Stop *stStopReq = NULL;
  struct _timg__StopResponse *stStopRes = NULL;

  SOAP_ASSERT(NULL != DevData);
  SOAP_ASSERT(NULL != DevData->profiles);
  SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
  
  stFocusMove = soap_new_tt__FocusMove(soap, 1);
  stContFocus = soap_new_tt__ContinuousFocus(soap, 1);
 
  stMoveReq = soap_new__timg__Move(soap, 1);
  stMoveRes = soap_new__timg__MoveResponse(soap, 1);

  stStopReq = soap_new__timg__Stop(soap, 1);
  stStopRes = soap_new__timg__StopResponse(soap, 1);
  
  memset(stMoveReq, 0x00, sizeof(struct _timg__Move));
  memset(stMoveRes, 0x00, sizeof(struct _timg__MoveResponse));
  
  memset(stStopReq, 0x00, sizeof(struct _timg__Stop));
  memset(stStopRes, 0x00, sizeof(struct _timg__StopResponse));

  stMoveReq->Focus = stFocusMove;
  stMoveReq->Focus->Continuous = stContFocus;

  stMoveReq->VideoSourceToken = DevData->profiles->videoCfg.sourceToken;
  stStopReq->VideoSourceToken = DevData->profiles->videoCfg.sourceToken;

  proto_SetAuthInfo(soap, username, passwd);

  speed_val = pos_res_z(speed);
  switch(cmd) {
    case PTZ_CMD_FOCUS_IN:
      stContFocus->Speed = speed_val;    
      
      break;

    case PTZ_CMD_FOCUS_OUT:
      stContFocus->Speed = speed_val * (-1);
      
      break;

    default:
      result = soap_call___timg__Stop(soap, DevData->capa.MediaXAddr, NULL, stStopReq, stStopRes);
      SOAP_CHECK_ERROR(result, soap, "proto_PTZ_ImagingSet");
      
      break;
  }

  stFocusMove->Continuous = stContFocus;
  stFocusMove->Absolute = NULL;
  stFocusMove->Relative = NULL;

  result = soap_call___timg__Move(soap, DevData->capa.MediaXAddr, NULL, stMoveReq, stMoveRes);
  SOAP_CHECK_ERROR(result, soap, "proto_PTZ_ImagingSet");
  
EXIT:
    if (NULL != soap) {
        proto_soap_delete(soap);
    }   

    return 0;
}

/*Set the thradditional preset pos such as the one from hikon*/
int proto_PTZPreset(PtzDevs *DevData, const char *posName, enum PreSetCMD cmd, \
                      int *posToken, const char *username, const char *passwd) {
  int result;
  char buf[100] = { 0x00 };
  struct soap *soap = NULL;
  char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };

  SOAP_ASSERT(DevData != NULL);
  SOAP_ASSERT(NULL != DevData->profiles);
  SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));

  strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
  proto_SetAuthInfo(soap, username, passwd);

  if (cmd == PRESET_SET) {
    struct _tptz__SetPreset *setPreset = soap_new__tptz__SetPreset(soap, 1);
    struct _tptz__SetPresetResponse *setPresetRes = soap_new__tptz__SetPresetResponse(soap, 1);

    memset(setPreset, 0x00, sizeof(struct _tptz__SetPreset));
    memset(setPresetRes, 0x00, sizeof(struct _tptz__SetPresetResponse));

    setPreset->ProfileToken = DevData->profiles->token;
    if (posName != NULL) {
      setPreset->PresetName = (char *)posName;
    }

    result = soap_call___tptz__SetPreset(soap, PTZXAddr, NULL, setPreset, setPresetRes);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");

    if (posToken != NULL) {
      *posToken = atoi(setPresetRes->PresetToken);
    }

  } else if (cmd == PRESET_GET) {
    struct _tptz__GetPresets *getPresets = soap_new__tptz__GetPresets(soap, 1);
    struct _tptz__GetPresetsResponse *getPresetsRes = soap_new__tptz__GetPresetsResponse(soap, 1);

    memset(getPresets, 0x00, sizeof(struct _tptz__GetPresets));
    memset(getPresetsRes, 0x00, sizeof(struct _tptz__GetPresetsResponse));

    getPresets->ProfileToken = DevData->profiles->token;

    result = soap_call___tptz__GetPresets(soap, PTZXAddr, NULL, getPresets, getPresetsRes);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");

#if defined(PROTO_DEBUG)
    dump__GetPresetsResponse(getPresetsRes);
#endif 

  } else if (cmd == PRESET_GOTO) {
    struct _tptz__GotoPreset *gotoPreset = soap_new__tptz__GotoPreset(soap, 1);
    struct _tptz__GotoPresetResponse *gotoPresetRes = soap_new__tptz__GotoPresetResponse(soap, 1);

    memset(gotoPreset, 0x00, sizeof(struct _tptz__GotoPreset));
    memset(gotoPresetRes, 0x00, sizeof(struct _tptz__GotoPresetResponse));

    gotoPreset->ProfileToken = DevData->profiles->token;

    if (posToken != NULL) {
      sprintf(buf, "%d", *posToken);
      gotoPreset->PresetToken = buf;
    }

    result = soap_call___tptz__GotoPreset(soap, PTZXAddr, NULL, gotoPreset, gotoPresetRes);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");

  } else if (cmd == PRESET_REMOVE) {
    struct _tptz__RemovePreset *rmPreset = soap_new__tptz__RemovePreset(soap, 1);
    struct _tptz__RemovePresetResponse *rmPresetRes = soap_new__tptz__RemovePresetResponse(soap, 1);

    memset(rmPreset, 0x00, sizeof(struct _tptz__RemovePreset));
    memset(rmPresetRes, 0x00, sizeof(struct _tptz__RemovePresetResponse));

    rmPreset->ProfileToken = DevData->profiles->token;
    if (posToken != NULL) {
      sprintf(buf, "%d", *posToken);
      rmPreset->PresetToken = buf;
    }

    result = soap_call___tptz__RemovePreset(soap, PTZXAddr, NULL, rmPreset, rmPresetRes);
    SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");
  }

EXIT:
  if (NULL != soap) {
    proto_soap_delete(soap);
  }

  return result;
}