/* 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 <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/poll.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>

#include <pthread.h>
#include <semaphore.h>

#include <stdio.h>

#include "serial.h"
#include "DataInterface.h"
#include "RobotViewData.h"
#include "SPOutDecoder.h"
#include "State.h"
#include "WaveServer.h"
#include "WLDecoder.h"
#include "../log_processing/shared/PacketDecoder.h"
#include "../log_processing/shared/BehaviorPacketDecoder.h"
#include "../log_processing/shared/LocalizationPacketDecoder.h"
#include "../log_processing/shared/MotionPacketDecoder.h"
#include "../log_processing/shared/VisionPacketDecoder.h"
#include "../log_processing/shared/VisionRawPacketDecoder.h"
#include "../log_processing/shared/VisionRLEPacketDecoder.h"
#include "../log_processing/shared/WorldModelPacketDecoder.h"
#include "../log_processing/shared/WMDebugPacketDecoder.h"
#include "../../agent/headers/CircBufPacket.h"
#include "../../agent/headers/WaveLAN.h"
#include "../../agent/Localization/GLocalization.h"
#include "../../agent/Motion/MotionInterface.h"

//#include "../util.h"
#include "agent/Vision/colors.h"

// This should be included for the defines...
// #include "../../agent/WLOut/WLOut.h"  

extern const int ImageXSize;
extern const int ImageYSize;

extern int streamWriteFd;
extern sem_t GuiDataSemaphore;

extern sem_t RunSemaphore;
extern bool Run;

void kickGUI(void) {
  if(write(streamWriteFd,"0",1)!=1) {
    printf("problem writing to GUI pipe\n");
  }
}

/* FIXME
void yuv2rgb(uchar* yuv, uchar* rgb)
{
  int y,u,v;

  y = yuv[0];
  u = yuv[1];
  v = yuv[2];

  rgb[2] = bound(y + u                     ,0,255);
  rgb[1] = bound((int)(y - 0.51*u - 0.19*v),0,255);
  rgb[0] = bound(y + v                     ,0,255);
}

void yuv2rgb(uchar *yuv, uchar *rgb, int width, int height)
{
  int size = width*height;
  int i;

  for(i=0; i<size; i++){
    yuv2rgb(&yuv[3*i], &rgb[3*i]);
  }
}
*/

rgb yuv2rgb(yuv c)
{
  rgb r;
  int y,u,v;

  y = c.y;
  u = c.u*2 - 255;
  v = c.v*2 - 255;

  r.red   = bound(y + u                     ,0,255);
  r.green = bound((int)(y - 0.51*u - 0.19*v),0,255);
  r.blue  = bound(y + v                     ,0,255);

  return(r);
}

void yuv2rgb(rgb *dest,yuv *src,int width,int height)
{
  int size = width*height;
  int i;

  for(i=0; i<size; i++){
    dest[i] = yuv2rgb(src[i]);
  }
}


