/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "SPOutTrackerEncoder.h"
#include "WorldModel.h"

void SPOutTrackerEncoder::encodeGaussian2(uchar **buf,
                                           const Gaussian2 * g) {
  encodeAs<float>(buf, (float)g->mean.x);
  encodeAs<float>(buf, (float)g->mean.y);
  encodeAs<float>(buf, (float)g->thetaMaj);  
  encodeAs<float>(buf, (float)g->sMaj);
  encodeAs<float>(buf, (float)g->sMin);
}

void SPOutTrackerEncoder::encodeValid(uchar **buf,
				      uchar valid) {
  encodeAs<uchar>(buf,(uchar)valid);
}

int SPOutTrackerEncoder::encodeTracker(uchar *buf, 
				       WorldModel *modeller) {
  uchar *orig_buf;
  orig_buf = buf;

  Gaussian2 temp;

  // Encode the ball's estimated position from our own observations
  for (int i=0;i<5;i++) {
    // Never use the old ball estimate here
    if (modeller->ball_tracker_observation(temp,i,false)) {
      encodeValid(&buf,(uchar)1); 
    } else { 
      encodeValid(&buf,(uchar)0); 
    }
    encodeGaussian2(&buf,&temp);
  }
  // Encode the ball's estimated position from our teammate's observations
  for (int i=0;i<5;i++) {
    // Never use the old ball estimate here
    if (modeller->teammate_ball_tracker_observation_global(temp,i)) {
      encodeValid(&buf,(uchar)1); 
    } else { 
      encodeValid(&buf,(uchar)0); 
    }
    encodeGaussian2(&buf,&temp);
  }
  return buf - orig_buf;
}