//Godzuki Platform

//The void of Godzilla
oDio1 voice = new oDio1;

//The "go" button
//oDio1 Go = new oDio1;
//oDio1 LED = new oDio1;

// PWM motor driver stuff
oPWM  rMotor = new oPWM;
oDio1	rmBack = new oDio1;
oDio1 rmFwd = new oDio1;
oPWM	lMotor = new oPWM;
oDio1 lmBack = new oDio1;
oDio1 lmFwd = new oDio1;

//edge detectors
oByte edgeLim = new oByte;
oA2D lrDet = new oA2D;
oA2D rrDet = new oA2D;
oA2D lfDet = new oA2D;
oA2D rfDet = new oA2D;
oMath lrMath = new oMath;
oMath rrMath = new oMath;
oMath lfMath = new oMath;
oMath rfMath = new oMath;
oGate lrGate = new oGate(1);
oGate rrGate = new oGate(1);
oGate lfGate = new oGate(1);
oGate rfGate = new oGate(1);
oEvent lrEvent = new oEvent;
oEvent rrEvent = new oEvent;
oEvent lfEvent = new oEvent;
oEvent rfEvent = new oEvent;

//Forward & Rear IRPD and tracking
oDio4 IRPD = new oDio4;
oGate irpdGate = new oGate(1);
oEvent irpdEvent = new oEvent;
oMath irpdConvert = new oMath;
oNibble irpdCheck = new oNibble;

//Wander stuff
oGate wanderGate = new oGate(1);
oevent wanderEvent = new oEvent;

//Motor control variables
final fwd = 0;
final rev = 1;
final slow = 40;
final med = 80;
final fast = 110;
Boolean rDir; 		//Right motor direction
Boolean lDir; 		//Left motor direction
Byte rSpeed; 		//right motor speed
Byte lSpeed; 	 	//left motor speed

//Behavior priorities
Byte cPriority;		// = new oNibble;	//Active Priority
Final pWander = 1;		//Seek opponent (Lowest)
Final pChase = 2;			//Chase wherever
Final pEdge = 3;              //Edge detect (Highest)
Final pWon = 4;			//We've won.

Sub void main(void) {
    //OOPic.node = 1;

    edgeLim = 55;			//white edge value
    cPriority = pWon;		//so no one fires off!

    setUpVoice;
    setGoStart;
    setUpMotors;
    setUpIRPD;
    setUpEdge;
    setUpWander;

    //Have to wait 5 seconds before starting
    //Will add the set button here.

    //LED.Set;
    OOPic.delay = 300; 		// 4 second delay   
    roar;
    OOPic.delay = 100;		// 1 second delay

    cPriority = 0;		//Turn him loose!
    //LED.Clear;

    //loop through forever
    while (cvTrue) {
        bWon;			//Look for winning sequence
    }
}

sub void wanderEvent_code(void) {
    //Seek behavior
    if (cPriority <= pWander){ 
        cPriority=pWander;		//try to take control
        lDir=fwd;lSpeed=med;
        rDir=fwd;rSpeed=slow;
        doMotors;
        cPriority = 0;
    }
}

sub void irpdEvent_code(void) {
    //Chase something that I saw!
    if (cPriority <= pChase) {
        cPriority = pChase;
        while (IRPD != 0) {
            lSpeed=fast;rSpeed=fast;	//Just CHASE it!
            if (IRPD == 1) {
                lSpeed = 0;
                rDir = rev;
            } else if (IRPD == 2) {
                lDir = rev;
                rSpeed = 0;
            } else if (IRPD == 3) {
                lDir = rev;
                rDir = rev;
            } else if (IRPD == 8) {
                lSpeed = 0;
                rDir = fwd;
            } else if (IRPD == 4) {
                lDir = fwd;
                rSpeed = 0;
            } else {
                lDir = fwd;
                rDir = fwd;
            }
            doMotors;
        }
        cPriority = 0;
    }
}

