// (c) MIT 2003.  All rights reserved.

// -*-Mode: Java-*- 
// $Id: SimPulleyRecognizer.java,v 1.12 2003/03/06 00:40:41 moltmans Exp $
// SimPulleyRecognizer.java -- 
// Author: Michael Oltmans <moltmans@ai.mit.edu> 
// Maintainer: Michael Oltmans <moltmans@ai.mit.edu> 
// Copyright: Copyright (C) 1999 by Michael Oltmans 
// Created: <Fri Oct 15 16:59:41 1999> 
// Time-stamp: <2000-07-17 13:52:48 moltmans> 
// Keywords: 
package rec.simulate.recognizers;

import rec.geo.*;
import java.util.*;
import rec.util.*;
import rec.sys.*;
import rec.core.widgets.*;
import rec.meche.Estimator;
import rec.simulate.*;
import rec.simulate.widgets.*;
import rec.simulate.*;

public class SimPulleyRecognizer extends Recognizer
{
    private List m_circles;
    private List m_ropes;
    private List m_pulleys;

    private double length;
    
    public boolean recognize( Surface s, Widget w )
    {
	// generate lists of ropes, circles and pulleys so we don't have 
	// to build them repeatedly

	m_circles = new LinkedList();
	m_ropes = new LinkedList();
	m_pulleys = new LinkedList();
	
	Iterator widgets = s.getAllWidgets().iterator();

	while( widgets.hasNext() ) {

	    Widget widget = (Widget)widgets.next();
	    
	    if( widget instanceof WSimRope ) {
		m_ropes.add( widget );
	    }
	    else if( widget instanceof WCircle ) {
		m_circles.add( widget );
	    }
	    else if( widget instanceof WSimPulley ){
		m_pulleys.add( widget );
	    }
	}
	
	if( w instanceof WSimRope ) {
	    return recognizeWithRope( s, (WSimRope)w );
	}
	else if( w instanceof WCircle ) {
	    return recognizeWithCircle( s, (WCircle)w );
	}
	else {
	    return false;
	}
    }

    private boolean recognizeWithRope( Surface s, WSimRope r ){

	List wheels = findWheels( s, r );
	
	Iterator it = wheels.iterator();

	WSimPulley pulleyToRemove = null;
	WSimPulley pulleyToAdd = null;

	PulleyWheel wheel;
	PulleyWheel wheelToRemove = null;

	// look for pulleys that can be made by extending other pulleys
	// return the "biggest" (most pieces) pulley that we can make

	while( it.hasNext() ) {
	    wheel = (PulleyWheel)it.next();
	    
	    Iterator oldPulleys = m_pulleys.iterator();
	    while( oldPulleys.hasNext() ) {
	
		WSimPulley testPulley = (WSimPulley)oldPulleys.next();
	
		int numRopes = testPulley.getNumRopes();


		WSimRope firstRope = testPulley.getRopeN(0);
		WSimRope lastRope = testPulley.getRopeN(numRopes-1);
		
		if(isPulley(wheel, r, firstRope) ||
		   isPulley(wheel, r, lastRope)){

		    if((pulleyToRemove == null) ||
		       (pulleyToRemove != null 
			&& (pulleyToRemove.getNumRopes() < numRopes))) {

			pulleyToAdd = createExtendedPulley(testPulley, 
							   r, 
							   wheel,
							   s);
			pulleyToRemove = testPulley;
			wheelToRemove = wheel;

		    }
		}
	    }
	}
	if(pulleyToAdd != null){
	    HashSet remove = new HashSet();
	    remove.add(pulleyToRemove);
	    remove.add(r);
	    remove.add(wheelToRemove.outer);
	    remove.add(wheelToRemove.inner);
	    
	    s.replaceWidget( remove, pulleyToAdd, true );
	    return true;
	}
	
	
	// if there are no pulleys made of other pulleys look
	// for simply a second rope.

	else{

	    it = wheels.iterator();

	    while(it.hasNext()){
		wheel = (PulleyWheel) it.next();

		List circles = new LinkedList();
		List ropes = new LinkedList();
		
		Iterator ropesIt = m_ropes.iterator();
		while( ropesIt.hasNext() ) {
		    WSimRope rope = (WSimRope)ropesIt.next();
		    if( isPulley( wheel, r, rope ) ) {
			circles.add(wheel.outer);
			ropes.add(r);
			ropes.add(rope);
			pulleyToAdd =
			    ((SimulateSurface)s)
			    .makeSimPulley( circles, ropes );
			
			HashSet remove = new HashSet();
			remove.add(wheel.outer);
			remove.add(wheel.inner);
			remove.add(r);
			remove.add(rope);
			
			s.replaceWidget( remove, pulleyToAdd, true );
			return true;
		    }
		}
	    }
	}
        return false;
    }
		    
