/*
Whip mechanics robot: 
uses tethered mouse feedback via motor impulse, and cord length to generate pseudo-random motion continuing from an initial motion. 
Possible improvements:
dont use delay, or dont interupt mouse feedback, or dont stop after initial start at 0,0 crossing.
   Initial start up is at 0,0, x,y +/-
works ok as is.
*/

#include "USBHost_t36.h"
//Built on a teensy 4.M1
//analog io 
const int LF = 0;
const int LR = 1;
const int RF = 2;
const int RR = 3;

USBHost myusb;
MouseController mouse1(myusb);
USBHub hub1(myusb);
USBHIDParser hid1(myusb);
//X:lft,rgt Y;fwd,rev
void fwd(int y);
void rev(int y);
void lft(int x);
void rgt(int x);
void stp();
int dtime =200; //Run Time
int n=100;  //multiplyer for runtime (speed)
void setup(){
 //while (!Serial) ; // wait for Arduino Serial Monitor:dubug use onlu
  myusb.begin();    //on board USB input
  pinMode(LF,OUTPUT);
  pinMode(LR,OUTPUT);
  pinMode(RF,OUTPUT);
  pinMode(RR,OUTPUT);
}

void loop(){
  myusb.Task();

  
  if (mouse1.available()) {
    int x = n * mouse1.getMouseX(); //X==right
    int y = n * mouse1.getMouseY(); //Y==left
    //gt 0=fwd
    //lt 0=rverse
    
    if(x==0 && y==0){
      stp();
        //all pins ==0== stoped       
      }
      if(y>0){ //fwd
        fwd(y);
       }
      if(y<0){
        rev(y);
        }
      if(x>0){ //right?
        rgt(x);
        }     
      if(x<0){
        lft(x);
        }
    
   /*
    //Serial.print("Mouse: buttons = ");
    //Serial.print(mouse1.getButtons());
    Serial.print(",  mouseX = ");
    Serial.print(mouse1.getMouseX());
    Serial.print(",  mouseY = ");
    Serial.print(mouse1.getMouseY());
    //Serial.print(",  wheel = ");
    //Serial.print(mouse1.getWheel());
    //Serial.print(",  wheelH = ");
    //Serial.print(mouse1.getWheelH());
    Serial.println();
 */
    mouse1.mouseDataClear();
  }

}
//X:lft,rgt Y;fwd,rev
void fwd(int y){ //pos Y
Serial.println("Forward");    
       analogWrite(LF,y);
       analogWrite(LR,0);
       analogWrite(RF,y);
       analogWrite(RR,0);
       delay(dtime);
       stp();  
  }
void rev(int y){  //neg Y
Serial.println("Reverse");
       analogWrite(LF,0);
       analogWrite(LR,y);
       analogWrite(RF,0);
       analogWrite(RR,y);
       delay(dtime);
       stp();
       }
void lft(int x){ //neg X
Serial.println("Left");
       analogWrite(LF,0);
       analogWrite(LR,x);
       analogWrite(RF,x);
       analogWrite(RR,0);
       delay(dtime);
       stp();
  }
void rgt(int x){ //pos X
Serial.println("Right");
       analogWrite(LF,x);
       analogWrite(LR,0);
       analogWrite(RF,0);
       analogWrite(RR,x);
       delay(dtime);
       stp();
       }
void stp(){
Serial.println("Stop");
       analogWrite(LF,0);
       analogWrite(LR,0);
       analogWrite(RF,0);
       analogWrite(RR,0);
       }