sub void lrEvent_code(void) {
    //A left rear checker hit the edge
    if (cPriority <= pEdge) {
        cPriority = pEdge;
        //LED.Set;
        if (rrMath.Negative == 1) {
            //Both rear ones hit
            lDir=fwd;lSpeed=fast;
            rDir=fwd;rSpeed=fast;
            doMotors;
            OOPic.delay=200;
        } else if (lfMath.Negative == 1) {
            //both left sides got a hit
            lDir=fwd;lSpeed=fast;
            rDir=fwd;rSpeed=slow;
            doMotors;
            OOPic.delay=200;
        } else {
            //Just a left rear hit
            lDir=fwd;lSpeed=fast;
            rDir=fwd;rSpeed=slow;
            doMotors;
            OOPic.delay=100;
        }            
        //LED.Clear;
        cPriority = 0;
    }
}

sub void rrEvent_code(void) {
    //Right rear edge hit.
    if (cPriority <= pEdge) {
        cPriority = pEdge;
        //LED.Set;
        if (lrMath.Negative == 1) {
            //Both rear edges hit
            lDir=fwd;lSpeed=fast;
            rDir=fwd;rSpeed=fast;
            doMotors;
            OOPic.delay=200;
        } else if (rfMath.Negative == 1) {
            //both right side hits
            lDir=fwd;lSpeed=slow;
            rDir=fwd;rSpeed=fast;
            doMotors;
            OOPic.delay=200;
        } else {
            //Just a right rear hit
            lDir=fwd;lSpeed=slow;
            rDir=fwd;rSpeed=fast;
            doMotors;
            OOPic.delay=100;
        }
        //LED.Clear;
        cPriority = 0;
    }
}

sub void lfEvent_code(void) {
    //left front edge hit.
    if (cPriority <= pEdge) {
        cPriority = pEdge;
        //LED.Set;
        if (lrMath.Negative == 1) {
            //Both left edges hit
            lDir=fwd;lSpeed=fast;
            rDir=fwd;rSpeed=slow;
            doMotors;
            OOPic.delay=200;
        } else if (rfMath.Negative == 1) {
            //both front side hits
            lDir=rev;lSpeed=med;
            rDir=rev;rSpeed=fast;
            doMotors;
            OOPic.delay=200;
        } else {
            //Just a left Front hit
            lDir=rev;lSpeed=fast;
            rDir=rev;rSpeed=slow;
            doMotors;
            OOPic.delay=100;
        }
        //LED.Clear;
        cPriority = 0;
    }
}

sub void rfEvent_code(void) {
    //Right front edge hit.
    if (cPriority <= pEdge) {
        cPriority = pEdge;
        //LED.Set;
        if (lfMath.Negative == 1) {
            //Both front edges hit
            lDir=rev;lSpeed=fast;
            rDir=rev;rSpeed=med;
            doMotors;
            OOPic.delay=200;
        } else if (rrMath.Negative == 1) {
            //both right side hits
            lDir=fwd;lSpeed=slow;
            rDir=fwd;rSpeed=fast;
            doMotors;
            OOPic.delay=200;
        } else {
            //Just a right Front hit
            lDir=rev;lSpeed=slow;
            rDir=rev;rSpeed=fast;
            doMotors;
            OOPic.delay=100;
        }
        //LED.Clear;
        cPriority = 0;
    }
}

sub void bWon(void) {
    //If we've been pushing forward and hit edge, we win
}

sub void doMotors(void) {
    //Act on motor actions chosen above
    lmFwd = lDir;
    lmBack = ~lmFwd;
    rmFwd = rDir;
    rmBack = ~rmFwd;
    lMotor.Value = lSpeed;
    rMotor.Value = rSpeed;
}

//Setup go button and LED
sub void setGoStart(void) {
    //Go.IOLine = 28;
    //Go.Direction  = cvInput;
    //LED.IOLine = 29;
    //LED.direction = cvOutput;
    //LED.value = cvLow;
}