    private boolean recognizeWithCircle( Surface s, WCircle c ){
	
	List wheels = findWheels( s, c );
	
	Iterator wheelIterator = wheels.iterator();

	HashSet removeSet = new HashSet();

	WSimPulley pulleyToAdd = null;

	PulleyWheel currentWheel;

	WSimRope firstRope1;
	WSimRope lastRope1;
	WSimRope firstRope2;
	WSimRope lastRope2;

	WSimPulley testPulley1;
	WSimPulley testPulley2;

	// look for pulleys that can be made by extending other pulleys
	// return the "biggest" (most pieces) pulley that we can make

	while( wheelIterator.hasNext() ) {
	    currentWheel = (PulleyWheel) wheelIterator.next();
	    
	    Iterator pulleys1 = m_pulleys.iterator();
	    while(pulleys1.hasNext()){
		testPulley1 = (WSimPulley)pulleys1.next();


		int numRopes1 = testPulley1.getNumRopes();

		firstRope1 = testPulley1.getRopeN(0);
		lastRope1 = testPulley1.getRopeN(numRopes1-1);
		
		Iterator pulleys2 = m_pulleys.iterator();
	
		
		// This loop is looking for pulleys that can be formed
		// by putting the wheel between two existing pulleys

		while(pulleys2.hasNext()){
		    testPulley2 = (WSimPulley)pulleys2.next();
		    
		    if(!testPulley2.equals(testPulley1)){

			int numRopes2 = testPulley2.getNumRopes();

			firstRope2 = testPulley2.getRopeN(0);
			lastRope2 = testPulley2.getRopeN(numRopes2-1);
			
						
			if(isPulley(currentWheel, firstRope1, firstRope2) ||
			   isPulley(currentWheel, firstRope1, lastRope2) ||
			   isPulley(currentWheel, lastRope1, firstRope2) ||
			   isPulley(currentWheel, lastRope1, lastRope2)){
			    
			    WSimPulley newPulley = createExtendedPulley(testPulley1,
									currentWheel,
									testPulley2,
									s);
			    if((pulleyToAdd == null) ||
			       (pulleyToAdd != null 
				&& (pulleyToAdd.getNumRopes() 
				    < newPulley.getNumRopes()))) {
				
				pulleyToAdd = newPulley;
				
				removeSet = new HashSet();
				
				removeSet.add(testPulley1);
				removeSet.add(testPulley2);
				removeSet.add(currentWheel.outer);
				removeSet.add(currentWheel.inner);
			    }
			}
		    }
		}

		// This loop looks for pulleys made by one existing pulley,
		// one existing rope, and the current wheel.
		Iterator ropesIterator = m_ropes.iterator();
		while(ropesIterator.hasNext()){

		    WSimRope testRope = (WSimRope) ropesIterator.next();

		    if(isPulley(currentWheel, firstRope1, testRope) ||
		       isPulley(currentWheel, lastRope1, testRope)){
		       
		       WSimPulley newPulley = createExtendedPulley(testPulley1,
								   testRope,
								   currentWheel,
								   s);
		       if((pulleyToAdd == null) ||
			    (pulleyToAdd != null 
			     && (pulleyToAdd.getNumRopes() 
				 < newPulley.getNumRopes()))) {
			   
			   pulleyToAdd = newPulley;
			   
			   removeSet = new HashSet();
				
			   removeSet.add(testPulley1);
			   removeSet.add(testRope);
			   removeSet.add(currentWheel.outer);
			   removeSet.add(currentWheel.inner);
		       }
		    }
		}
	    }
	    
	    if(pulleyToAdd != null){
		s.replaceWidget( removeSet, pulleyToAdd, true );
		return true;
	    }


	    // if we can't extend existing pulleys look for ropes...

	    Iterator ropesIterator1 = m_ropes.iterator();
	    Iterator ropesIterator2 = m_ropes.iterator();
	    
	    while (ropesIterator1.hasNext()){
		WSimRope testRope1 = (WSimRope) ropesIterator1.next();
		while(ropesIterator2.hasNext()){
		    WSimRope testRope2 = (WSimRope)ropesIterator2.next();
		    if (!testRope1.equals(testRope2)){
			if (isPulley(currentWheel, testRope1, testRope2)){
			    List makeWheelList = new LinkedList();
			    makeWheelList.add(currentWheel.outer);
			    List makeRopeList = new LinkedList();
			    makeRopeList.add(testRope1);
			    makeRopeList.add(testRope2);
			    pulleyToAdd = 
				((SimulateSurface) s).makeSimPulley(makeWheelList,
								    makeRopeList);

			    removeSet = new HashSet();
			    removeSet.addAll(makeRopeList);
			    removeSet.add(currentWheel.outer);
			    removeSet.add(currentWheel.inner);

			    s.replaceWidget( removeSet, pulleyToAdd, true );
			    return true;

			}
		    }
		}
	    }
	}
	
	return false;
    }

