/** 
* @file Platform/Win32/SimRobot/RobotConsole.cpp
*
* Implementation of RobotConsole.
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
*/

#include "RobotConsole.h"
#include "ConsoleRoboCupCtrl.h"
#include "Tools/FieldDimensions.h"
#include "RobotControl/Visualization/DrawingMethods.h"
#include "RobotControl/Visualization/ImageMethods.h"
#include "RobotControl/Visualization/PaintMethodsWin32.h"
#include "DataTypes/Motion/HeadMotionRequest.h"
#include "DataTypes/Motion/MotionRequest.h"
#include "DataTypes/Perception/JPEGImage.h"
#include "DataTypes/QueueFillRequest.h"
#include "View.h"

class ImageView : public DIRECTVIEW
{
  private:
    RobotConsole& console;
    const Image& image;
    const ColorTable64& colorTable;
    const DebugDrawing* drawings;
    const Drawings::ImageDrawing* layers;
    const int numOfLayers;
  
  public:
    ImageView(RobotConsole& c,const char* name,const Image& i,const ColorTable64& t,
      const DebugDrawing* d,const Drawings::ImageDrawing* l,int n)
      : DIRECTVIEW(name),
      console(c),
      image(i), 
      colorTable(t),
      drawings(d),
      layers(l),
      numOfLayers(n) {}
    void DrawView(CDC& dc);
};

void ImageView::DrawView(CDC& dc)
{
  SYNC_WITH(console);
  dc.SetWindowExt(image.width,image.height);
  for(int i = 0; i < numOfLayers; ++i)
  {
    if(layers[i] == Drawings::camera)
      ImageMethods::paintImage2CDCAsYUV(dc,image,CRect(0,0,176,144));
    else if(layers[i] == Drawings::colorClassImage)
    {
      ColorClassImage colorClassImage;
      colorTable.generateColorClassImage(image,colorClassImage);
      ImageMethods::paintColorClassImage2CDC(dc,colorClassImage,CRect(0,0,176,144));
    }
    else
      PaintMethodsWin32::paintDebugDrawingToCDC(drawings[layers[i]],dc);
  }
}

class FieldView : public DIRECTVIEW
{
  private:
    RobotConsole& console;
    const DebugDrawing* drawings;
    const Drawings::FieldDrawing* layers;
    const int numOfLayers;
    const Player::teamColor& ownColor;
  
  public:
    FieldView(RobotConsole& c,const char* name,const DebugDrawing* d,
      const Drawings::FieldDrawing* l,int n,Player::teamColor& color)
      : DIRECTVIEW(name),
      console(c),
      drawings(d),
      layers(l),
      numOfLayers(n),
      ownColor(color) {}
    void DrawView(CDC& dc);
};

void FieldView::DrawView(CDC& dc)
{
  SYNC_WITH(console);
  if(GetDeviceCaps(dc.m_hDC,TECHNOLOGY) != DT_METAFILE)
  {
    CSize size = dc.GetViewportExt();
    dc.SetMapMode(MM_ISOTROPIC);
    dc.SetViewportExt(size);
    dc.SetViewportOrg(size.cx/2,size.cy/2);
  }
  else
    dc.SetWindowOrg(int(-xPosOpponentGoal - flagRadius),
                    int(yPosLeftFlags + flagRadius));
  dc.SetWindowExt((int(xPosOpponentGoal + flagRadius) * 2),
                  int((yPosLeftFlags + flagRadius) * -2));
  for(int i = 0; i < numOfLayers; ++i)
  {
    if(layers[i] == Drawings::fieldPolygons)
    {
      DebugDrawing field;
      DrawingMethods::paintFieldPolygons(field,ownColor);
      PaintMethodsWin32::paintDebugDrawingToCDC(field,dc);
    }
    else if(layers[i] == Drawings::fieldLines)
    {
      DebugDrawing field;
      DrawingMethods::paintFieldLines(field);
      PaintMethodsWin32::paintDebugDrawingToCDC(field,dc);
    }
    else
    {
      PaintMethodsWin32::paintDebugDrawingToCDC(drawings[layers[i]],dc);
    }
  }
}

