/** 
 * @file Tools/Debugging/DebugDrawing.cpp
 * Implementation of class DebugDrawing.
 *
 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
 */
#include "DebugDrawing.h"
#include "Tools/FieldDimensions.h"

DebugDrawing::DebugDrawing()
{
  firstLine = 0;
  firstEllipse = 0;
  firstPolygon = 0;
  reset();
}

DebugDrawing::DebugDrawing
(
 FieldDrawing drawing
 )
{
  firstLine = 0;
  firstEllipse = 0;
  firstPolygon = 0;
  reset();
  this->fieldDrawingID = drawing;
  this->typeOfDrawing = drawingOnField;
}


DebugDrawing::DebugDrawing
(
 ImageDrawing drawing
 )
{
  firstLine = 0;
  firstEllipse = 0;
  firstPolygon = 0;
  reset();
  this->imageDrawingID = drawing;
  this->typeOfDrawing = drawingOnImage;
}

const DebugDrawing& DebugDrawing::operator=(const DebugDrawing& other)
{
  reset();

  fieldDrawingID = other.fieldDrawingID;
  imageDrawingID = other.imageDrawingID;
  typeOfDrawing = other.typeOfDrawing;

  numberOfLines = other.numberOfLines;
  numberOfEllipses = other.numberOfEllipses;
  numberOfPolygons = other.numberOfPolygons;

  LineList* currentLineInOriginal = other.firstLine;
  EllipseList* currentEllipseInOriginal = other.firstEllipse;
  PolygonList* currentPolygonInOriginal = other.firstPolygon;
  
  //copy Lines
  while(currentLineInOriginal != 0)
  { 
    //first element
    if (currentLine == 0)
    {
      firstLine = new LineList();
      currentLine = firstLine;
    }
    else
    {
      currentLine->next = new LineList();
      currentLine = currentLine->next;
    }      
    currentLine->line = currentLineInOriginal->line;
    currentLine->next = 0;
    
    currentLineInOriginal = currentLineInOriginal->next;
  }

  //copy ellipses
  while(currentEllipseInOriginal != 0)
  { 
    //first element
    if (currentEllipse == 0)
    {
      firstEllipse = new EllipseList();
      currentEllipse = firstEllipse;
    }
    else
    {
      currentEllipse->next = new EllipseList();
      currentEllipse = currentEllipse->next;
    }      
    currentEllipse->ellipse = currentEllipseInOriginal->ellipse;
    currentEllipse->next = 0;
    
    currentEllipseInOriginal = currentEllipseInOriginal->next;
  }

  //copy polygons
  while(currentPolygonInOriginal != 0)
  { 
    //first element
    if (currentPolygon == 0)
    {
      firstPolygon = new PolygonList();
      currentPolygon = firstPolygon;
    }
    else
    {
      currentPolygon->next = new PolygonList();
      currentPolygon = currentPolygon->next;
    }      
    currentPolygon->polygon = currentPolygonInOriginal->polygon;
    currentPolygon->next = 0;
    
    currentPolygonInOriginal = currentPolygonInOriginal->next;
  }
  return *this;
}

DebugDrawing::DebugDrawing(const DebugDrawing& other)
{
  firstLine = 0;
  firstEllipse = 0;
  firstPolygon = 0;
  *this = other;
}

DebugDrawing::DebugDrawing(const DebugDrawing* pDebugDrawing)
{
  firstLine = 0;
  firstEllipse = 0;
  firstPolygon = 0;
  *this = *pDebugDrawing;
}

void DebugDrawing::reset()
{
  while(firstLine != 0)
  {
    LineList* temp = firstLine;
    firstLine = firstLine->next;
    delete temp;
  }
  while(firstPolygon != 0)
  {
    PolygonList* temp = firstPolygon;
    firstPolygon = firstPolygon->next;
    delete temp;
  }
  while(firstEllipse != 0)
  {
    EllipseList* temp = firstEllipse;
    firstEllipse = firstEllipse->next;
    delete temp;
  }

  currentLine = 0;
  numberOfLines = 0;
  
  currentEllipse = 0;
  numberOfEllipses = 0;
  
  currentPolygon = 0;
  numberOfPolygons = 0;
}

