// Drag outline to create silhouette.
// Test reconstruction with 16 cameras
// Improve camera models with ProjectBack -- select one which views everything
// to be the stationary one.  Set this going with silhouettes I have cut out.
// See if I need to make this faster.
// Project one ray at a time, and figure out how to put current estimates in a matrix and adjust that matrix
// Do graphics homework.
// Look over notes for h-class so I can ask questions
// program faster and more effectively.

#include "TrackPoint.h"
#include "ShapeGlobal.h"
#include "shPoint.h"
#include "IPPList.h"
#include "SVec.h"
#include "shPolygon.h"
CLASSFUNCTION(TrackPoint)
extern int IPPround(float f);
Am_Slot_Key D_TRACK_POINT;
Am_Slot_Key D_ACT_TRACK_POINT;
Am_Define_Formula(int, watch_movement)
{
  static int sactive = 0;
  int active = self.Get(Am_ACTIVE);
  cout << "ACTIVE " << active << "\n";
  TrackPoint * tp = (TrackPoint * )GetPtr(self, D_TRACK_POINT);
  if  (sactive == 1 && active == 0) // this is the end
    {
      tp->MakePolygon();
    }
  else
    {
      tp->InitializePolygon();
    }
  sactive = active;
}

Am_Define_Formula(int, get_center)
{
  int t = self.Get(Am_TOP);
  int l = self.Get(Am_LEFT);
  int w = self.Get(Am_WIDTH, Am_NO_DEPENDENCY);
  int h = self.Get(Am_HEIGHT, Am_NO_DEPENDENCY);
  cout << "r,c " << t + h/2 << "  " << l + w/2 << "\n";
  TrackPoint * tp = (TrackPoint * )GetPtr(self, D_TRACK_POINT);
  tp->pts.Add(t+h/2, l+w/2, ListTAIL);
}

TrackPoint::TrackPoint()
{
  D_TRACK_POINT = Am_Register_Slot_Name("D_TRACK_POINT");
  D_ACT_TRACK_POINT = Am_Register_Slot_Name("D_ACT_TRACK_POINT");
}

void TrackPoint::add_my_parts(Am_Object &my_win)
{
  AddStandardParts();
  AddFunction("Track", &TrackPoint::Track, "^w");
  AddFunction("StopTrack", &TrackPoint::StopTrack, "^e");
  AddFunction("DetermineBoundary", &TrackPoint::DetermineBoundary);
  AddParts(g_TEXT, "mSample", d_INT, (Am_Ptr)&mSample);
  char ** arr1 = new char *[4];
  arr1[0] = mShape;  arr1[1] = "shPolygon";
  arr1[2] = "shSpline", arr1[3] = "shClosedSpline"; 
  dbutton db;
  db.type = g_CHOICE; db.label = "Shape"; 
  db.ptr = (void *)arr1; db.subtype = 3;
  AddParts(db);
}


void TrackPoint::StopTrack()
{
  baseShape **bs = GetShapes("shPoint");
  int i = 0, selected = 0; Am_Value_Type v;
  while (bs[i]!=0) // add a callback
    {
      shPoint *shp = (shPoint *)bs[i];
      v = shp->pt1->rect.Get_Slot_Type(D_ACT_TRACK_POINT);
      if (v!=Am_NONE)
	shp->pt1->rect.Remove_Slot(D_ACT_TRACK_POINT);
      v = shp->move_inter.Get_Slot_Type(D_ACT_TRACK_POINT);
      if (v!=Am_NONE)
	shp->move_inter.Remove_Slot(D_ACT_TRACK_POINT);
      mShp = 0; // save a pointer to the point
      i++;
    }
}

void TrackPoint::Track()
{
  baseShape **bs = GetShapes("shPoint");
  int i = 0, selected = 0;
  while (bs[i]!=0) // add a callback
    {
      shPoint *shp = (shPoint *)bs[i];
      shp->sp.GetProperty("selected", selected);
      if (selected)
	{
	  shp->pt1->rect.Set(D_TRACK_POINT, (Am_Ptr)this, Am_OK_IF_NOT_THERE);
	  shp->move_inter.Set(D_TRACK_POINT, (Am_Ptr)this, Am_OK_IF_NOT_THERE);
	  shp->pt1->rect.Set(D_ACT_TRACK_POINT, get_center, Am_OK_IF_NOT_THERE);
	  shp->move_inter.Set(D_ACT_TRACK_POINT, watch_movement, Am_OK_IF_NOT_THERE);
	  mShp = shp; // save a pointer to the point
	  break;
	}
      else i++;
    }
}


