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

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

#include "GuiState.h"

UI *ui=NULL;

WorldViewState::WorldViewState() {
  xSize = 1;
  ySize = 1;

  xScale = 1.0;
  yScale = 1.0;
}

void
WorldViewState::setSize(int new_xsize, int new_ysize) {
  xSize = new_xsize;
  ySize = new_ysize;

  xScale = (xSize/2.0) / (halfFieldViewLength*viewSize);
  yScale = (ySize/2.0) / (halfFieldViewWidth *viewSize);

  initFieldPoints();
}

int
WorldViewState::xWorldToScreen(float world_x) {
  int x=(int)rint( world_x*xScale + xSize/2.0);
  return x;
}

int
WorldViewState::yWorldToScreen(float world_y) {
  int y=(int)rint(-world_y*yScale + ySize/2.0);
  return y;
}

GdkPoint
WorldViewState::worldToScreen(vector2d world) {
  return worldToScreen(world.x,world.y);
}

GdkPoint
WorldViewState::worldToScreen(float x,float y) {
  GdkPoint pt;
  pt.x = xWorldToScreen(x);
  pt.y = yWorldToScreen(y);
  return pt;
}

int
WorldViewState::xWorldDiffToScreen(float x_diff) {
  return (int)rint(x_diff*xScale);
}

int
WorldViewState::yWorldDiffToScreen(float y_diff) {
  return (int)rint(y_diff*xScale);
}

GdkPoint *
WorldViewState::getFieldPoints() {
  return fspts;
}

void
WorldViewState::initFieldPoints() {
  static const double goal_back=halfLength+goalDepth;
  static const double goal_line=halfLength;
  static const double wedge_x  =halfLength-wedgeSize;
  static const double goal_side=goalHalfWidth;
  static const double wedge_y  =halfWidth-wedgeSize;
  static const double side_line=halfWidth;

  fspts[ 0]=worldToScreen(-goal_back,-goal_side);
  fspts[ 1]=worldToScreen(-goal_back,+goal_side);
  fspts[ 2]=worldToScreen(-goal_line,+goal_side);
  fspts[ 3]=worldToScreen(-goal_line,  +wedge_y);
  fspts[ 4]=worldToScreen(  -wedge_x,+side_line);
  fspts[ 5]=worldToScreen(  +wedge_x,+side_line);
  fspts[ 6]=worldToScreen(+goal_line,  +wedge_y);
  fspts[ 7]=worldToScreen(+goal_line,+goal_side);
  fspts[ 8]=worldToScreen(+goal_back,+goal_side);
  fspts[ 9]=worldToScreen(+goal_back,-goal_side);
  fspts[10]=worldToScreen(+goal_line,-goal_side);
  fspts[11]=worldToScreen(+goal_line,  -wedge_y);
  fspts[12]=worldToScreen(  +wedge_x,-side_line);
  fspts[13]=worldToScreen(  -wedge_x,-side_line);
  fspts[14]=worldToScreen(-goal_line,  -wedge_y);
  fspts[15]=worldToScreen(-goal_line,-goal_side);
  fspts[16]=worldToScreen(-goal_back,-goal_side);
}

EgoViewState::EgoViewState() {
  setSize(1,1);
}

void
EgoViewState::setSize(int new_xsize, int new_ysize) {
  xSize = new_xsize;
  ySize = new_ysize;
  minSize = min(xSize,ySize);

  center.set(xSize/2,ySize/2);

  initRobotPoints();
}

double
EgoViewState::distScale(double dist) {
  // about 2/3 of the display will be devoted to things under this distance
  static const double max_dist=1500.0;

  //return dist/max_dist;

  double scaled_dist;
  scaled_dist = 1-exp(-dist/max_dist);

  if(scaled_dist > 1.0) {
    printf("dist %g scaled %g\n",dist,scaled_dist);
  }

  return scaled_dist;
}

GVector::vector2d<int>
EgoViewState::egoToScreen(vector2d ego) {
  double dist,angle;

  dist  = ego.length();
  angle = ego.angle();

  dist = distScale(dist);

  vector2d screen;
  screen.set(cos(angle),sin(angle));
  screen *= dist*(minSize/2);
  screen.y = -screen.y;

  GVector::vector2d<int> screen_img;
  screen_img.set((int)(screen.x + .5),(int)(screen.y + .5));
  screen_img += center;

  return screen_img;
}

void
EgoViewState::initRobotPoints() {
  static const double robot_half_x=RobotLength / 2.0;
  static const double robot_half_y=RobotWidth  / 2.0;

  GVector::vector2d<int> robot_pt;

  robot_pt = egoToScreen(vector2d( 0*robot_half_x, robot_half_y));
  rpts[0].x = robot_pt.x; rpts[0].y = robot_pt.y;
  robot_pt = egoToScreen(vector2d(-2*robot_half_x, robot_half_y));
  rpts[1].x = robot_pt.x; rpts[1].y = robot_pt.y;
  robot_pt = egoToScreen(vector2d(-2*robot_half_x,-robot_half_y));
  rpts[2].x = robot_pt.x; rpts[2].y = robot_pt.y;
  robot_pt = egoToScreen(vector2d( 0*robot_half_x,-robot_half_y));
  rpts[3].x = robot_pt.x; rpts[3].y = robot_pt.y;
}

GdkPoint *
EgoViewState::getRobotPoints() {
  return rpts;
}

extern int ImageXSize;
extern int ImageYSize;

VisRLEState::VisRLEState() :
  xSize(ImageXSize),
  ySize(ImageYSize),
  xScale(1),
  yScale(1)
{
}

void
VisRLEState::setSize(int new_xsize, int new_ysize) {
  xSize = new_xsize;
  ySize = new_ysize;

  xScale = new_xsize / ImageXSize;
  if(xScale < 1) xScale = 1;
  yScale = new_ysize / ImageYSize;
  if(yScale < 1) yScale = 1;

  xOffset = (xSize - xScale*ImageXSize)/2;
  yOffset = (ySize - yScale*ImageYSize)/2;
}