#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"
using namespace DualCoding;


class SimpleGestalt : public VisualRoutinesStateNode {
  public: 
  
    SimpleGestalt() : VisualRoutinesStateNode("SimpleGestalt") {}
   
      
    class SimpleGestaltNode : public VisualRoutinesStateNode {
      public: SimpleGestaltNode() : VisualRoutinesStateNode("SimpleGestaltNode") {}
       
      virtual void DoStart() {       
            
                camSkS.clear();
                NEW_SKETCH(camFrame, uchar, sketchFromSeg());
               
                // Get colors
                NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
                NEW_SKETCH(yellow_stuff, bool, visops::colormask(camFrame, "yellow"));
               
              
                //extract all the yellow line segmanes
                NEW_SHAPEVEC(ylines, LineData, LineData::extractLines(yellow_stuff)); 
                
                vector<Point> myPoints; 
        
                myPoints.push_back((ylines.at(0))->end1Pt());
                myPoints.push_back((ylines.at(0))->end2Pt()); 

                int resLine = 0; //index of resulting line
                EndPoint cur = myPoints.back(); 
                EndPoint closest;
                EndPoint second; 
                float min=-1.0;
                int z = 0; 
                SHAPEVEC_ITERATE(ylines, LineData, line)
                    z++; 
                    float x = line->perpendicularDistanceFrom(line->end1Pt()); 
                    float y = line->perpendicularDistanceFrom(line->end2Pt());

                    if(x>=y) //y is closer
                    {
                        if(y<min || min < 0.0) 
                        { 
                            min = y; 
                            closest=line->end2Pt(); 
                            second = line->end1Pt();
                            resLine = z; 
                        }
                    }
                    else
                    {
                        if(x<min||min<0.0)
                        {
                            min = x; 
                            closest = line->end1Pt();
                            second = line->end2Pt(); 
                            resLine=z; 
                        }
                    }
                END_ITERATE; 

            
                myPoints.push_back(closest);
                myPoints.push_back(second); 

                
                 
                NEW_SHAPE(poly, PolygonData, PolygonData::PolygonData(camShS, const_cast<const Point> ( myPoints), true, true, true)); 

                //NEW_SHAPE(region, PolygonData, PolygonData::formPolygons(ylines)); 
                //NEW_SKETCH(filled, bool, visops::fillInterior(region.render())); 
          


           return; 
    }    
   };
   
    virtual void setup() {       

        //use an ArmNode to set the shoulder, elbow and wrist joint angles directly

       #statemachine
        startnode: StateNode=N=>nextNode 

        nextNode: SimpleGestaltNode() 

        
      
      #endstatemachine                            
    }
};