//----------------------------------------------------------------------
//               The Motion Strategy Library (MSL)
//----------------------------------------------------------------------
//
// Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
// All Rights Reserved.
// 
// Permission to use, copy, and distribute this software and its 
// documentation is hereby granted free of charge, provided that 
// (1) it is not a component of a commercial product, and 
// (2) this notice appears in all copies of the software and
//     related documentation. 
// 
// Iowa State University and the author make no representations
// about the suitability or fitness of this software for any purpose.  
// It is provided "as is" without express or implied warranty.
//----------------------------------------------------------------------

#include <math.h>
#include <stdio.h>
#include <fstream.h>

#include "guiplanner.h"
#include "defs.h"

#include "pixmaps/msl.xpm"

#include <LEDA/REDEFINE_NAMES.h>

//////////////////////////////////////////////////////////////////////////
//
//  GuiPlanner class
//
//////////////////////////////////////////////////////////////////////////

GuiPlanner::GuiPlanner(Render *render, Planner *planner):Gui(render) {
  Pl = planner;

  LineWidth = 1.0;
  PSLineWidth = 1.0;
  DrawIndexX = 0;
  DrawIndexY = 1;

  PlanCtlWindowOn = false;

  CreateMenuWindow();
}



void GuiPlanner::Init() {
  list<vector> tpath;
  
  // Read the planner type, if exists
  if (is_file(Pl->P->FilePath + "RRT"))
    ButtonHandle(201);
  if (is_file(Pl->P->FilePath + "RRTGoalBias"))
    ButtonHandle(202);
  if (is_file(Pl->P->FilePath + "RRTGoalPull"))
    ButtonHandle(203);
  if (is_file(Pl->P->FilePath + "RRTCon"))
    ButtonHandle(204);
  if (is_file(Pl->P->FilePath + "RRTPolar"))
    ButtonHandle(208);
  if (is_file(Pl->P->FilePath + "RRTHull"))
    ButtonHandle(209);
  if (is_file(Pl->P->FilePath + "RRTStar"))
    ButtonHandle(212);
  if (is_file(Pl->P->FilePath + "RRTGoalZoom"))
    ButtonHandle(207);
  if (is_file(Pl->P->FilePath + "RRTDual")) 
    ButtonHandle(205);
  if (is_file(Pl->P->FilePath + "RRTExtExt"))
    ButtonHandle(206);
  if (is_file(Pl->P->FilePath + "RRTExtCon"))
    ButtonHandle(210);
  if (is_file(Pl->P->FilePath + "RRTConCon"))
    ButtonHandle(211);
  if (is_file(Pl->P->FilePath + "PlannerHsu"))
    ButtonHandle(213);
  if (is_file(Pl->P->FilePath + "RRTNaive"))
    ButtonHandle(214);    
  if (is_file(Pl->P->FilePath + "PRM"))
    ButtonHandle(230);    
  if (is_file(Pl->P->FilePath + "FDP"))
    ButtonHandle(240);    
  if (is_file(Pl->P->FilePath + "FDPStar"))
    ButtonHandle(241);    
  if (is_file(Pl->P->FilePath + "FDPBestFirst"))
    ButtonHandle(242);    
  if (is_file(Pl->P->FilePath + "FDPBi"))
    ButtonHandle(243);    
  
  Gui::Init();

  // Set up to animate a single frame initially
  tpath.clear();
  //tpath.push(Pl->P->GoalState);
  tpath.push(Pl->P->InitialState);
  R->MakeAnimationFrames(tpath, 1.0);   
  R->AnimationActive = true;
}


void GuiPlanner::CreateMenuWindow() {
  menu *m2,*m3,*m4,*mm2;
 
  W = new window(500,62,"MSL Planner, Iowa State University, 1998-2001");
  W->set_icon_pixrect(W->create_pixrect(msl_xpm));
  W->buttons_per_line(5);
  m2 = new menu();
  m3 = new menu();
  m4 = new menu();
  mm2 = new menu();

  W->button("Construct",22);
  W->button("Plan",23);
  W->button("Clear",24);
  W->button("PlanCtl",50);
  W->button("RenderCtl",60);

  mm2->button("RRT",201);
  mm2->button("RRTGoalBias",202);
  mm2->button("RRTGoalPull",203);
  mm2->button("RRTCon",204);
  mm2->button("RRTPolar",208);
  mm2->button("RRTHull",209);
  mm2->button("RRTStar",212);
  mm2->button("RRTGoalZoom",207);
  mm2->button("RRTDual",205);
  mm2->button("RRTExtExt",206);
  mm2->button("RRTExtCon",210);
  mm2->button("RRTConCon",211);
  mm2->button("PlannerHsu",213);
  mm2->button("RRTNaive",214);
  mm2->button("PRM",230);
  mm2->button("FDP",240);
  mm2->button("FDPStar",241);
  mm2->button("FDPBestFirst",242);
  mm2->button("FDPBi",243);
  W->button("Planner",*mm2);

  m3->button("2D Graph Projection",31);
  //m3->button("3D Graph Projection",32);
  //m3->button("Voronoi Diagram",33);
  W->button("Display",*m3);

  m4->button("2D Graph Proj. to PS",41);
  //m4->button("Voronoi Diagram to PS",42);
  m4->button("Write Graph(s)",44);
  m4->button("Read Graph(s)",45);
  m4->button("Write Path",48);
  m4->button("Read Path",49);
  m4->button("Write Animation Frames",46);
  m4->button("Read Animation Frames",47);
  W->button("File",*m4);

  W->button("Exit",-1,exit);

  W->display();
}