RobotConsole::RobotConsole(MessageQueue& in,MessageQueue& out)
: Process(in,out),
  debugIn(in), debugOut(out),
  logPlayer(out),
  sr(true)
{
  // this is a hack: call global functions to get parameters
  ctrl = (ConsoleRoboCupCtrl*) RoboCupCtrl::getController();
  STRING robotName = RoboCupCtrl::getRobotName();

  teamColor = Player::red; // will be replaced later
  InBinaryFile("coltable.c64") >> colorTable;

  for(List<ImageViewBase*>::Pos iv = ImageViewBase::list.getFirst(); iv; ++iv)
  {
    const char* name;
    Drawings::ImageDrawing* layers;
    int numOfLayers;
    ImageViewBase::list[iv]->getInfo(name,layers,numOfLayers);
    ctrl->AddView(new ImageView(*this,STRING("robot ") + robotName.ButFirst(robotName.Pos('.')+1) + " " + name,image,colorTable,imageDrawings,layers,numOfLayers));
  }
  
  for(List<FieldViewBase*>::Pos fv = FieldViewBase::list.getFirst(); fv; ++fv)
  {
    const char* name;
    Drawings::FieldDrawing* layers;
    int numOfLayers;
    FieldViewBase::list[fv]->getInfo(name,layers,numOfLayers);
    ctrl->AddView(new FieldView(*this,STRING("robot ") + robotName.ButFirst(robotName.Pos('.')+1) + " " + name,fieldDrawings,layers,
                                numOfLayers,teamColor));
  }
  
  printMessages = true;
  calculateImage = true;
}

bool RobotConsole::handleMessage(InMessage& message)
{
  // Only one thread can access *this now.
  SYNC;

  logPlayer.handleMessage(message);
  message.resetReadPosition();

  switch(message.getMessageID())
  {
    case idText:
    {
      char* buffer = new char[message.getMessageSize() * 2 + 1];
      message.text.readAll(buffer);
      if(printMessages)
        ctrl->printLn(buffer);
      delete [] buffer;
      return true;
    }
    case idImage:
      message.bin >> image;
      return true;
    case idJPEGImage:
    {
      JPEGImage jpi;
      message.bin >> jpi;
      jpi.toImage(image);
      return true;
    }
    case idSensorData:
      message.bin >> sensorData;
      return true;
    case idDebugDrawing:
    {
      DebugDrawing debugDrawing;
      message.bin >> debugDrawing;
      if(debugDrawing.typeOfDrawing == Drawings::drawingOnImage)
        imageDrawings[debugDrawing.imageDrawingID] = debugDrawing;
      else if (debugDrawing.typeOfDrawing == Drawings::drawingOnField)
        fieldDrawings[debugDrawing.fieldDrawingID] = debugDrawing;
      return true;
    }
    case idDebugDrawing2:
    {
      char shapeType,
           id,
           typeOfDrawing;
      message.bin >> shapeType >> id >> typeOfDrawing;
 
      if((Drawings::TypeOfDrawing)typeOfDrawing == Drawings::drawingOnImage)
        id += Drawings::numberOfFieldDrawings;

      incompleteDrawings[id].addShapeFromQueue(message, (Drawings::ShapeType)shapeType);
      
      return true;
    }
    case idDebugDrawingFinished:
    {
      char id, typeOfDrawing;
      message.bin >> id;
      message.bin >> typeOfDrawing;
      if((Drawings::TypeOfDrawing)typeOfDrawing == Drawings::drawingOnField)
      {
        incompleteDrawings[id].fieldDrawingID = (Drawings::FieldDrawing)id;
        incompleteDrawings[id].typeOfDrawing = Drawings::drawingOnField;
        fieldDrawings[id] = incompleteDrawings[id];
        incompleteDrawings[id].reset();
      }
      else
      {
        incompleteDrawings[id + Drawings::numberOfFieldDrawings].imageDrawingID = (Drawings::ImageDrawing)id;
        incompleteDrawings[id + Drawings::numberOfFieldDrawings].typeOfDrawing = Drawings::drawingOnImage;
        imageDrawings[id] = incompleteDrawings[id + Drawings::numberOfFieldDrawings];
        incompleteDrawings[id + Drawings::numberOfFieldDrawings].reset();
      }
      return true;
    }
  }
  return false;
}