//Edge detection A/D routines
sub void setUpEdge(void) {
    lrDet.IOLine = 1;
    lrDet.Operate = cvTrue;
    lrMath.Input2.Link(lrDet.Value);
    lrMath.Input1.Link(edgeLim);
    lrMath.Mode=cvSubtract;
    lrMath.Operate=cvTrue;
    lrGate.Input1.Link(lrMath.Negative);
    lrGate.Output.Link(lrEvent.Operate);
    lrGate.Operate = cvTrue;
    rrDet.IOLine = 2;
    rrDet.Operate = cvTrue;
    rrMath.Input2.Link(rrDet.Value);
    rrMath.Input1.Link(edgeLim);
    rrMath.Mode=cvSubtract;
    rrMath.Operate=cvTrue;
    rrGate.Input1.Link(rrMath.Negative);
    rrGate.Output.Link(rrEvent.Operate);
    rrGate.Operate = cvTrue;
    lfDet.IOLine = 3;
    lfDet.Operate = cvTrue;
    lfDet.Operate = cvTrue;
    lfMath.Input2.Link(lfDet.Value);
    lfMath.Input1.Link(edgeLim);
    lfMath.Mode=cvSubtract;
    lfMath.Operate=cvTrue;
    lfGate.Input1.Link(lfMath.Negative);
    lfGate.Output.Link(lfEvent.Operate);
    lfGate.Operate = cvTrue;
    rfDet.IOLine = 4;
    rfDet.Operate = cvTrue;
    rfMath.Input2.Link(rfDet.Value);
    rfMath.Input1.Link(edgeLim);
    rfMath.Mode=cvSubtract;
    rfMath.Operate=cvTrue;
    rfGate.Input1.Link(rfMath.Negative);
    rfGate.Output.Link(rfEvent.Operate);
    rfGate.Operate = cvTrue;
}

sub void setUpWander(void) {
    //Configure the wander event
    wanderGate.Input1.Link(OOPic.hz1);
    wanderGate.Output.Link(wanderEvent.Operate);
    wanderGate.Operate = cvTrue;
}

//Voice routines here
sub void setUpVoice(void) {
    voice.IOLine = 31;
    voice.Direction = cvOutput;
    voice = 0;
}

sub void roar(void) {
    voice = 1;
    OOPic.delay = 1;
    voice = 0;
}

//IRPD routines follow
sub void setUpIRPD(void) {
    IRPD.Direction = cvInput;
    IRPD.IOGroup = 3;
    IRPD.Nibble = 0;
    irpdConvert.Input1.Link(IRPD.Value);
    irpdConvert.Output.Link(irpdCheck.Value);
    irpdConvert.Mode = cvOr;
    irpdConvert.Operate = cvTrue;
    irpdGate.Input1.Link(irpdCheck.NonZero);
    irpdGate.Output.Link(irpdEvent.Operate);
    irpdGate.Operate = cvTrue;
}

//PWM motor routines follow
sub void setUpMotors(void) {
    lmBack.IOLine = 16;
    lmBack.Direction = cvOutput;
    lmBack = 1;			//off
    lmFwd.IOLine = 21;
    lmFwd.Direction = cvOutput;
    lmFwd = 0;			//go forward
    lMotor.IOLine = 1;		//I/O 17
    lMotor.Prescale = 2;	// div by 16
    lMotor.Period = 156;	// 2Khz = 156
    lMotor.Value = 0;
    lMotor.Operate = cvTrue;

    rmBack.IOLine = 19;
    rmBack.Direction = cvOutput;
    rmBack = 1;			//Turned off
    rmFwd.IOLine = 20;
    rmFwd.Direction = cvOutput;
    rmFwd = 0;			//Go forward
    rMotor.IOLine = 0;		//I/O line 18
    rMotor.Prescale =2;		// div by 16
    rMotor.Period = 156;
    rMotor.Value = 0;
    rMotor.Operate = cvTrue;
}