void TrackPoint::MakePolygon()
{
  int len; iPoint *ipts = 0;
  DivideIntoNSamples(pts, ipts, len);
  iPoint ip(10,10);
  MoveShape(mShp, ip);
  // move the point
  // now move pts toward the edge and make them smooth
  // put ends on for spline
  if (strcmp("shSpline", mShape)==0)
    {
      iPoint *sipts  = new iPoint[len+2];
      SVec dir(2), cr(2), p1(2), p2(2);
      p1[0] = ipts[0].v, p1[1] = ipts[0].h;
      p2[0] = ipts[1].v, p2[1] = ipts[1].h;
      dir = p1-p2; dir=dir/dir.norm();
      cr = p1+10*dir;
      sipts[0].v = (int)cr[0]; sipts[0].h = (int)cr[1];
      p1[0] = ipts[len-1].v, p1[1] = ipts[len-1].h;
      p2[0] = ipts[len-2].v, p2[1] = ipts[len-2].h;
      dir = p1-p2; dir=dir/dir.norm();
      cr = p1+10*dir;
      sipts[len+1].v = (int)cr[0]; sipts[len+1].h = (int)cr[1];
      int i = 1;
      for (i=0; i<len;i++) sipts[i+1] = ipts[i];
      MakeShapeFromPoints("shSpline", sipts, len+2);
    }
  else if (strcmp("shClosedSpline", mShape)==0)
    {
      MakeShapeFromPoints("shClosedSpline", ipts, len);
    }
  else 
    {
      MakeShapeFromPoints("shPolygon", ipts, len);
    }
  StopTrack();
  pts.Make_Empty();
}


int TrackPoint::GetEdgeMapForCurrentImage(Image &edge)
{
  baseImage **bim = GetImages();
  // determine edges for image
  int i = 0;
  if (bim[0]==0) return 0;
  char classname[20];
  bim[0]->GetProperty("classname", classname);
  Image *im = 0;
  if (strcmp(classname, "colorImage")==0) 
    {
      colorImage *cim = (colorImage *)bim[0];
      im = new Image;
      cim->GetGrayImage(*im);
    }
  else im = (Image *)bim[0];
  Image kernh(1,3,true); Image kernv(3,1, true);
  FPIX(kernh,0,0) = -1; FPIX(kernh,0,1) = 0; FPIX(kernh,0,2) = 1;
  FPIX(kernv,0,0) = -1; FPIX(kernv,1,0) = 0; FPIX(kernv,2,0) = 1;
  Image edge1,edge2;
  im->convolve2d(edge1, kernh);
  im->convolve2d(edge2, kernv);
  int r,c;
  edge.resize(*im,true);
  for (r=0; r<edge.rows; r++)
    for (c=0; c<edge.cols; c++)
      {
	FPIX(edge,r,c) = sqrt(FPIX(edge1,r,c)*FPIX(edge1,r,c)+
			      FPIX(edge2,r,c)*FPIX(edge2,r,c));
      }
  return 1;

}

 // Use edge info, polygon info and smoothness