void ProcessPacket(int robot_id,
		   WaveServerConfig *config,
		   RobotDataPacket *packet) {
  //printf("packet of type %x, ts=%lx, length %ld\n",packet->dataType,packet->timestamp,packet->length);

  static BehNames names;

  sem_wait(&GuiDataSemaphore);
  DataControl->allRobotViewData->robotsActive[robot_id] = true;
  sem_post(&GuiDataSemaphore);

  switch(packet->dataType) {
    case PacketDecoder::TextLead: 
      {
        packet->data[packet->length]='\x00'; 
        printf("%s",packet->data);
      }
    break;
    case PacketDecoder::VisionObjLead:
      {
        //printf("vis obj packet\n");
        sem_wait(&GuiDataSemaphore);
        DataControl->visDecoder->decode(&DataControl->allRobotViewData->robots[robot_id].visionObjs,packet);

        //VObject *obj;
        //for(int obj_idx=0; obj_idx<NUM_VISION_OBJECTS; obj_idx++) {
        //  obj = ((VObject *)&DataControl->allRobotViewData->robots[robot_id].visionObjs)+obj_idx;
        //  if(obj->confidence > 0.2) {
        //    printf("obj_idx %2d conf %8.3g loc (%8.3g,%8.3g,%8.3g) dist %8.3g edge %x\n",
        //           obj_idx,obj->confidence,
        //           obj->loc.x,obj->loc.y,obj->loc.z,
        //           obj->distance,obj->edge);
        //  }
        //}
	
        sem_post(&GuiDataSemaphore);
        kickGUI();
      }
      break;
  case PacketDecoder::ModelObjLead:
    {
      //printf("world model packet\n");
      sem_wait(&GuiDataSemaphore);
      DataControl->wmDecoder->decode(DataControl->allRobotViewData->robots[robot_id].worldModelObjs,DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid,packet);
      sem_post(&GuiDataSemaphore);
      kickGUI();
    }
    break;
  case PacketDecoder::TrackerLead:
    {
      sem_wait(&GuiDataSemaphore);
      DataControl->trackerDecoder->decode(DataControl->allRobotViewData->robots[robot_id].trackerObjs,DataControl->allRobotViewData->robots[robot_id].trackerObjsValid,packet);
      sem_post(&GuiDataSemaphore);
      kickGUI();
    }
    break;
  case PacketDecoder::WMDebugLead:
    {
      sem_wait(&GuiDataSemaphore);
      DataControl->wmDebugDecoder->decode(DataControl->allRobotViewData->robots[robot_id].wmDebugObjs,packet);
      sem_post(&GuiDataSemaphore);
      kickGUI();
    }
    break;
  case PacketDecoder::MotionUpdateLead:
    {
      //printf("motion update packet\n");
      MotionPacketDecoder decoder;
      Motion::MotionLocalizationUpdate mlu;
      if(decoder.decode(&mlu,packet)) {
	//printf("motion update: %lu pos_delta (%g,%g) heading_delta (%g,%g)\n",
	//       mlu.timestamp,mlu.pos_delta.x,mlu.pos_delta.y,mlu.heading_delta.x,mlu.heading_delta.y);
      } else {
	printf("problem decoding motion update packet\n");
      }
    }
    break;
  case PacketDecoder::LocalizationLead:
    {
      //printf("localization packet\n");
      GLocalizer posn;
      DataControl->locDecoder->decode(&posn,packet);
      sem_wait(&GuiDataSemaphore);
      DataControl->allRobotViewData->robots[robot_id].robotPosn.position = 
	posn.position;
      DataControl->allRobotViewData->robots[robot_id].robotPosn.heading = 
	posn.heading;
        sem_post(&GuiDataSemaphore);
        kickGUI();

        //printf("rb %g %g %g\n",
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
        //printf("rb dev maj %g min %g at ang %g, ang dev %g\n",
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sMaj,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sMin,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.thetaMaj,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.sMaj);
        //printf("rb dev (%g,%g) sxy %g, ang dev %g\n",
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sx,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sy,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.psxsy,
        //       DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.sMaj);
      }
    break;
    case PacketDecoder::VisionRLELead:
      {
        //printf("vision rle packet\n");
        sem_wait(&GuiDataSemaphore);
        DataControl->visRLEDecoder->decode(ImageXSize,ImageYSize,
                                           DataControl->allRobotViewData->robots[robot_id].rleImg,
                                           packet);
        sem_post(&GuiDataSemaphore);
        kickGUI();
      }
    break;
    case PacketDecoder::VisionRLE2Lead:
      {
        //printf("vision rle2 packet\n");
        sem_wait(&GuiDataSemaphore);
        if(!DataControl->visRLEDecoder->decode(DataControl->allRobotViewData->robots[robot_id].rleImageXSize,
                                               DataControl->allRobotViewData->robots[robot_id].rleImageYSize,
                                               DataControl->allRobotViewData->robots[robot_id].rleImg,
                                               MaxImageXSize,MaxImageYSize,
                                               packet)){
          printf("problem decoding rle2 packet\n");
        }
        sem_post(&GuiDataSemaphore);
        kickGUI();
      }
    break;

    case PacketDecoder::VisionRawLead:
    case PacketDecoder::VisionRaw2Lead:
      {
        static int nImages = 0;
        printf("vision raw2 packet\n");
  
        //sem_wait(&GuiDataSemaphore);
        VisionRawPacketDecoder d = *(DataControl->visRawDecoder);

        if (!d.decode(DataControl->allRobotViewData->robots[robot_id].rawImageXSize,
                      DataControl->allRobotViewData->robots[robot_id].rawImageYSize,
                      DataControl->allRobotViewData->robots[robot_id].rawImg,
                      MaxImageXSize,
                      MaxImageYSize,
                      packet)) {
          printf("Problem decoding raw2 packet\n");
        }
        else {
          int xsize = DataControl->allRobotViewData->robots[robot_id].rawImageXSize;
          int ysize = DataControl->allRobotViewData->robots[robot_id].rawImageYSize;
          uchar* img = DataControl->allRobotViewData->robots[robot_id].rawImg;

          int nBytes = xsize * ysize * 3;

          double angles[5];
          char filename[64];

          // Write out the vision info file
          if(d.decode(angles,packet)){
            sprintf(filename, "i%04d.inf", nImages);
            printf("Writing to %s\n", filename);
            FILE* fp = fopen(filename, "w");
            fprintf(fp,"%f %f %f %f %f\n",angles[0],angles[1],angles[2],angles[3],angles[4]);
            fclose(fp);
            printf("done\n");            
          }

          // Write out the YUV data
          sprintf(filename, "i%04d.ppm", nImages);
          printf("Writing to %s\n", filename);
          FILE* fp = fopen(filename, "w");
          fprintf(fp, "P6\n%d %d\n255\n", xsize, ysize);
          fwrite(img, nBytes, 1, fp);
          fclose(fp);
          printf("done\n");

          // Convert to RGB and write out the RGB data
          rgb* buffer = (rgb*) malloc(sizeof(rgb) * xsize * ysize);          
          yuv2rgb(buffer, (yuv*) img, xsize, ysize);
          sprintf(filename, "r%04d.ppm", nImages);
          printf("Writing to %s\n", filename);
          fp = fopen(filename, "w");
          fprintf(fp, "P6\n%d %d\n255\n", xsize, ysize);
          fwrite(buffer, nBytes, 1, fp);
          fclose(fp);
          free(buffer);
          printf("done\n");
          
          nImages++;
          //sem_post(&GuiDataSemaphore);
        }
      }
    break;


    case PacketDecoder::VisionAvgColorLead:
      {
        //printf("vision avg color packet\n");
        float avg_color[3];
        uchar *buf;
        buf = packet->data;
        avg_color[0] = PacketDecoder::grab<float>(&buf);
        avg_color[1] = PacketDecoder::grab<float>(&buf);
        avg_color[2] = PacketDecoder::grab<float>(&buf);
        printf("vis:avg color was %10g %10g %10g\n",avg_color[0],avg_color[1],avg_color[2]);
      }
    break;
    case PacketDecoder::VisionColorAreaLead:
      {
        //printf("vision color area packet\n");
        uchar num_colors;
        ulong color_area;

        uchar *buf;
        buf = packet->data;

        num_colors = PacketDecoder::grab<uchar>(&buf);
        printf("vis:color area, %d colors=[",num_colors);
        for(int color_idx=0; color_idx<num_colors; color_idx++) {
          color_area = PacketDecoder::grab<ulong>(&buf);
          printf("%lu,",color_area);
        }
        printf("]\n");
      }
    break;
    case PacketDecoder::VisionRadialMapLead:
      {
        //printf("vis radial map packet, time %lu\n",packet->timestamp);
        //WLDecoder::dumpBinary(packet->data,packet->length);

        sem_wait(&GuiDataSemaphore);

        DataControl->visDecoder->decode(&DataControl->allRobotViewData->robots[robot_id].visMap,packet);
	//        RadialObjectMap &vis_map=DataControl->allRobotViewData->robots[robot_id].visMap;

        //pprintf(TextOutputStream,"%17s %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s, "
        //        "%1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s\n","",
        //        0,"VisStart",1,"VisEnd",2,"ClearStart",3,"ClearEnd",4,"Wall",5,"Robot",
        //        6,"RedRobot",7,"BlueRobot",8,"Ball",9,"CyanGoal",10,"YellowGoal",11,"Stripe");
        //for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
        //  pprintf(TextOutputStream,"ang %5.2f idx %3d ",RadialObjectMap::idxToAngle(ang_idx),ang_idx);
        //  for(int obj_type=0; obj_type<NUM_ROBJECTS; obj_type++) {
        //    double dist = vis_map.robjects[obj_type][ang_idx].distance;
        //    if(dist < HUGE_VAL && dist > 1e48)
        //      pprintf(TextOutputStream,"c%4.2f d%8s, ",
        //              vis_map.robjects[obj_type][ang_idx].confidence,
        //              "BIG");
        //    else
        //      pprintf(TextOutputStream,"c%4.2f d%8.f, ",
        //              vis_map.robjects[obj_type][ang_idx].confidence,
        //              dist);
        //  }
        //  pprintf(TextOutputStream,"\n");
        //}

        sem_post(&GuiDataSemaphore);
        kickGUI();
      }
    break;
    case PacketDecoder::BehaviorNamesLead:
      {
        if(!DataControl->behNamesDecoder->decode(&names,packet))
          printf("problem decoding behavior names packet\n");
        //names.dump();
      }
      break;
    case PacketDecoder::BehaviorTraceLead:
      {
        if(!DataControl->behTraceDecoder->decode(packet,&names))
          printf("problem decoding behavior trace packet\n");
      }
      break;
    default:
      printf("unrecognized packet type %x\n",packet->dataType);
      break;
  }
}