void RobotConsole::handleConsole(InConfigMemory& stream)
{
  char command[80];
  stream >> command;
  bool result = false;
  if(!strcmp(command,"msg"))
    result = msg(stream);
  else if(!strcmp(command,"ci"))
    result = calcImage(stream);
  else if(!strcmp(command,"dk"))
    result = debugKey(stream);
  else if(!strcmp(command,"hmr"))
    result = sendHeadMotionRequest(stream);
  else if(!strcmp(command,"log"))
    result = log(stream);
  else if(!strcmp(command,"mr"))
    result = sendMotionRequest(stream);
  else if(!strcmp(command,"qfr"))
    result = queueFillRequest(stream);
  else if(!strcmp(command,"tr"))
    result = sendTailRequest(stream);
  if(!result)
    ctrl->printLn("Syntax Error");
}

bool RobotConsole::msg(InConfigMemory& stream)
{
  char state[80];
  stream >> state;
  if(!strcmp(state,"off"))
  {
    printMessages = false;
    return true;
  }
  else if(!strcmp(state,"on"))
  {
    printMessages = true;
    return true;
  }
  return false;
}

bool RobotConsole::calcImage(InConfigMemory& stream)
{
  char state[80];
  stream >> state;
  if(!strcmp(state,"off"))
  {
    calculateImage = false;
    return true;
  }
  else if(!strcmp(state,"on"))
  {
    calculateImage = true;
    return true;
  }
  return false;
}

bool RobotConsole::debugKey(InConfigMemory& stream)
{
  char debugKey[80],
       state[80];
  stream >> debugKey >> state;
  if(!strcmp(debugKey,"?"))
  {
    for(int i = 0; i < DebugKeyTable::numOfDebugKeys; ++i)
      ctrl->print(STRING(DebugKeyTable::getDebugKeyName(DebugKeyTable::debugKeyID(i))) + " ");
    ctrl->printLn("");
    return true;
  }
  else
    for(int i = 0; i < DebugKeyTable::numOfDebugKeys; ++i)
      if(!strcmp(debugKey,DebugKeyTable::getDebugKeyName(DebugKeyTable::debugKeyID(i))))
        if(!strcmp(state,"off"))
        {
          SYNC;
          dkt.set(DebugKeyTable::debugKeyID(i),DebugKey::disabled);
          debugOut.out.bin << dkt;
          debugOut.out.finishMessage(idDebugKeyTable);
          return true;
        }
        else if(!strcmp(state,"on"))
        {
          SYNC;
          dkt.set(DebugKeyTable::debugKeyID(i),DebugKey::always);
          debugOut.out.bin << dkt;
          debugOut.out.finishMessage(idDebugKeyTable);
          return true;
        }
        else if(!strcmp(state,"every"))
        {
          char ms[80];
          int times;
          stream >> times >> ms;
          if(times)
          {
            SYNC;
            if(!strcmp(ms,"ms"))
              dkt.set(DebugKeyTable::debugKeyID(i),DebugKey::every_n_ms,times);
            else
              dkt.set(DebugKeyTable::debugKeyID(i),DebugKey::every_n_times,times);
            debugOut.out.bin << dkt;
            debugOut.out.finishMessage(idDebugKeyTable);
            return true;
          }
        }
        else if(atoi(state))
        {
          SYNC;
          dkt.set(DebugKeyTable::debugKeyID(i),DebugKey::n_times,atoi(state));
          debugOut.out.bin << dkt;
          debugOut.out.finishMessage(idDebugKeyTable);
          return true;
        }
  return false;
}