    public WSimPulley createExtendedPulley(WSimPulley pulley1,
					   PulleyWheel wheel,
					   WSimPulley pulley2,
					   Surface s){

	int numRopes1 = pulley1.getNumRopes();

	WSimRope firstRope1 = pulley1.getRopeN(0);
	WSimRope lastRope1 = pulley1.getRopeN(numRopes1-1);

	int numRopes2 = pulley2.getNumRopes();
	
	WSimRope firstRope2 = pulley2.getRopeN(0);
	WSimRope lastRope2 = pulley2.getRopeN(numRopes2-1);
	
	int start1 = 0;
	int inc1 = 0;
	int end1 = 0;
	int start2 = 0;
	int inc2 = 0;
	int end2 = 0;


	// We need to add the ropes and wheels in a sequential order.
	// This logic tells the loops which order to go through the
	// existing pulleys in

	if(isPulley(wheel, firstRope1, firstRope2)){
	    start1 = numRopes1-1;
	    inc1 = -1;
	    end1 = -1;
	    start2 = 0;
	    inc2 = 1;
	    end2 = numRopes2;
	}
	else if(isPulley(wheel, firstRope1, lastRope2)){
	    start1 = numRopes1-1;
	    inc1 = -1;
	    end1 = -1;
	    start2 = numRopes2-1;
	    inc2 = -1;
	    end2 = -1;
	}
	else if(isPulley(wheel, lastRope1, firstRope2)){
	    start1 = 0;
	    inc1 = 1;
	    end1 = numRopes1;
	    start2 = 0;
	    inc2 = 1;
	    end2 = numRopes2;
	}
	else if(isPulley(wheel, lastRope1, lastRope2)){
	    start1 = 0;
	    inc1 = 1;
	    end1 = numRopes1;
	    start2 = numRopes2-1;
	    inc2 = -1;
	    end2 = 0;
	}

	List theRopes = new LinkedList();

	for (int i = start1; i != end1; i+=inc1){
	    WSimRope currentRope = pulley1.getRopeN(i);
	    theRopes.add(currentRope);
	}

	for (int i = start2; i != end2; i+=inc2){
	    WSimRope currentRope = pulley2.getRopeN(i);
	    theRopes.add(currentRope);
	}
	
	//There's one fewer wheel than pulley
	if(start1 == 0)
	    end1--;
	else
	    start1--;
	if(start2 == 0)
	    end2--;
	else
	    start2--;

	List wWheels = new LinkedList();
	
	// Change the wheels from GCircle to WCircle and insert them for pulley 1
	for (int i = start1; i != end1; i+=inc1){
	    GCircle currentWheel = pulley1.getWheelN(i);
	    WCircle currentWWheel = new WCircle(currentWheel.getCenter(),
						currentWheel.getRadius());
	    wWheels.add(currentWWheel);
	}	

	// Insert the new wheel in the middle
	wWheels.add(wheel.outer);

	// Change the wheels from GCircle to WCircle and insert them for pulley 1
	for (int i = start2; i != end2; i+=inc2){
	    GCircle currentWheel = pulley2.getWheelN(i);
	    WCircle currentWWheel = new WCircle(currentWheel.getCenter(),
						currentWheel.getRadius());
	    wWheels.add(currentWWheel);
	}	

	return ((SimulateSurface) s).makeSimPulley(wWheels, theRopes);

    }

    public WSimPulley createExtendedPulley(WSimPulley pulley,
					   WSimRope rope, 
					   PulleyWheel wheel,
					   Surface s){

	List wheels = pulley.getWheels();
	List ropes = pulley.getRopes();
	List wWheels = new LinkedList();
	List theRopes = new LinkedList();
	

	WSimRope firstRope = pulley.getRopeN(0);
	WSimRope lastRope = pulley.getRopeN(pulley.getNumRopes()-1);
	
	if(isPulley(wheel, rope, firstRope)){
	    theRopes.add(rope);
	    wWheels.add(wheel.outer);
	}    

	Iterator it = ropes.iterator();
	while(it.hasNext()){
	    WSimRope currentRope = (WSimRope) it.next();
	    theRopes.add(currentRope);
	}	    
	    
	// getRopes returns GCircles....
	
	it = wheels.iterator();
	while(it.hasNext()){
	    GCircle currentWheel = (GCircle) it.next();
	    WCircle currentWWheel = new WCircle(currentWheel.getCenter(),
					      currentWheel.getRadius());
	    wWheels.add(currentWWheel);
	}	 

	if (isPulley(wheel, rope, lastRope)){
	    theRopes.add(rope);
	    wWheels.add(wheel.outer);
	}

	return ((SimulateSurface) s).makeSimPulley(wWheels, theRopes);
    }
    