void *SerialProcessor(void *data) {

  char *device="/dev/ttyS0";
  int baud=19200;
  SPOutDecoder decoder;
  serial port(device, baud);

  if(fcntl(port.fd,F_SETFL,O_NONBLOCK)==-1)
    perror("Problem making serial port non-blocking");

  decoder.setCallback(NULL,(SPOutDecoder::PacketCallback)ProcessPacket);
  decoder.setRobotId(0);

  struct pollfd poll_fds[1];
  poll_fds[0].fd = port.fd;
  poll_fds[0].events = POLLIN | POLLPRI;

  bool run;
  run = Run;
  while(run) {
    poll(poll_fds,sizeof(poll_fds)/sizeof(poll_fds[0]),10);
    if(poll_fds[0].revents & (POLLIN | POLLPRI))
      decoder.processInput(&port);

    sem_wait(&RunSemaphore);
    run = Run;
    sem_post(&RunSemaphore);
  }

  return NULL;
}

//===========================================================================
// These values are no longer needed

#define DEFAULT_CLIENT "127.0.0.1"
#define DEFAULT_PORT 2048
#define DEFAULT_OUT_PORT 5046

static const char *hosts[NumAcceptingSockets]={DEFAULT_CLIENT,DEFAULT_CLIENT};
static int ports[NumAcceptingSockets]={DEFAULT_PORT,DEFAULT_OUT_PORT};