void GuiPlanner::ResetPlanner()
{
  R->SetScene(new Scene(Pl->P,FilePath));
  Pl->Reset();
}


void GuiPlanner::ButtonHandle(int b){
  //cout << "button " << b << "\n";
  char *icon;

  switch (b) {
  case 12: cout << "Read Inputs\n";
    //ReadInputs();
    break;
  case 13: cout << "Read Initial State\n";
    //ReadInitialState();
    break;
  case 14: cout << "Read Goal State\n";
    //ReadGoalState();
    break;
  case 15: cout << "Change File Path\n";
    //NewPathName();
    break;
  case 22: cout << "Construct\n";
    Pl->Construct();
    //U.DrawGraphs();
    break;
  case 23: cout << "Plan\n";
    if (Pl->Plan()) {
      cout << "Making Animation Frames\n";
      // Make the animation frames
      if ((Pl->Path.length() >= 2)||
	  (R->FrameList.length() > 0))
	{
	  R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
	}
    }
    //U.DrawPath();
    //U.DrawGraphs();
    break;
  case 24: cout << "Clear Graphs\n";
    ResetPlanner();
    break;
  case 31: cout << "2D Graph Projection\n";
    DrawGraphs();
    break;
  case 32: cout << "3D Graph Projection\n";
    //DrawRRTNodes();
    break;
  case 33: cout << "Draw Voronoi Diagram\n";
    //DrawVoronoi();
    break;
  case 41: cout << "Write 2D Graph Projection to PS\n";
    PSDrawGraphs();
    break;
  case 42: cout << "Write Voronoi Edges to PS\n";
    //PSDrawVoronoi();
    break;
  case 43: cout << "Write Path to PS\n";
    //PSDrawPath();
    break;
  case 44: cout << "Write Graph(s)\n";
    WriteGraphs();
    break;
  case 45: cout << "Read Graph(s)\n";
    ReadGraphs();
    break;
  case 46: cout << "Write Animation Frames\n";
    WriteAnimationFrames();
    break;
  case 47: cout << "Read Animation Frames\n";
    ReadAnimationFrames();      
    break;
  case 48: cout << "Write Path\n";
    WritePath();
    break;
  case 49: cout << "Read Path\n";
    ReadPath();      
    break;
  case 50:
    cout << "Toggle Planner Control Window\n";
    PlanCtlWindowOn = !PlanCtlWindowOn;
    if (PlanCtlWindowOn) {
      PlanCtlWindow = new window(200,155,"MSL: Planner Control Window");
      icon = PlanCtlWindow->create_pixrect(msl_xpm);
      PlanCtlWindow->set_icon_pixrect(icon);
      PlanCtlWindow->double_item("PlannerDeltaT",Pl->PlannerDeltaT);
      PlanCtlWindow->int_item("NumNodes",Pl->NumNodes);
      PlanCtlWindow->int_item("DrawIndexX",DrawIndexX);
      PlanCtlWindow->int_item("DrawIndexY",DrawIndexY);
      PlanCtlWindow->double_item("LineWidth",LineWidth);
      PlanCtlWindow->double_item("PSLineWidth",PSLineWidth);
      PlanCtlWindow->display();
    }
    else {
      PlanCtlWindow->close();
    }
    if ((DrawIndexX < 0) || (DrawIndexX >= Pl->P->StateDim))
      DrawIndexX = 0;
    if ((DrawIndexY < 0) || (DrawIndexY >= Pl->P->StateDim))
      DrawIndexX = 1;
    break;
  case 51: cout << "Done\n";
    Finished = true;  // This should trigger leaving (for most renderers)
    break;
  case 60: 
    cout << "Toggle Render Control Window\n";
    R->ToggleRenderCtlWindow();
    break;
  case 70: 
    cout << "Test";
    R->ToggleRenderCtlWindow();
    break;
  case 201: cout << "Switch to RRT Planner\n";
    ResetPlanner();
    Pl = new RRT(Pl->P); // Keep the old model, change the planner
    break;
  case 202: cout << "Switch to RRTGoalBias Planner\n";
    ResetPlanner();
    Pl = new RRTGoalBias(Pl->P); // Keep the old model, change the planner
    break;
  case 203: cout << "Switch to RRTGoalPull Planner\n";
    ResetPlanner();
    Pl = new RRTGoalPull(Pl->P); // Keep the old model, change the planner
    break;
  case 204: cout << "Switch to RRTCon Planner\n";
    ResetPlanner();
    Pl = new RRTCon(Pl->P); // Keep the old model, change the planner
    break;
  case 205: cout << "Switch to RRTDual Planner\n";
    ResetPlanner();
    Pl = new RRTDual(Pl->P); // Keep the old model, change the planner
    break;
  case 206: cout << "Switch to RRTExtExt Planner\n";
    ResetPlanner();
    Pl = new RRTExtExt(Pl->P); // Keep the old model, change the planner
    break;
  case 207: cout << "Switch to RRTGoalZoom Planner\n";
    ResetPlanner();
    Pl = new RRTGoalZoom(Pl->P); // Keep the old model, change the planner
    break;
  case 208: cout << "Switch to RRTPolar Planner\n";
    ResetPlanner();
    Pl = new RRTPolar(Pl->P); // Keep the old model, change the planner
    break;
  case 209: cout << "Switch to RRTHull Planner\n";
    ResetPlanner();
    Pl = new RRTHull(Pl->P);
    break;
  case 210: cout << "Switch to RRTExtCon (RRT-Connect) Planner\n";
    ResetPlanner();
    Pl = new RRTExtCon(Pl->P);
    break;
  case 211: cout << "Switch to RRTConCon Planner\n";
    ResetPlanner();
    Pl = new RRTConCon(Pl->P);
    break;
  case 212: cout << "Switch to RRTStar Planner\n";
    ResetPlanner();
    Pl = new RRTStar(Pl->P);
    break;
  case 213: cout << "Switch to PlannerHsu Planner\n";
    ResetPlanner();
    Pl = new PlannerHsu(Pl->P);
    break;
  case 214: cout << "Switch to RandomTree Planner\n";
    ResetPlanner();
    Pl = new RandomTree(Pl->P);
    break;
  case 230: cout << "Switch to PRM Planner\n";
    ResetPlanner();
    Pl = new PRM(Pl->P);
    break;
  case 240: cout << "Switch to FDP Planner\n";
    ResetPlanner();
    Pl = new FDP(Pl->P);
    break;
  case 241: cout << "Switch to FDPStar Planner\n";
    ResetPlanner();
    Pl = new FDPStar(Pl->P);
    break;
  case 242: cout << "Switch to FDPBestFirst Planner\n";
    ResetPlanner();
    Pl = new FDPBestFirst(Pl->P);
    break;
  case 243: cout << "Switch to FDPBi Planner\n";
    ResetPlanner();
    Pl = new FDPBi(Pl->P);
    break;

  default: cout << "Function not implemented\n";
    break;
  }
}