bool RobotConsole::sendHeadMotionRequest(InConfigMemory& stream)
{
  SYNC;
  double tilt = 0,
         pan = 0,
         roll = 0,
         mouth = 0;
  stream >> tilt >> pan >> roll >> mouth;
  headMotionRequest.tilt = toMicroRad(fromDegrees(tilt));
  headMotionRequest.pan = toMicroRad(fromDegrees(pan));
  headMotionRequest.roll = toMicroRad(fromDegrees(roll));
  headMotionRequest.mouth = toMicroRad(fromDegrees(mouth));
  debugOut.out.bin << headMotionRequest;
  debugOut.out.finishMessage(idHeadMotionRequest);
  return true;
}

bool RobotConsole::log(InConfigMemory& stream)
{
  char command[80];
  stream >> command;
  if(!strcmp("start",command) || !strcmp("stop",command))
  {
    logPlayer.record();
    return true;
  }
  else if(!strcmp("clear",command))
  {
    logPlayer._new();
    return true;
  }
  else if(!strcmp("save",command))
  {
    stream >> command;
    STRING name(command);
    if(name.Length() == 0)
      return false;
    else 
    {
      if(name.Pos('.') == name.Length())
        name = name + ".log";
      return logPlayer.save(name);
    }
  }
  else
    return false;
}

bool RobotConsole::queueFillRequest(InConfigMemory& stream)
{
  char request[80];
  stream >> request;
  QueueFillRequest qfr;
  if(!strcmp("queue",request))
    qfr.mode = QueueFillRequest::immediateReadWrite;
  else if(!strcmp("replace",request))
    qfr.mode = QueueFillRequest::overwriteOlder;
  else if(!strcmp("collect",request))
  {
    qfr.mode = QueueFillRequest::collectNSeconds;
    stream >> qfr.seconds;
    if(!qfr.seconds)
      return false;
  }
  else if(!strcmp("save",request))
  {
    qfr.mode = QueueFillRequest::toStickNSeconds;
    stream >> qfr.seconds;
    if(!qfr.seconds)
      return false;
  }
  else
    return false;
  debugOut.out.bin << qfr;
  debugOut.out.finishMessage(idQueueFillRequest);
  return true;
}

bool RobotConsole::sendMotionRequest(InConfigMemory& stream)
{
  char request[80];
  stream >> request;
  if(!strcmp("?",request))
  {
    ctrl->print("getup ");
    for(int i = 0; i < MotionRequest::numOfWalkType; ++i) 
      ctrl->print(STRING(MotionRequest::getWalkTypeName(MotionRequest::WalkType(i))) + " ");
    for(i = 0; i < MotionRequest::numOfSpecialAction; ++i) 
      ctrl->print(STRING(MotionRequest::getSpecialActionName(MotionRequest::SpecialActionID(i))) + " ");
    ctrl->printLn("");
    return true;
  }
  else if(!strcmp("getup",request))
  {
    SYNC;
    motionRequest.motionType = MotionRequest::getup;
    debugOut.out.bin << motionRequest;
    debugOut.out.finishMessage(idMotionRequest);
    return true;
  }
  else
  {
    for(int i = 0; i < MotionRequest::numOfWalkType; ++i) 
      if(!strcmp(request,MotionRequest::getWalkTypeName(MotionRequest::WalkType(i))))
      {
        SYNC;
        motionRequest.motionType = MotionRequest::walk;
        motionRequest.walkType = MotionRequest::WalkType(i);
        double rotation;
        stream >> motionRequest.walkParams.translation.x 
          >> motionRequest.walkParams.translation.y
          >> rotation;
        motionRequest.walkParams.rotation = rotation * pi / 180;
        debugOut.out.bin << motionRequest;
        debugOut.out.finishMessage(idMotionRequest);
        return true;
      }
    for(i = 0; i < MotionRequest::numOfSpecialAction; ++i) 
      if(!strcmp(request,MotionRequest::getSpecialActionName(MotionRequest::SpecialActionID(i))))
      {
        SYNC;
        motionRequest.motionType = MotionRequest::specialAction;
        motionRequest.specialActionType = MotionRequest::SpecialActionID(i);
        debugOut.out.bin << motionRequest;
        debugOut.out.finishMessage(idMotionRequest);
        return true;
      }
  }
  return false;
}