void DebugDrawing::arrow
( 
  Vector2<double> offset,
  Vector2<double> direction,
  Color penColor
)
{
  Vector2<double> perpendicular((.25*direction.y), (-.25*direction.x));

  line((int)offset.x, (int)offset.y, (int)(offset.x - direction.x), (int)(offset.y - direction.y), DebugDrawing::ps_solid, 0, penColor);
  line((int)offset.x, (int)offset.y, (int)(offset.x - direction.x/4 + perpendicular.x), (int)(offset.y - direction.y/4 + perpendicular.y), DebugDrawing::ps_solid, 0, penColor);
  line((int)offset.x, (int)offset.y, (int)(offset.x - direction.x/4 - perpendicular.x), (int)(offset.y - direction.y/4 - perpendicular.y), DebugDrawing::ps_solid, 0, penColor);
}


void DebugDrawing::line
(
 int xStart, 
 int yStart, 
 int xEnd,
 int yEnd,
 PenStyle penStyle,
 int width,
 Color penColor
 )
{
  if(currentLine == 0)
  {
    firstLine = new LineList();
    currentLine = firstLine;
  }
  else
  {
    currentLine->next = new LineList();
    currentLine = currentLine->next;
  }
  currentLine->line.xStart = xStart;
  currentLine->line.yStart = yStart;
  currentLine->line.xEnd = xEnd;
  currentLine->line.yEnd = yEnd;
  currentLine->line.penStyle = penStyle;
  currentLine->line.width = width;
  currentLine->line.penColor = penColor;
  currentLine->next = 0;
  numberOfLines++;
}

void DebugDrawing::line
(
 int xStart, 
 int yStart, 
 int xEnd,
 int yEnd
 )
{
  line(xStart, yStart, xEnd, yEnd, ps_solid, 1, Color(0,0,0));
}

void DebugDrawing::polygon
(
 Vector2<int> points[MAX_NUMBER_OF_POINTS],
 int nCount,
 int width,
 PenStyle penStyle,
 Color penColor,
 FillStyle fillStyle,
 Color fillColor
 )
{
  if(currentPolygon == 0)
  {
    firstPolygon = new PolygonList();
    currentPolygon = firstPolygon;
  }
  else
  {
    currentPolygon->next = new PolygonList();
    currentPolygon = currentPolygon->next;
  }
  for(int i = 0; i < nCount; i++)
  {
    currentPolygon->polygon.points[i] = points[i];
  }
  currentPolygon->polygon.nCount = nCount;
  currentPolygon->polygon.width = width;
  currentPolygon->polygon.penStyle = penStyle;
  currentPolygon->polygon.penColor = penColor;
  currentPolygon->polygon.fillStyle = fillStyle;
  currentPolygon->polygon.fillColor = fillColor;
  currentPolygon->next = 0;
  numberOfPolygons++;
}

void DebugDrawing::dot
(
 int x,
 int y,
 Color penColor,
 Color fillColor
 )
{
  Vector2<int> points[4];
  points[0].x = x - 1;
  points[0].y = y - 1;
  points[1].x = x + 1;
  points[1].y = y - 1;
  points[2].x = x + 1;
  points[2].y = y + 1;
  points[3].x = x - 1;
  points[3].y = y + 1;
  polygon(
    points,
    4,
    0,
    ps_solid,
    penColor,
    bs_solid,
    fillColor);
}

void DebugDrawing::rectangle
(
 int left, 
 int right, 
 int top,
 int bottom,
 int width,
 PenStyle penStyle,
 Color penColor,
 FillStyle fillStyle,
 Color fillColor
 )
{
  Vector2<int> points[4];
  points[0].x = left;
  points[0].y = top;
  points[1].x = right;
  points[1].y = top;
  points[2].x = right;
  points[2].y = bottom;
  points[3].x = left;
  points[3].y = bottom;
  polygon(
    points,
    4,
    width,
    penStyle,
    penColor,
    fillStyle,
    fillColor);
}

void DebugDrawing::ellipse
(
 int left, 
 int top,
 int right, 
 int bottom,
 int width,
 PenStyle penStyle,
 Color penColor,
 FillStyle fillStyle,
 Color fillColor
 )
{
  if(currentEllipse == 0)
  {
    firstEllipse = new EllipseList();
    currentEllipse = firstEllipse;
  }
  else
  {
    currentEllipse->next = new EllipseList();
    currentEllipse = currentEllipse->next;
  }
  currentEllipse->ellipse.left = left;
  currentEllipse->ellipse.right= right;
  currentEllipse->ellipse.top = top;
  currentEllipse->ellipse.bottom = bottom;
  currentEllipse->ellipse.width = width;
  currentEllipse->ellipse.penStyle = penStyle;
  currentEllipse->ellipse.penColor = penColor;
  currentEllipse->ellipse.fillStyle = fillStyle;
  currentEllipse->ellipse.fillColor = fillColor;
  currentEllipse->next = 0;
  numberOfEllipses++;
}