void GuiPlanner::WriteGraphs()
{
  string fname;
  fname = FilePath + "graph";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  Pl->WriteGraphs(fname);
}



void GuiPlanner::ReadGraphs()
{
  string fname;
  fname = FilePath + "graph";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  Pl->ReadGraphs(fname);
}



void GuiPlanner::WriteAnimationFrames()
{
  string fname;
  fname = FilePath + "frames";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
  ofstream fout(fname);
  fout << R->FrameList;
  fout.close();
}



void GuiPlanner::ReadAnimationFrames()
{
  string fname;
  fname = FilePath + "frames";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  ifstream fin(fname);
  fin >> R->FrameList;
  fin.close();

  R->AnimationActive = true;
}


void GuiPlanner::WritePath()
{
  string fname;
  fname = FilePath + "path";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  ofstream fout(fname);
  fout << Pl->Path;
  fout.close();
  ofstream fout2(string(fname+".times"));
  fout2 << Pl->TimeList;
  fout2.close();
}



void GuiPlanner::ReadPath()
{
  string fname;
  fname = FilePath + "path";
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  ifstream fin(fname);
  fin >> Pl->Path;
  fin.close();
  ifstream fin2(string(fname+".times"));
  fin2 >> Pl->TimeList;
  fin2.close();

  R->AnimationActive = true;
  R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
}



