/* 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 "../headers/Util.h"
#include "../headers/Geometry.h"
#include "../headers/Sensors.h"
#include "../Motion/MotionInterface.h"

#include "PostureDetector.h"


PostureDetector::PostureDetector()
{
  down.set(0,0,-1);
  kicking = false;
  current = PosStable;
}

void PostureDetector::update(SensorData &sensor)
{
  const SensorDataFrame *s;
  const int num_frames = 16;
  int i;

  vector3d dir,dir_avg;
  double cos_avg;

  // loop over past frames getting average dir and cosine with down vector
  cos_avg = 0.0;
  dir_avg.set(0,0,0);
  for(i=0; i<num_frames; i++){
    s = sensor.getFrame(-i);
    dir = s->accel.norm();
    dir_avg += dir;
    cos_avg += down.dot(dir);
  }
  cos_avg /= num_frames;
  dir_avg /= num_frames;

  if((cos_avg > 0.50) || kicking){
    current = PosStable;
  }else{
    if(fabs(dir_avg.x) > fabs(dir_avg.y)){
      current = (dir_avg.x > 0)? PosFallFront : PosFallBack;
    }else{
      current = (dir_avg.y > 0)? PosFallLeft : PosFallRight;
    }
  }
}

void PostureDetector::update(Motion::MotionLocalizationUpdate &motion)
{
  kicking = (motion.state_type == Motion::STATE_KICKING);

  down.set(0,0,-1);
  down = down.rotate_y(-motion.body.angle);
}