void DebugDrawing::ellipse
(
 int left, int top, int right, int bottom, Color color
 )
{
  ellipse(left, top, right, bottom, 0, ps_solid, Color(0,0,0), bs_solid, color);
}

void DebugDrawing::circle
(
 int xCenter,
 int yCenter,
 int radius, 
 int penWidth,
 PenStyle penStyle,
 Color penColor,
 FillStyle fillStyle,
 Color fillColor
 )
{
  ellipse(
    xCenter - radius, yCenter - radius,
    xCenter + radius + 1, yCenter + radius + 1, 
    penWidth, penStyle, penColor,
    fillStyle, fillColor);
}


// added Feb., 6th 2002 by Denny N. Kone
// draws a Pose2D as an Arrow on the field

void DebugDrawing::pose2DSample
(
 Pose2D pose
 )
{
  Pose2D current = pose;
  current += Pose2D(-100,0);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y));
  current = pose;
  current += Pose2D(-40,-40);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y));
  current = pose;
  current += Pose2D(-40,40);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y));
}

void DebugDrawing::pose2DSample
(
 Pose2D pose, Color color
 )
{
  Pose2D current = pose;
  current += Pose2D(-100,0);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y),
    ps_solid, 1, color);
  current = pose;
  current += Pose2D(-40,-40);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y),
    ps_solid, 1, color);
  current = pose;
  current += Pose2D(-40,40);
  line(int(current.translation.x),int(current.translation.y),int(pose.translation.x),int(pose.translation.y),
    ps_solid, 1, color);
}

void DebugDrawing::potentialfieldRectangles(int potentialColor[])
{
  int fieldWidth = ((2*xPosFrontFlags) / DebugDrawing::POTENTIALFIELD_DRAWING_X_RESOLUTION);
  int fieldHeight = ((2*yPosLeftFlags) / DebugDrawing::POTENTIALFIELD_DRAWING_Y_RESOLUTION);

  int top = yPosRightFlags,
      bottom = top + fieldHeight,
      left = xPosFrontFlags,
      right = left - fieldWidth;

  int index = 0;
  for(int y = 0; y < DebugDrawing::POTENTIALFIELD_DRAWING_Y_RESOLUTION; y++)
  {
    for(int x = 0; x < DebugDrawing::POTENTIALFIELD_DRAWING_X_RESOLUTION; x++)
    {
/*
      this->rectangle(left, right, top, bottom,
                      0, PenStyle::ps_null, DebugDrawing::Color(0,0,0), FillStyle::bs_solid,
                      DebugDrawing::Color(potentialColor[index], potentialColor[index], potentialColor[index]));
*/
      this->rectangle(left, right, top, bottom, DebugDrawing::Color(potentialColor[index], potentialColor[index], potentialColor[index]));
      left = right - 1;
      right = left - fieldWidth;
      
      index++;
    }
    top = bottom + 1;
    bottom = top + fieldHeight;
    left = xPosFrontFlags;
    right = left - fieldWidth;

  }

}


