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

#include <math.h>

#include "../../headers/field.h"
#include "../../headers/random.h"
#include "Constants.h"
#include "Environment.h"
#include "SensorInterp.h"

void
PointLandmarkObject::init(double x, double y) {
  loc.point_data.x=x;
  loc.point_data.y=y;
}

Environment::~Environment() {
}

RoboCupFieldEnvironment::RoboCupFieldEnvironment() {
  objects=new EnvironmentObject *[MaxNumObjectTypes];
  int next_object_class=0;
  // markers
  for(int object_type_idx=next_object_class; object_type_idx<next_object_class+6; object_type_idx++) {
    objects[object_type_idx]=new PointLandmarkObject;
  }
  next_object_class+=6;
  // goal distance
  for(int object_type_idx=next_object_class+2; object_type_idx<=next_object_class+2+3; object_type_idx+=3) {
    objects[object_type_idx]=new PointLandmarkObject;
  }
  // goal edges
  for(int object_type_idx=next_object_class; object_type_idx<next_object_class+2; object_type_idx++) {
    objects[object_type_idx]=new PointLandmarkObject;
  }
  for(int object_type_idx=next_object_class+3; object_type_idx<next_object_class+3+2; object_type_idx++) {
    objects[object_type_idx]=new PointLandmarkObject;
  }

  next_object_class=0;
  // markers
  ((PointLandmarkObject *)(objects[next_object_class+0]))->init(+halfLength+markerOffset, +halfWidth+markerOffset);
  ((PointLandmarkObject *)(objects[next_object_class+1]))->init(+halfLength+markerOffset, -halfWidth-markerOffset);
  ((PointLandmarkObject *)(objects[next_object_class+2]))->init(                     0.0, +halfWidth+markerOffset);
  ((PointLandmarkObject *)(objects[next_object_class+3]))->init(                     0.0, -halfWidth-markerOffset);
  ((PointLandmarkObject *)(objects[next_object_class+4]))->init(-halfLength-markerOffset, +halfWidth+markerOffset);
  ((PointLandmarkObject *)(objects[next_object_class+5]))->init(-halfLength-markerOffset, -halfWidth-markerOffset);
  next_object_class+=6;

  // goal distance
  ((PointLandmarkObject *)objects[next_object_class+0])->init(-halfLength, 0);
  ((PointLandmarkObject *)objects[next_object_class+3])->init(+halfLength, 0);
  // goal edges
  ((PointLandmarkObject *)objects[next_object_class+1])->init(-halfLength, -goalHalfWidth);
  ((PointLandmarkObject *)objects[next_object_class+1])->loc.unif_range = goalDepth*1.4;
  ((PointLandmarkObject *)objects[next_object_class+2])->init(-halfLength, +goalHalfWidth);
  ((PointLandmarkObject *)objects[next_object_class+2])->loc.unif_range = goalDepth*1.4;
  ((PointLandmarkObject *)objects[next_object_class+4])->init(+halfLength, +goalHalfWidth);
  ((PointLandmarkObject *)objects[next_object_class+4])->loc.unif_range = goalDepth*1.4;
  ((PointLandmarkObject *)objects[next_object_class+5])->init(+halfLength, -goalHalfWidth);
  ((PointLandmarkObject *)objects[next_object_class+5])->loc.unif_range = goalDepth*1.4;
  next_object_class+=6;

  EnvironmentObject *lines=new EnvironmentObject;
  lines->loc.line_data.num_lines=4;
  lines->loc.line_data.p0[0].set(-halfLength, halfWidth);
  lines->loc.line_data.p1[0].set( halfLength, halfWidth);
  lines->loc.line_data.p0[1].set( halfLength, halfWidth);
  lines->loc.line_data.p1[1].set( halfLength,-halfWidth);
  lines->loc.line_data.p0[2].set( halfLength,-halfWidth);
  lines->loc.line_data.p1[2].set(-halfLength,-halfWidth);
  lines->loc.line_data.p0[3].set(-halfLength,-halfWidth);
  lines->loc.line_data.p1[3].set(-halfLength, halfWidth);
  objects[next_object_class] = lines;
  next_object_class++;

  lines=new EnvironmentObject;
  lines->loc.line_data.num_lines=9;
  lines->loc.line_data.p0[0].set(-halfLength, goalHalfWidth);
  lines->loc.line_data.p1[0].set(-halfLength,-goalHalfWidth);
  lines->loc.line_data.p0[1].set(        0.0, halfWidth);
  lines->loc.line_data.p1[1].set(        0.0,-halfWidth);
  lines->loc.line_data.p0[2].set( halfLength, goalHalfWidth);
  lines->loc.line_data.p1[2].set( halfLength,-goalHalfWidth);

  lines->loc.line_data.p0[3].set(-halfLength,                penaltyRegionHalfWidth);
  lines->loc.line_data.p1[3].set(-penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  lines->loc.line_data.p0[4].set(-penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  lines->loc.line_data.p1[4].set(-penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  lines->loc.line_data.p0[5].set(-penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  lines->loc.line_data.p1[5].set(-halfLength,               -penaltyRegionHalfWidth);

  lines->loc.line_data.p0[6].set( halfLength,                penaltyRegionHalfWidth);
  lines->loc.line_data.p1[6].set( penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  lines->loc.line_data.p0[7].set( penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  lines->loc.line_data.p1[7].set( penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  lines->loc.line_data.p0[8].set( penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  lines->loc.line_data.p1[8].set( halfLength,               -penaltyRegionHalfWidth);

  objects[next_object_class] = lines;
  next_object_class++;

  // yellow goal
  lines=new EnvironmentObject;
  lines->loc.line_data.num_lines=3;
  lines->loc.line_data.p0[0].set(-halfLength,           goalHalfWidth);
  lines->loc.line_data.p1[0].set(-halfLength-goalDepth, goalHalfWidth);
  lines->loc.line_data.p0[1].set(-halfLength-goalDepth, goalHalfWidth);
  lines->loc.line_data.p1[1].set(-halfLength-goalDepth,-goalHalfWidth);
  lines->loc.line_data.p0[2].set(-halfLength-goalDepth,-goalHalfWidth);
  lines->loc.line_data.p1[2].set(-halfLength,          -goalHalfWidth);

  objects[next_object_class] = lines;
  next_object_class++;

  // cyan goal
  lines=new EnvironmentObject;
  lines->loc.line_data.num_lines=3;
  lines->loc.line_data.p0[0].set( halfLength,           goalHalfWidth);
  lines->loc.line_data.p1[0].set( halfLength+goalDepth, goalHalfWidth);
  lines->loc.line_data.p0[1].set( halfLength+goalDepth, goalHalfWidth);
  lines->loc.line_data.p1[1].set( halfLength+goalDepth,-goalHalfWidth);
  lines->loc.line_data.p0[2].set( halfLength+goalDepth,-goalHalfWidth);
  lines->loc.line_data.p1[2].set( halfLength,          -goalHalfWidth);

  objects[next_object_class] = lines;
  next_object_class++;

  // stripe/stripe corners
  EnvironmentObject *corners=new EnvironmentObject;
  corners->loc.corner_data.num_corners=10;

  corners->loc.corner_data.loc[0].set(                halfLength, penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[2].set( penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[4].set(                       0.0,     centerCircleRadius);
  corners->loc.corner_data.loc[6].set(-penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[8].set(               -halfLength, penaltyRegionHalfWidth);

  corners->loc.corner_data.loc[1].set(                halfLength,-penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[3].set( penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[5].set(                       0.0,    -centerCircleRadius);
  corners->loc.corner_data.loc[7].set(-penaltyRegionLengthOffset,-penaltyRegionHalfWidth);
  corners->loc.corner_data.loc[9].set(               -halfLength,-penaltyRegionHalfWidth);

  objects[next_object_class] = corners;
  next_object_class++;

  sensorInterp=new SensorInterp;
}

RoboCupFieldEnvironment::~RoboCupFieldEnvironment() {
}

bool
RoboCupFieldEnvironment::inEnvironment(Sample *sample) const {
  double abs_x,abs_y;

  abs_x=fabs(sample->loc.x);
  abs_y=fabs(sample->loc.y);

  // in middle of field
  if(abs_x < (halfLength - wedgeSize) &&
     abs_y < (halfWidth))
     return true;

  if(abs_x < (halfLength) &&
     abs_y < (halfWidth - wedgeSize))
     return true;

  // in goal
  if(abs_y < goalHalfWidth &&
     abs_x < halfLength + goalDepth)
    return true;

  // near wedge
  if(abs_y < halfWidth &&
     abs_x < halfLength &&
     abs_x + abs_y < (halfLength + halfWidth - wedgeSize))
    return true;

  return false;
}

bool
RoboCupFieldEnvironment::placeInEnviron(Sample *sample, double offset) const {
  Sample::Element *x=&sample->loc.x;
  Sample::Element *y=&sample->loc.y;

  if(inEnvironment(sample))
    return false;

  // off field!

  double abs_x,abs_y;

  abs_y=fabs(*y);
  abs_x=fabs(*x);

  double clipped_x=*x,clipped_y=*y;

  if(abs_y - goalHalfWidth < abs_x - halfLength) {
    // in goal

    if(abs_x > halfLength + goalDepth) {
      if(*x > 0.0)
        clipped_x = halfLength + goalDepth - offset;
      else
        clipped_x = -halfLength - goalDepth + offset;
    }
    
    if(abs_y > goalHalfWidth) {
      if(*y > 0.0)
        clipped_y = +goalHalfWidth - offset;
      else
        clipped_y = -goalHalfWidth + offset;
    }
  }
  else {
    // off ends
    if(abs_x > halfLength) {
      if(*x > 0.0)
        clipped_x = halfLength - offset;
      else
        clipped_x = -halfLength + offset;
    }
    
    // off sides
    if(abs_y > halfWidth) {
      if(*y > 0.0)
        clipped_y = halfWidth - offset;
      else
        clipped_y = -halfWidth + offset;
    }

    // in wedge
    double abs_clipped_x=fabs(clipped_x);
    double abs_clipped_y=fabs(clipped_y);
    if(abs_clipped_y < halfWidth &&
       abs_clipped_x < halfLength &&
       abs_clipped_x + abs_clipped_y > (halfLength + halfWidth - wedgeSize)) {
      double off_by = abs_clipped_x + abs_clipped_y - (halfLength + halfWidth - wedgeSize);

      off_by+=offset;
      off_by/=2.0;

      if(*x < 0.0) {
        clipped_x += off_by;
      }
      else {
        clipped_x -= off_by;
      }

      if(*y < 0.0) {
        clipped_y += off_by;
      }
      else {
        clipped_y -= off_by;
      }
    }
  }

  *x=clipped_x;
  *y=clipped_y;

  return true;
}

void
RoboCupFieldEnvironment::getAnglesAllowed(double marker_x,double marker_y,double *low_angle,double *high_angle) const {
  double corner_goal_angle=-1.0; // angle from corner of goal to marker in radians
  if(corner_goal_angle==-1.0)
    corner_goal_angle=atan((halfWidth+markerOffset-goalHalfWidth)/(goalDepth-markerOffset));

  if(marker_x >= 0.0)
    *low_angle=0.0;
  else
    *low_angle=corner_goal_angle;
  
  if(marker_x <= 0.0)
    *high_angle=M_PI;
  else
    *high_angle=M_PI-corner_goal_angle;
  
  if(fabs(marker_y) <= goalHalfWidth && fabs(marker_x)>=halfLength && fabs(marker_x)<=halfLength+goalDepth) {
    if(marker_y > 0) {
      *low_angle =-M_PI/2;
      *high_angle=M_PI/2;
    }
    else {
      *low_angle =M_PI/2;
      *high_angle=3*M_PI/2;
    }
    return;
  }

  if(marker_y < 0) {
    double tmp =*low_angle;
    *low_angle =2*M_PI - *high_angle;
    *high_angle=2*M_PI - tmp;
  }
}

void
RoboCupFieldEnvironment::genUniformSample(Sample *sample) const {
  sample->loc.x=GlobalRandom.uniform(-halfLength,+halfLength);
  sample->loc.y=GlobalRandom.uniform(-halfWidth, +halfWidth );
  sample->angle=GlobalRandom.uniform(-M_PI,      +M_PI      );
}

EnvironmentObject *
RoboCupFieldEnvironment::getObject(int reading_class) const {
  return objects[reading_class];
}