    private LinkedList findWheels( Surface s, WSimRope r )
    {
	LinkedList list = new LinkedList();
	WCircle outer;
	WCircle inner;
	PulleyWheel wheel;
	Iterator outers = m_circles.iterator();
	
	//find the outer wheel
  	while( outers.hasNext() ) {
  	    outer = (WCircle)outers.next();

	    Iterator centers = m_circles.iterator();
	    while( centers.hasNext() ) {

		inner = (WCircle)centers.next();

		if( !inner.equals(outer) &&
		    Estimator.connects( outer.getCenter(),
					inner.getCenter(), 20 ) &&
		    outer.getRadius() > inner.getRadius()){

		    wheel = new PulleyWheel(outer, inner);
		    if (wheel.connects(r)){
			list.add(wheel);
		    }
		}
	    }
	}
	
	return list;
    }


    private LinkedList findWheels( Surface s, WCircle c )
    {
	LinkedList list = new LinkedList();
	WCircle other;
	PulleyWheel wheel;
	Iterator circles = m_circles.iterator();
	
  	while( circles.hasNext() ) {
  	    other = (WCircle)circles.next();

	    if( !other.equals(c) &&
		Estimator.connects( other.getCenter(),
				    c.getCenter(), 20 )) {

		if(other.getRadius() > c.getRadius()){
		    wheel = new PulleyWheel(other, c);
		    list.add(wheel);
		}
		else{
		    wheel = new PulleyWheel(c, other);
		    list.add(wheel);
		}
	    }
	}
	
	return list;
    }


    private boolean isPulley(PulleyWheel wheel, WSimRope r1, WSimRope r2 ){
	return ( wheel.connects( r1 ) &&
		 wheel.connects( r2 ) &&
		 isViableOrientation( wheel.outer, r1, r2 ) );
    }

    private double getOrientation( GCircle c, WSimRope r ) 
    {
	GPoint a = null;
	GPoint b = null;
	GPoint p = c.getCenter();
	GPoint p1 = r.getHeadPoint();
	GPoint p2 = r.getTailPoint();
	
	if ( c.includes( p1, 20 ) ) {
	    a = p1;
	    b = p2;
	}
	else if ( c.includes( p2, 20 ) ) {
	    a = p2;
	    b = p1;
	}
	else {
	    Assert.error( "Invalid configuration in SimPulleyRecognizer" );
	}
	
	double theta = Geometry.angleBetween( p, a, b );
	
	if ( theta < Math.PI )
	    return 1;
	else if ( theta == Math.PI )
	    return 0;
	else
	    return -1;
    }

    private boolean isViableOrientation( WCircle c, WSimRope r1, WSimRope r2 )
    {

	double x1 = getOrientation( c.getCircle(), r1 );
	double x2 = getOrientation( c.getCircle(), r2 );
	if ( x1 * x2 < 0 ) {
	    return true;
	}
	return false;
    }
    

    public Iterator getInputTypes()
    {
	Vector v = new Vector();
	
	v.addElement( "rec.core.widgets.WCircle" );
	v.addElement( "rec.simulate.widgets.wm.WMSimRope" );
	
	return( v.iterator() );
    }

    public Iterator getOutputTypes()
    {
	Vector v = new Vector();
	
	v.addElement( "rec.simulate.widgets.WSimPulley" );
	
	return( v.iterator() );
    }

    class PulleyWheel
    {
	WCircle outer;
	WCircle inner;
	
	PulleyWheel( WCircle out, WCircle in )
	{
	    outer = out;
	    inner = in;
	}
	
	public boolean equals( Object o )
	{
	    if( o instanceof PulleyWheel ) {
		PulleyWheel w = (PulleyWheel)o;
		return ( (w.outer == outer) && (w.inner == inner) );
	    }
	    return false;
	}
	
	boolean connects( WSimRope r )
	{
	    return ( outer.includes( r.getHeadPoint(), 7 ) ||
		     outer.includes( r.getTailPoint(), 7 ) );
	}
    }
}