bool RobotConsole::sendTailRequest(InConfigMemory& stream)
{
  char request[80];
  stream >> request;
  if(!strcmp("?",request))
  {
    for(int i = 0; i < MotionRequest::numOfTailRequests; ++i) 
      ctrl->print(STRING(MotionRequest::getTailRequestName(MotionRequest::TailRequest(i))) + " ");
    ctrl->printLn("");
    return true;
  }
  else
  {
    for(int i = 0; i < MotionRequest::numOfTailRequests; ++i) 
      if(!strcmp(request,MotionRequest::getTailRequestName(MotionRequest::TailRequest(i))))
      {
        SYNC;
        motionRequest.tailRequest = MotionRequest::TailRequest(i);
        debugOut.out.bin << motionRequest;
        debugOut.out.finishMessage(idMotionRequest);
        return true;
      }
  }
  return false;
}

IMAGE_VIEW(image)
  Drawings::camera, Drawings::image
END_VIEW(image)
  
IMAGE_VIEW(segmentedImage)
  Drawings::colorClassImage, Drawings::image
END_VIEW(segmentedImage)
  
FIELD_VIEW(field)
  Drawings::fieldPolygons, Drawings::fieldLines, Drawings::field
END_VIEW(field)

/*
* Change log :
*
* $Log: RobotConsole.cpp,v $
* Revision 1.16  2003/01/15 13:47:28  roefer
* Progress in LinesSelfLocator, new debug drawing
*
* Revision 1.15  2002/12/16 23:50:44  roefer
* New command qfr added
*
* Revision 1.14  2002/12/14 19:44:55  roefer
* Lines drawing in image coordinates removed
*
* Revision 1.13  2002/12/14 17:56:08  roefer
* Names of drawing functions changed
*
* Revision 1.12  2002/12/07 18:43:55  roefer
* SimGT2003 log file handling added
*
* Revision 1.11  2002/12/06 00:44:56  roefer
* teamColor transfered, command prc removed
*
* Revision 1.10  2002/11/28 09:53:49  roefer
* Additional drawing bug fixed
*
* Revision 1.9  2002/11/26 10:23:57  roefer
* Minor debug drawing errors corrected
*
* Revision 1.8  2002/11/12 10:49:01  juengel
* New debug drawing macros - second step
* -moved /Tools/Debugging/PaintMethods.h and . cpp
*  to /Visualization/DrawingMethods.h and .cpp
* -moved DebugDrawing.h and .cpp from /Tools/Debugging/
*  to /Visualization
*
* Revision 1.7  2002/10/11 17:00:32  roefer
* Tail request command added
*
* Revision 1.6  2002/10/11 13:54:43  roefer
* JPEGImage added
*
* Revision 1.5  2002/10/11 10:13:53  roefer
* Solution request corrected
*
* Revision 1.4  2002/10/01 11:14:34  loetzsch
* Redesigned DebugKey and DebugKeyTable
*
* Revision 1.3  2002/09/22 18:40:55  risler
* added new math functions, removed GTMath library
*
* Revision 1.2  2002/09/18 16:35:12  loetzsch
* made GT2003 compilable,
* rechanged message ids
*
* Revision 1.1  2002/09/10 15:40:05  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/09/08 14:55:38  roefer
* Player role and color setable from console
*
* Revision 1.5  2002/08/08 17:09:03  roefer
* SimGT2002 scripting enhanced
*
* Revision 1.4  2002/08/04 19:42:09  roefer
* SimGT2002 receives player config
*
* Revision 1.3  2002/08/04 17:53:18  roefer
* SimGT2002 connection to physical robots added
*
* Revision 1.2  2002/08/01 12:52:26  roefer
* RouterCtrl and TcpConnection added
*
* Revision 1.1  2002/07/28 21:23:36  roefer
* SimGT2002 OVirtualRobotComm divided into two classes
*
*
*/