//===========================================================================

bool SendTestMessage(WaveServerConfig *config) {
  static int count=0;

  static int threshold=0;
  threshold = (threshold+1)%3;

  char buf[256];
  NetMsgHeader *header;

  header = (NetMsgHeader *)(buf);
  header->totalLength       = 1+sizeof(NetMsgHeader);
  header->senderID          = NetMsgHeader::Linux;
  header->senderSubsystem   = NetMsgHeader::Sub_Main;
  header->receiverID        = 2;
  header->receiverSubsystem = NetMsgHeader::Sub_Main;
  header->messageType       = NetMsgHeader::MsgText;
  //header->messageType       = NetMsgHeader::MsgThresholdSelect;
  *(buf+sizeof(NetMsgHeader))=(uchar)threshold;

  if((++count) % 100 == 0) {
    return SendMessage(&config->server,header);
  } else {
    return true;
  }
}

//===========================================================================
// This version requires the use of the 'wlan_connect' utility
void StartServer(pthread_t *server_thread,
		 WaveServerConfig *wave_server_config,
		 sem_t *run_semaphore,bool *run) {
  wave_server_config->packetCallback  = ProcessPacket;
  wave_server_config->outputProcessor = NULL;
  wave_server_config->outputCallback  = SendTestMessage;
  wave_server_config->hosts           = hosts;
  wave_server_config->ports           = ports;
  wave_server_config->RunSemaphore    = run_semaphore;
  wave_server_config->Run             = run;
  wave_server_config->runOnce         = false;
  wave_server_config->userData        = NULL;

  if(pthread_create(server_thread,
		    NULL,
		    (void *(*) (void *))SocketServer,
		    (void *)wave_server_config)!=0) {
    perror("error creating socket thread\n");
    exit(1);
  }
}

//===========================================================================
// This version connects directly to the AIBO

void StartServer2(pthread_t *server_thread,
		  WaveServerConfig *wave_server_config,
		  sem_t *run_semaphore,bool *run,
		  const char * dog_host_name,
		  int dog_port_number) {

  // Reset the input values
  hosts[0]=dog_host_name;
  hosts[1]=NULL;
  ports[0]=dog_port_number; 
  ports[1]=-1;

  wave_server_config->packetCallback  = ProcessPacket;
  wave_server_config->outputProcessor = NULL;
  wave_server_config->outputCallback  = SendTestMessage;
  wave_server_config->hosts           = hosts;
  wave_server_config->ports           = ports;
  wave_server_config->RunSemaphore    = run_semaphore;
  wave_server_config->Run             = run;
  wave_server_config->runOnce         = false;
  wave_server_config->userData        = NULL;

  if(pthread_create(server_thread,
		    NULL,
		    (void *(*) (void *))SocketServer2,
		    (void *)wave_server_config)!=0) {
    perror("error creating socket thread\n");
    exit(1);
  }
}