In& operator>>(In& stream,DebugDrawing& debugDrawing)
{
  debugDrawing.reset();

  int fieldDrawingID, imageDrawingID, typeOfDrawing, penStyle, fillStyle;
  stream >> fieldDrawingID;
  debugDrawing.fieldDrawingID = (DebugDrawing::FieldDrawing)fieldDrawingID;

  stream >> imageDrawingID;
  debugDrawing.imageDrawingID = (DebugDrawing::ImageDrawing)imageDrawingID;

  stream >> typeOfDrawing;
  debugDrawing.typeOfDrawing = (enum DebugDrawing::TypeOfDrawing)typeOfDrawing;

  stream >> debugDrawing.numberOfLines;
  stream >> debugDrawing.numberOfEllipses;
  stream >> debugDrawing.numberOfPolygons;

  //Lines
  DebugDrawing::LineList* currentLine = 0;
  for(int lineNumber = 0; lineNumber < debugDrawing.numberOfLines; lineNumber++)
  {
    //first element
    if (currentLine == 0)
    {
      debugDrawing.firstLine = new DebugDrawing::LineList();
      currentLine = debugDrawing.firstLine;
    }
    else
    {
      currentLine->next = new DebugDrawing::LineList();
      currentLine = currentLine->next;
    }      
    stream >> currentLine->line.xStart;
    stream >> currentLine->line.yStart;
    stream >> currentLine->line.xEnd;
    stream >> currentLine->line.yEnd;
    stream >> currentLine->line.penColor.red;
    stream >> currentLine->line.penColor.blue;
    stream >> currentLine->line.penColor.green;
    stream >> penStyle;
    currentLine->line.penStyle = (DebugDrawing::PenStyle)penStyle;
    stream >> currentLine->line.width;
    currentLine->next = 0;
  }

  //Ellipses
  DebugDrawing::EllipseList* currentEllipse = 0;
  for(int ellipseNumber = 0; ellipseNumber < debugDrawing.numberOfEllipses; ellipseNumber++)
  {
    //first element
    if (currentEllipse == 0)
    {
      debugDrawing.firstEllipse = new DebugDrawing::EllipseList();
      currentEllipse = debugDrawing.firstEllipse;
    }
    else
    {
      currentEllipse->next = new DebugDrawing::EllipseList();
      currentEllipse = currentEllipse->next;
    }      
    stream >> currentEllipse->ellipse.left;
    stream >> currentEllipse->ellipse.right;
    stream >> currentEllipse->ellipse.top;
    stream >> currentEllipse->ellipse.bottom;

    stream >> penStyle;
    currentEllipse->ellipse.penStyle = (DebugDrawing::PenStyle)penStyle;
    stream >> currentEllipse->ellipse.width;
    
    stream >> currentEllipse->ellipse.penColor.red;
    stream >> currentEllipse->ellipse.penColor.green;
    stream >> currentEllipse->ellipse.penColor.blue;

    stream >> fillStyle;
    currentEllipse->ellipse.fillStyle = (DebugDrawing::FillStyle)fillStyle;
    stream >> currentEllipse->ellipse.fillColor.red;
    stream >> currentEllipse->ellipse.fillColor.green;
    stream >> currentEllipse->ellipse.fillColor.blue;
    currentEllipse->next = 0;
   }

  //Polygons
  DebugDrawing::PolygonList* currentPolygon = 0;
  for(int polygonNumber = 0; polygonNumber < debugDrawing.numberOfPolygons; polygonNumber++)
  {
    //first element
    if (currentPolygon == 0)
    {
      debugDrawing.firstPolygon = new DebugDrawing::PolygonList();
      currentPolygon = debugDrawing.firstPolygon;
    }
    else
    {
      currentPolygon->next = new DebugDrawing::PolygonList();
      currentPolygon = currentPolygon->next;
    }      
    stream >> penStyle;
    currentPolygon->polygon.penStyle = (DebugDrawing::PenStyle)penStyle;
    stream >> currentPolygon->polygon.width;

    stream >> currentPolygon->polygon.penColor.red;
    stream >> currentPolygon->polygon.penColor.green;
    stream >> currentPolygon->polygon.penColor.blue;

    stream >> fillStyle;
    currentPolygon->polygon.fillStyle = (DebugDrawing::FillStyle)fillStyle;
    stream >> currentPolygon->polygon.fillColor.red;
    stream >> currentPolygon->polygon.fillColor.green;
    stream >> currentPolygon->polygon.fillColor.blue;


    stream >> currentPolygon->polygon.nCount;

    for(int n = 0; n < currentPolygon->polygon.nCount; n++)
    {
      stream >> currentPolygon->polygon.points[n].x;
      stream >> currentPolygon->polygon.points[n].y;
    }

    currentPolygon->next = 0;
  }
  return stream;
}