void GuiPlanner::DrawGraphs()
{
  edge e;
  vector x1,x2;
  double tranx,trany,scalex,scaley;

  window dw(600,600,string("RRT Display Window:     x[%d] versus x[%d]",
			    DrawIndexY,DrawIndexX));

  dw.set_line_width(LineWidth);
  dw.display();

  // The default window coords are [0,100]x[0,100]
  scalex = 100.0/(Pl->P->UpperState[DrawIndexX] - 
		  Pl->P->LowerState[DrawIndexX]); 
  scaley = 100.0/(Pl->P->UpperState[DrawIndexY] - 
		  Pl->P->LowerState[DrawIndexY]); 
  tranx = -1.0 * scalex * Pl->P->LowerState[DrawIndexX];
  trany = -1.0 * scaley * Pl->P->LowerState[DrawIndexY];

  // Show first tree
  if (Pl->G.number_of_nodes() > 0) {
    x1 = Pl->G.inf(Pl->G.first_node());
    dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		 0.5,blue);
    // wcchang+ add nodes...
    node n;
    forall_nodes(n, Pl->G)
      {
	x1 = Pl->G.inf(n);
	dw.draw_disc(x1[DrawIndexX]*scalex+tranx,
		     x1[DrawIndexY]*scaley+trany, 0.5, green);	
      }
    // wcchang-

    forall_edges(e,Pl->G) {
      x1 = Pl->G.inf(Pl->G.source(e));
      x2 = Pl->G.inf(Pl->G.target(e));
      dw.draw_segment(x1[DrawIndexX]*scalex+tranx,
		      x1[DrawIndexY]*scaley+trany,
		      x2[DrawIndexX]*scalex+tranx,
		      x2[DrawIndexY]*scaley+trany,blue);
    }
  }
  // Show second tree (if it exists)
  if (Pl->G2.number_of_nodes() > 0) {
    x1 = Pl->G2.inf(Pl->G2.first_node());
    dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		 0.5,red);
    // wcchang+ add nodes...
    node n;
    forall_nodes(n, Pl->G2)
      {
	x1 = Pl->G2.inf(n);
	dw.draw_disc(x1[DrawIndexX]*scalex+tranx,
		     x1[DrawIndexY]*scaley+trany, 0.5, blue);	
      }
    // wcchang-

    forall_edges(e,Pl->G2) {
      x1 = Pl->G2.inf(Pl->G2.source(e));
      x2 = Pl->G2.inf(Pl->G2.target(e));
      dw.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		      x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
		      red);
    }
  }

  dw.read_mouse();
  dw.close();
}



void GuiPlanner::PSDrawGraphs()
{
  edge e;
  vector x1,x2;
  double tranx,trany,scalex,scaley;
  string fname;

  fname = FilePath + string("rrtedges.%d.%d.ps",DrawIndexY,DrawIndexX);
  window m(200,63);
  m.string_item("Filename",fname);
  m.button("Done",51);
  m.display();
  m.read_mouse();
  m.close();

  ps_file f(15.0,15.0,fname);
  //f.set_draw_bb(false);
  f.set_line_width(PSLineWidth);

  // The default window coords are [0,100]x[0,100]
  scalex = 100.0/(Pl->P->UpperState[DrawIndexX] - 
		  Pl->P->LowerState[DrawIndexX]); 
  scaley = 100.0/(Pl->P->UpperState[DrawIndexY] - 
		  Pl->P->LowerState[DrawIndexY]); 
  tranx = -1.0 * scalex * Pl->P->LowerState[DrawIndexX];
  trany = -1.0 * scaley * Pl->P->LowerState[DrawIndexY];

  // Show first tree
  if (Pl->G.number_of_nodes() > 0) {
    x1 = Pl->G.inf(Pl->G.first_node());
    f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		 0.5,blue);
    forall_edges(e,Pl->G) {
      x1 = Pl->G.inf(Pl->G.source(e));
      x2 = Pl->G.inf(Pl->G.target(e));
      f.draw_segment(x1[DrawIndexX]*scalex+tranx,
		     x1[DrawIndexY]*scaley+trany,
		     x2[DrawIndexX]*scalex+tranx,
		     x2[DrawIndexY]*scaley+trany,blue);
    }
  }
  // Show second tree (if it exists)
  if (Pl->G2.number_of_nodes() > 0) {
    x1 = Pl->G2.inf(Pl->G2.first_node());
    f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		0.5,red);
    forall_edges(e,Pl->G2) {
      x1 = Pl->G2.inf(Pl->G2.source(e));
      x2 = Pl->G2.inf(Pl->G2.target(e));
      f.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
		     x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
		      red);
    }
  }

  f.close();
}



#include <LEDA/UNDEFINE_NAMES.h>