void TrackPoint::DetermineBoundary()
{
  Image edge;
  if (!GetEdgeMapForCurrentImage(edge))
    { cerr << "can't get edge map\n"; return;}
  ShowImage(edge);
  // now get the polygon
  baseShape **bs = GetShapes("shPolygon");
  // find the strongest edge closest to the poly
  if (bs[0] == 0) { cerr << "no polygon\n"; return; }
  iPoint *ip = 0; int len;
  shPolygon *shp = (shPolygon *)bs[0];
  shp->GetPoints(ip,len);
  if (len==0) return;
  iPoint *nip = new iPoint[len];
  
  SVec dir(2), st0(2), st1(2), st2(2), perp(2), cr(2), bedge(2), idir(2), cdir(2);
  // Move perp
  st1[0] = ip[0].v; st1[1] = ip[0].h; // this is the first point
  // initialize the direction
  st2[0] = ip[1].v; st2[1] = ip[1].h;
  idir = st2-st1; idir=idir/idir.norm();
  float emax; int i =0, j=0;
  int jmin=-4; float bestdir = 0, dotdir;
  for (i=1; i<len; i++)
    {
      st2[0] = ip[i].v; st2[1] = ip[i].h;
      dir = st2-st1;
      dir = dir/dir.norm();
      perp[0] = dir[1]; perp[1] = dir[0];
      emax = -1; jmin = 8; bestdir = 0;
      for (j=-5;j<=5; j++)
	{
	  cr = st1 + j*perp;
	  cdir = st2-cr; cdir=cdir/=cdir.norm();
	  dotdir = cdir*idir;
	  bedge = st1;
	  if (!edge.IsWithinBoundaries( (int)cr[0], (int)cr[1])) continue;
	  if (FPIX(edge, (int)cr[0], (int)cr[1])>15 && 
	      abs(j)<jmin &&
	      dotdir>bestdir)
	    {
	      bedge = cr;
	      jmin = abs(j);
	      bestdir = dotdir;
	    }
	}
      // set new polygon point
      if (i==1) idir = st2-cr; 
      else idir = cr-st0;
      idir/=idir.norm();
      nip[i-1].v = IPPround(bedge[0]); nip[i-1].h = IPPround(bedge[1]);
      st0 = st1;
      st1 = st2;
    }

  // just set the last point to be the same for now
  nip[len-1] = ip[len-1];
  // delete the old polygon
  DeleteShape(shp);

  // make a new polygon
  MakeShapeFromPoints("shPolygon", nip, len);
}


void TrackPoint::InitializePolygon()
{
  pts.Make_Empty();
}

void TrackPoint::DivideIntoNSamples(IPPList &tpts, iPoint *(&ipts), int &len)
{
  float length=0;
  ComputeCurveLength(tpts, length);
  float dist = length/(mSample-1);
  ipts = new iPoint[mSample];
  len = mSample;
  int r,c;
  
  int i =0,j=0 ;
  
  SVec cr(2), dir(2), pt1(2), pt2(2), pt3(2);
  tpts.Start();
  tpts.Get(r,c); tpts.Next();
  // cout << "r,c " << r << " " << c << "\n";
  pt1[0] = r; pt1[1] = c;
  ipts[i].h = c; ipts[i].v = r; i++;
  float clen = 0, disttogo,totlen =0 ; 
  for (j=1; !tpts.Last(); tpts.Next(),j++)
    {
      tpts.Get(r,c);
      //  cout << "r,c " << r << " " << c << "\n";
      pt2[0] = r; pt2[1] = c;
      pt3 = pt2;
      clen += (pt2-pt1).norm();
      totlen+=(pt2-pt1).norm();
      while (clen>=dist)
	{
	  // adnavce from last point
	  clen -= (pt2-pt1).norm();
	  totlen -=(pt2-pt1).norm();// interpolate
	  disttogo = dist-clen;
	  dir = pt2-pt1; dir/=dir.norm();
	  cr = pt1+dir*(disttogo);
	  clen+= (cr-pt1).norm();
	  totlen+=(cr-pt1).norm();
	  //cout << "clen " << clen << "\n";
	  ipts[i].h = (int)cr[1]; ipts[i].v = (int)cr[0];
	  pt1 = cr; 
	  clen = (pt2-pt1).norm();
	  totlen+=(pt2-pt1).norm();
	  i++;
	}
      pt1 = pt3;
    }
  // make sure end point is there also
  ipts[mSample-1].h = c; ipts[mSample-1].v = r;
  cout << "hits " << i << "\n";
  cout <<"totlen " << totlen << " len " << length << "\n";
}

void TrackPoint::ComputeCurveLength(IPPList &tpts, float &length)
{
  int r,c,  or,oc;
  length = 0;
  int i=0;
  tpts.Start();
  tpts.Get(or,oc); tpts.Next();
  // cout << "r,c " << or << " " << oc << "\n";
  for (i=1; !tpts.Last(); tpts.Next(),i++)
    {
      tpts.Get(r,c);
      // cout << "r,c " << r << " " << c << "\n";
      length += sqrt((or-r)*(or-r)+(oc-c)*(oc-c));
      or = r; oc = c;
    }
  cout << "Num Links " << i << "\n";
}
