/* 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 <iostream>
#include <math.h>
#include <stdio.h>

using namespace std;

#include "Constants.h"
#include "Environment.h"
#include "SensorInterp.h"
#include "Primitives.h"

#include "../../headers/field.h"

void
SensorInterp::interpretSensors(const Environment *environ,
                               int num_sensors,      const SensorInfo *sensor_info,
                               int *num_primitives_p,ProbEvaluator **primitives_p) {
  static PointDistProbEvaluator            point_dist_pes[32];
  static PointDistWithUniformProbEvaluator point_dist_uniform_pes[32];
  static PointAngleProbEvaluator           point_angle_pes[32];
  static LinePointProbEvaluator            line_point_pes[32];
  static CornerProbEvaluator               corner_pes[32];

  int point_dist_count        =0;
  int point_dist_uniform_count=0;
  int point_angle_count       =0;
  int line_point_count        =0;
  int corner_count            =0;

  int observation_id;
  observation_id=0;
  *num_primitives_p=0;
  for(int sensor_idx=0; sensor_idx<num_sensors; sensor_idx++) {
    int *observation_id_p = &observation_id;

    switch(sensor_info[sensor_idx].getType()) {
      case SensorInfo::PointLandmark:
        {
          const SensorInfo *obs=&sensor_info[sensor_idx];
          PointDistProbEvaluator *pt_dist_pe;
          PointAngleProbEvaluator *pt_angle_pe;

          pt_dist_pe=&point_dist_pes[point_dist_count++];
          pt_dist_pe->matchedTo=environ->getObject(obs->readingClass);
          pt_dist_pe->observationId=(*observation_id_p);
          pt_dist_pe->dist_obs = obs->data.point_data.dist;
          pt_dist_pe->dist_dev_frac = obs->data.point_data.dist_dev / obs->data.point_data.dist;
          pt_dist_pe->loc.set(V2COMP(pt_dist_pe->matchedTo->loc.point_data));
          primitives_p[*num_primitives_p]=pt_dist_pe;
          (*num_primitives_p)++;

          pt_angle_pe=&point_angle_pes[point_angle_count++];
          pt_angle_pe->matchedTo=environ->getObject(obs->readingClass);
          pt_angle_pe->observationId=(*observation_id_p);
          pt_angle_pe->angle_obs = obs->data.point_data.angle;
          pt_angle_pe->angle_dev = obs->data.point_data.angle_dev;
          pt_angle_pe->loc.set(V2COMP(pt_angle_pe->matchedTo->loc.point_data));
          primitives_p[*num_primitives_p]=pt_angle_pe;
          (*num_primitives_p)++;

          (*observation_id_p)++;
        }
      break;
      case SensorInfo::GoalLandmark:
        {
          const SensorInfo *obs=&sensor_info[sensor_idx];
          PointDistWithUniformProbEvaluator *pt_dist_uniform_pe;
          PointAngleProbEvaluator *pt_angle_pe;

          pt_dist_uniform_pe=&point_dist_uniform_pes[point_dist_uniform_count++];
          pt_dist_uniform_pe->matchedTo=environ->getObject(obs->readingClass);
          pt_dist_uniform_pe->observationId=(*observation_id_p);
          pt_dist_uniform_pe->dist_obs=obs->data.point_data.dist;
          pt_dist_uniform_pe->dist_dev_frac = obs->data.point_data.dist_dev / obs->data.point_data.dist;
          pt_dist_uniform_pe->dist_unif=pt_dist_uniform_pe->matchedTo->loc.unif_range;
          pt_dist_uniform_pe->loc.set(V2COMP(pt_dist_uniform_pe->matchedTo->loc.point_data));
          primitives_p[*num_primitives_p]=pt_dist_uniform_pe;
          (*num_primitives_p)++;

          pt_angle_pe=&point_angle_pes[point_angle_count++];
          pt_angle_pe->matchedTo=environ->getObject(obs->readingClass);
          pt_angle_pe->observationId=(*observation_id_p);
          pt_angle_pe->angle_obs=obs->data.point_data.angle;
          pt_angle_pe->angle_dev=obs->data.point_data.angle_dev;
          pt_angle_pe->loc.set(V2COMP(pt_angle_pe->matchedTo->loc.point_data));
          primitives_p[*num_primitives_p]=pt_angle_pe;
          (*num_primitives_p)++;

          (*observation_id_p)++;
        }
      break;
      case SensorInfo::LinePoint:
        {
          const SensorInfo *obs=&sensor_info[sensor_idx];
          LinePointProbEvaluator *line_point_pe=&line_point_pes[line_point_count++];

          line_point_pe->matchedTo=environ->getObject(obs->readingClass);
          line_point_pe->observationId=(*observation_id_p);
          line_point_pe->obs_ego.set(1.0,0.0);
          line_point_pe->obs_ego = line_point_pe->obs_ego.rotate(obs->data.line_point_data.angle)*obs->data.line_point_data.dist;
          line_point_pe->num_lines = line_point_pe->matchedTo->loc.line_data.num_lines;
          for(int i=0; i<line_point_pe->num_lines; i++){
            line_point_pe->p0[i] = line_point_pe->matchedTo->loc.line_data.p0[i];
            line_point_pe->p1[i] = line_point_pe->matchedTo->loc.line_data.p1[i];
          }
          line_point_pe->init_internals();
          primitives_p[*num_primitives_p]=line_point_pe;
          (*num_primitives_p)++;

          (*observation_id_p)++;
        }
      break;
      case SensorInfo::CornerLandmark:
        {
          const SensorInfo *obs=&sensor_info[sensor_idx];
          EnvironmentObject *eo;
          CornerProbEvaluator *corner_pe;

          corner_pe=&corner_pes[corner_count++];
          eo = environ->getObject(obs->readingClass);
          corner_pe->matchedTo = eo;
          corner_pe->observationId=(*observation_id_p);
          corner_pe->dist_obs = obs->data.point_data.dist;
          corner_pe->dist_dev_frac = obs->data.point_data.dist_dev / obs->data.point_data.dist;
          corner_pe->angle_obs = obs->data.point_data.angle;
          corner_pe->angle_dev = obs->data.point_data.angle_dev;
          corner_pe->num_corners = eo->loc.corner_data.num_corners;
          corner_pe->corners = eo->loc.corner_data.loc;
          primitives_p[*num_primitives_p]=corner_pe;
          (*num_primitives_p)++;

          (*observation_id_p)++;
        }
      break;
    }

    //printf("sensor_idx=%d obs_id=%d num_primitives=%d readingClass=%d\n",
    //       sensor_idx,*observation_id_p,*num_primitives_p,sensor_info[sensor_idx].readingClass);
  }
}