Out& operator<<(Out& stream, const DebugDrawing& debugDrawing)
{
  stream << debugDrawing.fieldDrawingID;
  stream << debugDrawing.imageDrawingID;
  stream << debugDrawing.typeOfDrawing;
  stream << debugDrawing.numberOfLines;
  stream << debugDrawing.numberOfEllipses;
  stream << debugDrawing.numberOfPolygons;

  //Lines
  DebugDrawing::LineList* currentLine = debugDrawing.firstLine;
  while(currentLine != 0)
  {
    stream << currentLine->line.xStart;
    stream << currentLine->line.yStart;
    stream << currentLine->line.xEnd;
    stream << currentLine->line.yEnd;
    stream << currentLine->line.penColor.red;
    stream << currentLine->line.penColor.blue;
    stream << currentLine->line.penColor.green;
    stream << currentLine->line.penStyle;
    stream << currentLine->line.width;

    currentLine = currentLine->next;
  }

  //Ellipses
  DebugDrawing::EllipseList* currentEllipse = debugDrawing.firstEllipse;
  while(currentEllipse != 0)
  {
    stream << currentEllipse->ellipse.left;
    stream << currentEllipse->ellipse.right;
    stream << currentEllipse->ellipse.top;
    stream << currentEllipse->ellipse.bottom;

    stream << currentEllipse->ellipse.penStyle;
    stream << currentEllipse->ellipse.width;
    
    stream << currentEllipse->ellipse.penColor.red;
    stream << currentEllipse->ellipse.penColor.green;
    stream << currentEllipse->ellipse.penColor.blue;

    stream << currentEllipse->ellipse.fillStyle;
    stream << currentEllipse->ellipse.fillColor.red;
    stream << currentEllipse->ellipse.fillColor.green;
    stream << currentEllipse->ellipse.fillColor.blue;
    

    currentEllipse = currentEllipse->next;
  }

  //Polygons
  DebugDrawing::PolygonList* currentPolygon = debugDrawing.firstPolygon;
  while(currentPolygon != 0)
  {
    stream << currentPolygon->polygon.penStyle;
    stream << currentPolygon->polygon.width;

    stream << currentPolygon->polygon.penColor.red;
    stream << currentPolygon->polygon.penColor.green;
    stream << currentPolygon->polygon.penColor.blue;

    stream << currentPolygon->polygon.fillStyle;
    stream << currentPolygon->polygon.fillColor.red;
    stream << currentPolygon->polygon.fillColor.green;
    stream << currentPolygon->polygon.fillColor.blue;


    stream << currentPolygon->polygon.nCount;

    for(int n = 0; n < currentPolygon->polygon.nCount; n++)
    {
      stream << currentPolygon->polygon.points[n].x;
      stream << currentPolygon->polygon.points[n].y;
    }
    currentPolygon = currentPolygon->next;
  }
  return stream;
}


/*
 * Change log :
 * 
 * $Log: DebugDrawing.cpp,v $
 * Revision 1.3  2002/10/14 13:14:24  dueffert
 * doxygen comments corrected
 *
 * Revision 1.2  2002/09/22 18:40:52  risler
 * added new math functions, removed GTMath library
 *
 * Revision 1.1  2002/09/10 15:53:58  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed challenge related source code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.6  2002/06/17 18:34:43  kspiess
 * new drawing method for pose2d with color
 *
 * Revision 1.5  2002/06/12 11:35:36  roefer
 * Deleting lists is now less stack-intensive
 *
 * Revision 1.4  2002/06/06 13:59:35  jhoffman
 * no message
 *
 * Revision 1.3  2002/06/03 13:12:57  kspiess
 * debug drawing for potentialfield added
 *
 * Revision 1.2  2002/05/27 10:40:20  dueffert
 * warnings removed
 *
 * Revision 1.1.1.1  2002/05/10 12:40:32  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.12  2002/04/23 14:16:47  jhoffman
 * added gradient image processor
 *
 * Revision 1.1.1.1  2002/04/10 11:03:22  juengel
 * no message
 *
 * Revision 1.11  2002/04/07 13:02:13  roefer
 * Improved goal drawing
 *
 * Revision 1.10  2002/02/10 12:45:15  loetzsch
 * radar viewer and percept visualization continued
 *
 * Revision 1.9  2002/02/08 13:28:08  loetzsch
 * Visualization of PerceptCollection and RadarViewer started
 * (not implemented yet)
 *
 * Revision 1.8  2002/02/06 15:34:44  Denny
 * added method for drawing of Pose2D as Arrows on the field
 *
 * Revision 1.7  2002/01/22 14:55:15  juengel
 * ImageToPerceptCollection eingeführt
 *
 * Revision 1.6  2002/01/17 15:13:18  roefer
 * Views and debug output in SimGT2002
 *
 * Revision 1.5  2002/01/06 12:15:01  juengel
 * Streaming Operatoren iplementiert
 *
 * Revision 1.4  2002/01/04 14:35:19  juengel
 * no message
 *
 * Revision 1.3  2001/12/14 17:35:49  juengel
 * struct Color, enum penStyle und enum fillStyle in die Klasse DebugDrawing hereingenommen.
 *
 * Revision 1.2  2001/12/12 17:34:26  risler
 * changed Point to Vector2
 *
 * Revision 1.1  2001/12/12 15:21:09  juengel
 * CDebugKeyToolBar in eigenes .cpp und .h File verlagert.
 *
 * Revision 1.7  2001/12/10 17:47:10  risler
 * change log added
 *
 */