//PShape ROV2;
import processing.video.*;
import meter.*;
import controlP5.*;
//ControlP5 cp5;
Meter m;
Capture video;


import g4p_controls.*;
import net.java.games.input.*;
import org.gamecontrolplus.*;
import org.gamecontrolplus.gui.*;

import cc.arduino.*;
import org.firmata.*;
import processing.net.*; 


ControlDevice cont;
ControlIO  control;
ControlHat cooliehat;
Arduino  arduino;
Client myClient;
int dataIn;
boolean alreadyConnected = false;


/* PImage allows you use JPEG files to insert them into your sketch. GrOnOff is a green on/off button, RdOnOff is a red one and OrOnOff is an orange On/Off Button */
PImage GrOnOff1, GrOnOff2, GrOnOff3, GrOnOff4, GrOnOff5, GrOnOff6, GrOnOff7, GrOnOff8;
PImage RdOnOff1, RdOnOff2, RdOnOff3, RdOnOff4, RdOnOff5, RdOnOff6, RdOnOff7, RdOnOff8;
PImage OrOnOff1, OrOnOff2, OrOnOff3, OrOnOff4, OrOnOff5, OrOnOff6;

/* These PImage files are Indicator Lights */
PImage GreenSm1, GreenSm2, GreenSm3, GreenSm4, GreenSm5, GreenSm6, GreenSm7, GreenSm8;
PImage RedSm1, RedSm2, RedSm3, RedSm4, RedSm5, RedSm6, RedSm7, RedSm8;
PImage BlueSm1, BlueSm2, BlueSm3, BlueSm4;
PImage YellowSm1, YellowSm2, YellowSm3,YellowSm4, YellowSm5;

int FwdSlow;
int FwdFast;
int RevSlow;
int RevFast;
int Lfront;
int Rfront;
int i;
//int fwdRev;
//int leftRight;
int upDown;
//int val;
//int fwdRevMem;
//int LeftDrive;
//int LeftDriveMem =0;
//int RightDrive;
//int RightDriveMem = 0;
int UpDown;
//int UpDownMem = 0;
//int lightsOnMem = 0;
int lightsOn;

boolean aButtonMem =false;
//boolean current_state =false;
//boolean reading;
//int counter = 0;
//int buttonOld =1;
//int buttonNew;
int newSensorReading;

//boolean greenRect = false;
//boolean blueRect = false;
boolean home=false;
boolean home1 = false;
//boolean buttonOld =true;
//boolean buttonNew;
boolean button;
boolean button1;
boolean button2;
//boolean button3;
//boolean button4;
//boolean button5;
//boolean button6;
boolean xButtonPush;
boolean yButtonPush;


final int LIGHTS_ON= 195;
final int LIGHTS_OFF = 196;
final int FWD_FAST_ON  = 200;
final int FWD_FAST_OFF = 205;
final int REV_FAST_ON = 210;
final int REV_FAST_OFF = 215;
final int REV_SLOW_ON =220;
final int REV_SLOW_OFF= 225;
//final int FWD_SLOW_ON = 230;
//final int FWD_SLOW_OFF = 235;
//final int JOYSTICK_LEFTDRIVE_MOTION =60;
//final int JOYSTICK_RIGHTDRIVE_MOTION = 70;
//final int JOYSTICK_UPDOWN_MOTION = 80;
//final in JOYSTICK_FWDINCDIR_MOTION = 90;
final int LED29_ON = 240;
//final int LED35_ON = 250;
final int LED41_ON = 260;
final int LED47_ON = 270;
final int LED29_OFF = 280;
final int LED35_OFF = 290;
final int LED41_OFF = 300;
final int LED47_OFF = 310;



//byte BUFFER_SIZE = 1;
// byte [] byteBuffer = new byte [6];
 

boolean xButton;
boolean yButton;
boolean aButton;
boolean bButton;
boolean backButton;
boolean startButton;
boolean lights;
boolean LfrontSw;
boolean RfrontSw;
boolean RJstick;
boolean LJstick;
boolean hat_left;
boolean hat_right;
boolean hat_up;
boolean hat_down;


float hat_x;
float hat_y;
float  fwdRevMotion;
float leftRightMotion;
float upDownMotion;
float yRot;
float xAxis;
float LeftDriveMotion =0;
float RightDriveMotion = 0;
float UpDownMotion;
float fwdLeftIncDir = 0;
float revLeftIncDir = 0;
float fwdRightIncDir =0;
float revRightIncDir =0;
float fwdIncDir=0;
float revIncDir = 0;
float upDownDir;
float upDir;
float downDir;
float battVolts;
float battVolts1;
float battVolts2;
float volts1;
float volts2;

color c1;

int returnVar;



/* mmmmmmmmmmmmmmmmmmmmmmmmmmm Clock Logic Begins m Clock Logic  mmmmmmmmmm */


/*
void start() {
  startTime = millis();
  running = true;
}

void stop() {
  stopTime = millis();
  running = false;
}

int getElapsedTime() {
    int elapsed;
    
    if(running)
      {
        elapsed = (millis() - startTime);
      }
  else {
       elapsed = (stopTime-startTime);
        }
        return elapsed;
    } 
    
 int second() {
   return(getElapsedTime() /1000) % 60;
 }
 
 int minute() {
   return(getElapsedTime() /(1000*60)) % 60;
 }
 
 int hour() {
     return(getElapsedTime() /(1000*60*60)) % 24;
 }
   
   
   */
/* mmmmmmmmmmmmmmmmmmmm  Clock Logic Ends  Clock Logic  mmmmmmmmmmm  */ 


void initializeImage(PImage img)
{  // 1

/* this code takes the JPEG file and breaks it down and reassembles it into pixels so toy can manipulate the colors of each pixel */
   img.loadPixels();
   for (int x = 0; x< img.width; x++)
   {  //2
     for (int y =0; y<img.height; y++)
   {  //3
          int loc =x + y*img.width;
          float r = red(img.pixels[loc]);
          float g = green(img.pixels[loc]);
          float b = blue(img.pixels[loc]);
          img.pixels[loc] = color(r, g, b);
        }  // 1   
      }  //2
  img.updatePixels();
}  //3


void updateJoystickDisplay(float value, String str41,  int sizeX,  int sizeY, int str41X, int str41Y, int rectY)  
{ //4
  fill(255);
  rect(1450, rectY, 175,100);
  fill(0); 
  text(str41, str41X, str41Y); 
  textSize(20);
  stroke(0);
  text(value, sizeX, sizeY);
}  //4
  
  
void updateLEDButtonGraphic(PImage img, String strLed, String onOff, int sizeX, int sizeY, int strX, int strY, int onX, int onY)
{ //5
    image(img, sizeX, sizeY); 
    textSize(12);
    fill(0);
    text(strLed ,strX, strY, 100,100); 
    textAlign(CENTER); 
    textSize(40);
    text(onOff,onX,onY);
   // textSize(20);  
}  //5

void updateOnOffButtonGraphic(PImage img, String str1, String str2, int sizeX, int sizeY, int str1x, int str1y, int str2x, int str2y)
{    //  49
    image(img, sizeX, sizeY); // ON Button
    textSize(20);
    fill(0,0,0);
    text(str1, str1x, str1y);
    text (str2, str2x, str2y); 
}  //6



void setup() 
{  //7

  background(255, 255, 200);
  size(2000,1200); //sets size of window
   background(255, 255, 255);
   fill (0);
   textSize(50);
   String str0= "ROV-NISK1 Control Panel"; 
   fill (0);     
   text(str0,275, 50);







   size(displayWidth, displayHeight);
   video = new Capture(this, "pipeline:ksvideosrc device-index=0"); // looks for the USB camera
   video.start(); // start the video
   
   
 /*
 /* mmmmmmmmmmmmmmmmmm  Voltmeter Begins mmmmmmmmmmmmmmmmmmmm  */
 
 
  m = new Meter(this, 210, 120, false); // Instantiate a meter class.   
   m.setMeterWidth(250);
 
 
  
  m.setTitleFontSize(34);
  m.setTitleFontName("Arial Bold Italic");
  m.setTitleFontColor(color(0, 200, 0));
  m.setFrameStyle(PConstants.MITER);
  m.setTitle("Battery Volts");
 
     
  //Move Title Down
  m.setTitleYOffset(32);  // default is 12 pixels
 
 // Input signal range
  m.setMinInputSignal(0);
  m.setMaxInputSignal(255);

// Arc Settings
m.setArcPositionOffset(138);
 m.setArcColor(color(141, 113, 178));
  m.setArcThickness(7);
  
  // Define where the scale will appear
  m.setArcMinDegrees(180.0); // Zero (right side start)
  m.setArcMaxDegrees(360.0); // TWO_PI (right side end)
  m.setDisplayLastScaleLabel(true);
  m.setMinScaleValue(0);
  m.setMaxScaleValue(30);
 
   // Set a warning if sensor value is too low.
  m.setLowSensorWarningActive(true);
  m.setSensorWarningLowText(" Batt Low");
  m.setLowSensorWarningValue((float)13.50);
   m.setLowSensorWarningArcColor(color(255, 2, 33));
   
  // Set a warning if sensor value is too high.
  m.setHighSensorWarningActive(true);
  m.setHighSensorWarningValue((float)15.0);
  
  m.setMidSensorWarningArcColor(color(45,255,33));
  
  // m.setSensorWarningHighText("Battery Voltage Above Normal"); 
  m.setHighSensorWarningArcColor(color(45, 45, 255));  
  
  // Meter Scale
   String[] scaleLabels = {"0",  "5", "10", "15", "20", "25", "30"};
  m.setScaleLabels(scaleLabels);
  m.setScaleFontSize(30);
  m.setScaleFontName("Times New Roman Bold Italic");
  m.setScaleFontColor(color(23, 33, 253));
  m.setScaleOffsetFromPivotPoint(160);   
 
// Tic Marks

m.setLongTicMarkLength(15);
  m.setShortTicMarkLength(5);
  m.setShortTicsBetweenLongTics(4);
  m.setTicMarkColor(color(250, 150, 20));
  m.setTicMarkThickness(4);
  m.setTicMarkOffsetFromPivotPoint(128);
  m.setTicMarkColor(color(25, 22, 247));
  // m.setArcColor(color(141, 113, 178));
  
        
   // Display digital meter value.
  m.setDisplayDigitalMeterValue(true);
   m.setInformationAreaFontSize(26);
  m.setInformationAreaTextYOffset(9);
  m.setInformationAreaFontName("Verdana Bold Italic");
  m.setInformationAreaFontColor(color(32, 173, 33));

    
 /* mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm Voltmeter  Ends mmmmmmmmmmmmmmmmmm  */  
   
  // ROV2 = loadShape("ROV2.svg");
  // This code sets up the Left motor's text under the Left Button */
   String str20=" Left Motors ";
   textSize(20);
  text(str20,845, 250);
  String str21 = "Set Speed Indicator";
  text(str21,820, 275);
  
  
  String str27="Right Motors "; // sets up the text for the right motors
  text(str27, 1020, 250);
  String str28 = "Set Speed Indicator";
  text(str28, 1010, 275);
  
  String str29="Up Down "; // sets up the text for the Up Down motors
  text(str29, 675, 250);
  String str23 = "Indicator";
  text(str23, 675, 275);
  
   String str40="Lights ";
  text(str40, 1440, 260);
 
 
  /* This tells the progam that each green on/off button corresponds to a  JPG file and to load that file from a "data" file which resides with the sketch's folder */
  GrOnOff1 = loadImage("GrOnOff1.jpg");
  GrOnOff2 = loadImage("GrOnOff2.jpg");
  GrOnOff3 = loadImage("GrOnOff3.jpg");
  GrOnOff4 = loadImage("GrOnOff4.jpg");
  GrOnOff5 = loadImage("GrOnOff5.jpg");
  GrOnOff6 = loadImage("GrOnOff6.jpg");
  GrOnOff7 = loadImage("GrOnOff7.jpg");
  GrOnOff8 = loadImage("GrOnOff8.jpg");
  
  RdOnOff1 = loadImage("RdOnOff1.jpg");
  RdOnOff2 = loadImage("RdOnOff2.jpg");
  RdOnOff3 = loadImage("RdOnOff3.jpg");
  RdOnOff4 = loadImage("RdOnOff4.jpg");
  RdOnOff5 = loadImage("RdOnOff5.jpg");
  RdOnOff6 = loadImage("RdOnOff6.jpg");
  RdOnOff7 = loadImage("RdOnOff7.jpg");
  RdOnOff8 = loadImage("RdOnOff8.jpg");
  
  OrOnOff1 = loadImage("OrOnOff1.jpg");
  OrOnOff2 = loadImage("OrOnOff2.jpg");
  OrOnOff3 = loadImage("OrOnOff3.jpg");
  OrOnOff4 = loadImage("OrOnOff4.jpg");
  OrOnOff5 = loadImage("OrOnOff5.jpg");
  OrOnOff6 = loadImage("OrOnOff6.jpg");
 
  
  
  /* These indicating lamps get loaded from the "data" file and are JPG files */
  GreenSm1 = loadImage("GreenSm1.jpg");
  GreenSm2 = loadImage("GreenSm2.jpg");
  GreenSm3 = loadImage("GreenSm3.jpg");
  GreenSm4 = loadImage("GreenSm4.jpg");
  GreenSm5 = loadImage("GreenSm5.jpg");
  GreenSm6 = loadImage("GreenSm6.jpg");
  GreenSm7 = loadImage("GreenSm7.jpg");
  GreenSm8 = loadImage("GreenSm8.jpg");
  
   RedSm1 = loadImage("RedSm1.jpg");
   RedSm2 = loadImage("RedSm2.jpg");
   RedSm3 = loadImage("RedSm3.jpg");
   RedSm4 = loadImage("RedSm4.jpg");
   RedSm5 = loadImage("RedSm5.jpg"); 
   RedSm6 = loadImage("RedSm6.jpg");
   RedSm7 = loadImage("RedSm7.jpg");
   RedSm7 = loadImage("RedSm8.jpg"); 
   RedSm8 = loadImage("RedSm8.jpg");
   
   BlueSm1 = loadImage("BlueSm1.jpg");   
   BlueSm2 = loadImage("BlueSm2.jpg"); 
   BlueSm3 = loadImage("BlueSm3.jpg"); 
   BlueSm4 = loadImage("BlueSm4.jpg"); 
   
   YellowSm1 = loadImage("YellowSm1.jpg");
   YellowSm2 = loadImage("YellowSm2.jpg");
   YellowSm3 = loadImage("YellowSm3.jpg");
   YellowSm4 = loadImage("YellowSm4.jpg");
   YellowSm5 = loadImage("YellowSm5.jpg");
   
   
   initializeImage(RdOnOff1);
   initializeImage(RdOnOff2);
   initializeImage(RdOnOff3);
   initializeImage(RdOnOff4);
   initializeImage(RdOnOff5);
   initializeImage(RdOnOff6);
   initializeImage(RdOnOff7);
   initializeImage(RdOnOff8);
   initializeImage(GrOnOff1);
   initializeImage(GrOnOff2);
   initializeImage(GrOnOff3);
   initializeImage(GrOnOff4);
   initializeImage(GrOnOff5);
   initializeImage(GrOnOff6);
   initializeImage(GrOnOff7);
   initializeImage(GrOnOff8);
   initializeImage(GreenSm1);
   initializeImage(GreenSm2);
   initializeImage(GreenSm3);
   initializeImage(GreenSm4);
   initializeImage(GreenSm5);
   initializeImage(GreenSm6);
   initializeImage(GreenSm7);
   initializeImage(GreenSm8);
   initializeImage(RedSm1);
   initializeImage(RedSm2);
   initializeImage(RedSm3);
   initializeImage(RedSm4);
   initializeImage(RedSm5);
   initializeImage(RedSm6);
   initializeImage(RedSm7);
   initializeImage(RedSm8);
   initializeImage(BlueSm1);
   initializeImage(YellowSm1);
   initializeImage(YellowSm2);
   initializeImage(YellowSm3);
   initializeImage(YellowSm4);
    initializeImage(YellowSm5);
   
   
 /* The Yellow indicator's jpg file is broken down, then reassembled into pixels so they can be changed later, ie change a yellow to blue by changing the r,g,b */
   YellowSm1.loadPixels();
for (int x =0; x<YellowSm1.width; x++){
    for(int y = 0; y<YellowSm1.height; y++){
      int loc =x + y*YellowSm1.width;
          float r = red(YellowSm1.pixels[loc]);
           float g = green(YellowSm1.pixels[loc]);
            float b = blue(YellowSm1.pixels[loc]);
            YellowSm1.pixels[loc] = color(r, r, b); // to get a yellow indicator, the normal r,g,b color is changed to r,r,b
          }     
        }
     YellowSm1.updatePixels();    
     
     
    YellowSm2.loadPixels();
       for (int x =0; x<YellowSm2.width; x++){
         for(int y = 0; y<YellowSm2.height; y++){
          int loc =x + y*YellowSm2.width;
           float r = red(YellowSm2.pixels[loc]);
           float g = green(YellowSm2.pixels[loc]);
            float b = blue(YellowSm2.pixels[loc]);
            YellowSm2.pixels[loc] = color(r, r, b);
          }     
        }
     YellowSm2.updatePixels();    
    
     
    YellowSm3.loadPixels();
      for (int x =0; x<YellowSm3.width; x++){
          for(int y = 0; y<YellowSm3.height; y++){
           int loc =x + y*YellowSm3.width;
             float r = red(YellowSm3.pixels[loc]);
             float g = green(YellowSm3.pixels[loc]);
            float b = blue(YellowSm3.pixels[loc]);
            YellowSm3.pixels[loc] = color(r, r, b);
          }     
        }
     YellowSm3.updatePixels();   
     
     
     YellowSm4.loadPixels();
      for (int x =0; x<YellowSm4.width; x++){
          for(int y = 0; y<YellowSm4.height; y++){
           int loc =x + y*YellowSm4.width;
             float r = red(YellowSm4.pixels[loc]);
             float g = green(YellowSm4.pixels[loc]);
            float b = blue(YellowSm4.pixels[loc]);
            YellowSm4.pixels[loc] = color(r, r, b);
          }     
        }
     YellowSm4.updatePixels();    
 
  YellowSm5.loadPixels();
      for (int x =0; x<YellowSm5.width; x++){
          for(int y = 0; y<YellowSm5.height; y++){
           int loc =x + y*YellowSm5.width;
             float r = red(YellowSm5.pixels[loc]);
             float g = green(YellowSm5.pixels[loc]);
            float b = blue(YellowSm5.pixels[loc]);
            YellowSm5.pixels[loc] = color(r, r, b);
          }     
        }
     YellowSm5.updatePixels();    


   image(YellowSm5,1400,115);
   textSize(12);
   fill(0);
   String str304 ="Lights";
   text(str304, 1445, 155);
   textAlign(CENTER);
   textSize(40);
   String str303="OFF";
   text(str303, 1410, 155, 100,100); 

fwdFastUpdate(false, false, true);

 revFastUpdate(false, false, true);
 
  control = ControlIO.getInstance(this); // needed to control the Joystick buttons from Game Control Plus software
  cont = control.filter(GCP.GAMEPAD).getMatchedDevice("ROV_Nisk1"); // this tells the sketch the file name where the Joystick parameters are set it. It located in the "data" file
//hat = cont.getHat("coolie Hat Switch");


  if (cont == null) 
  {  
    println("no valid controller connected");
    System.exit(-1); 
  } 
    
  // connect to the local machine at port 80
  myClient  = new Client(this, "192.168.137.211", 90); // This looks for the Ethernet board attached to the arduino mega board. The mega is the master
       
  println("Connected to server");
  myClient.write("hello from processing"); // myClient.write is how data is sent to the mega
  
}  //8

 /*  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm  void setup ends mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm  */
 /*  This is the kist of the joystick buttons and their names. They are stored in the "data" file called ROV_Nisk1 */
 public void getUserInput() 
 {  //9
     LeftDriveMotion = map(cont.getSlider("LeftDrive").getValue(), -10, 10 ,-10, 10 ); 
    // xRot = map(cont.getSlider("xRot").getValue(), -1, 1, 6, 178 ); 
     UpDownMotion = map(cont.getSlider("UpDown").getValue(), -1, 1,-10,10); 
     // xAxis = map(cont.getSlider("xAxis").getValue(), -1, 1,-10, 10);    
     RightDriveMotion = map(cont.getSlider("RightDrive").getValue(), -10, 10,-10, 10);
      xButton = cont.getButton("FwdFast").pressed();
      yButton = cont.getButton("RevFast").pressed();
      aButton = cont.getButton("RevSlow").pressed();
     // aButton.getButton("RevSlow").plug(this, "Light1", control.ON_RELEASE);
      bButton = cont.getButton("FwdSlow").pressed();
      LfrontSw = cont.getButton("Lfront").pressed();
      RfrontSw = cont.getButton("Rfront").pressed(); 
      backButton = cont.getButton("Back").pressed();
      startButton = cont.getButton("Start").pressed();
      LJstick = cont.getButton("LjoystickSw").pressed();
      RJstick = cont.getButton("RjoystickSw").pressed();
      cooliehat = cont.getHat("cooliehat");
   hat_left = cooliehat.left();
   hat_right = cooliehat.right();
   hat_up = cooliehat.up();
   hat_down = cooliehat.down();
      
 }  //9

/* mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm  Fwd Fast start  mmmmmmmmmmmmm */

void fwdFastUpdate(boolean update, boolean on, boolean off)
{  //10
  if(update)
  {  //11
  myClient.write("200" + ",");
  print("200 sent");
  }  //110
  
  if(on)
  {  //12
    noStroke();
    fill(0,0); 
    ellipse(135, 270, 60,60);  
   updateLEDButtonGraphic(GreenSm1, "Speed Dir", "ON", 75, 100, 90, 130, 135, 180);      
    updateOnOffButtonGraphic(GrOnOff1, "Speed Direction", "Forward", 110, 240, 135, 325, 135, 350);
  }  //11
  else if(off)
  {  //13
   updateLEDButtonGraphic(RedSm1, "Speed Dir", "OFF", 75, 100, 90, 130, 135, 180); 
   updateOnOffButtonGraphic(RdOnOff1, "Speed Direction", "Forward", 110, 240, 135, 325, 135, 350);
  } //12
} //13

/*  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm Fwd Fast end  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmm*/


/* mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmRev Fast start  mmmmmmmmmmmmm*/
void revFastUpdate(boolean update, boolean on, boolean off)
{  //14
  if(update)
  {//  48
    myClient.write("210" +","); 
    print("210 sent");
  }  //15

  if(on)
  { //15
      noStroke();
      fill(0,0); 
      ellipse(535, 270, 60,60);  
      updateLEDButtonGraphic(GreenSm3, "Speed Dir", "ON", 475, 100, 485, 130, 535, 180);      
      updateOnOffButtonGraphic(GrOnOff3, "Speed Direction","Reverse", 510 ,240, 535, 325, 535, 350);
  } //16
  else if(off)
  {// 16
     updateLEDButtonGraphic(RedSm3, "Speed Dir", "OFF", 475, 100, 485, 130, 535, 180);  
     updateOnOffButtonGraphic(RdOnOff3, "Speed Direction","Reverse", 510, 240, 535, 325, 535, 350);  
  } //17
}  //18
/* mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm  Rev Fast end  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm*/

/* mmmmmmmmmmmmmmmmm  Lights On/Off  start  mmmmmmmmmmmmm*/




void lightsOnUpdate(boolean update, boolean on, boolean off)
{ 
  if(update)
  { 
  myClient.write("197"); 
  }  
  
  if(on)
  {   
    updateLEDButtonGraphic(BlueSm1, "Lights", "ON", 1400, 115, 1410, 140, 1460, 190);      
    
  } 
  else if(off)
  { 
    updateLEDButtonGraphic(YellowSm4, "Lights", "OFF", 1400, 115, 1410, 140, 1460, 190); 
    
  }
}  

/*  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm Light On/Off end  mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm*/

/*  mmmmmmmmmmmmmmmmmmmmmmmmmmm   Up Down Motion ends  mmmmmmmmmmmmmmmm  */

void captureEvent(Capture video)
{
  video.read();
}


/* mmm VOID  DRAW BEGINS  VOID DRAW BEGINS  DRAW BEGINS mmmmmmmmmmmmmmmmm */

void draw()
{  //34
  getUserInput();  
  

  
image(video, 250, 400, 1200, 550); // sets the video window ( x, y, width, height) of the video stream


 UpDown = int (UpDownMotion);

   if(myClient.active() == true)
  { //35
     if (myClient.available() > 0) 
     {  //36
       if(!alreadyConnected)
       { //37
          myClient.write("I am available");
          println("My Client is available");
          alreadyConnected = true;
       }  //36
       
   /* mmmmmmmmmmmmmmmmmmmmmmmmmmmm Battery Voltmeter Begins mmmmmmmmmmmmmmmmmm  */  

 




if (dataIn> 100 && dataIn <150)
   {
    
  battVolts = (float) dataIn *12 *0.01;
//print("battVolts is: ");
//println(battVolts);
   
 
   // print("Battery Voltage is: ");
  //  println( battVolts); 
   

      //myClient.readBytes((byte*) &vout, sizeof(vout)); 
  if (battVolts > 13.5 && battVolts < 15.0)
     {
      m.setFrameThickness(10);
      m.setFrameColor(color(3, 255, 120));
    String str59 = "Battery Voltage Low";
     textSize(20); 
   fill(255,255,255);
     text(str59, 330,310);     
     }
     
else  if (battVolts <=13.5) 
        {      
        m.setFrameThickness(10);
        m.setFrameColor(color(245, 10, 14));
       String str59 = "Battery Voltage Low";
       textSize(20); 
      fill(255,0,0);
     text(str59, 330,310); 
        }
      
   /*  
     m = new Meter(this, 210, 120, false); // Instantiate a meter class.   
  m.setTitleFontSize(34);
  m.setTitleFontName("Arial Bold Italic");
  m.setTitleFontColor(color(0, 200, 0));
  m.setFrameStyle(PConstants.MITER);
  // Define where the scale labele will appear
  m.setArcMinDegrees(180.0); // Zero (right side start)
  m.setArcMaxDegrees(360.0); // TWO_PI (right side end)
  m.setDisplayLastScaleLabel(false);
  
 // m.setTitle("Batt Volts"); 
  m.setTitleYOffset(18);  // default is 12 pixels
  m.setMeterWidth(250);
  m.setTitle("Battery Volts");
  m.setTicMarkColor(color(25, 22, 247));
   m.setArcColor(color(141, 113, 178));
    m.setScaleFontColor(color(35, 25, 255));
  String[] scaleLabels = {"0", "2", "4", "6", "8", "10", "12", "14", "16"};
  m.setScaleLabels(scaleLabels);
  m.setScaleFontSize(30);
  m.setScaleFontName("Times New Roman Bold Italic");
  m.setScaleFontColor(color(232, 33, 73));
  m.setScaleOffsetFromPivotPoint(165);   
   m.setTicMarkColor(color(25, 22, 247));
   m.setArcColor(color(141, 113, 178));
    m.setScaleFontColor(color(35, 25, 255));
 // Display digital meter value.
  m.setDisplayDigitalMeterValue(true);
  // Set a warning if sensor value is too low.
  m.setLowSensorWarningActive(true);
  m.setSensorWarningLowText(" Batt Low");
  m.setLowSensorWarningValue((float)10.0);
   m.setLowSensorWarningArcColor(color(50, 2, 233));
  // Set a warning if sensor value is too high.
  m.setHighSensorWarningActive(false);
  m.setHighSensorWarningValue((float)13.5);
  // m.setSensorWarningHighText("Battery Voltage Normal"); 
  m.setHighSensorWarningArcColor(color(45, 255, 23));    
   */ 

  //m = new Meter(this, 210, 120, false); // Instantiate a meter class.   
  
  
 // m.setTitleFontSize(34);
 // m.setTitleFontName("Arial Bold Italic");
 // m.setTitleFontColor(color(0, 200, 0));
//  m.setFrameStyle(PConstants.MITER);
 // m.setTitle("Batt Volts"); 
 // m.setTitleYOffset(18);  // default is 12 pixels
 // m.setMeterWidth(250);
 // Meter Scale
 
     
     
     
     
 int newSensorReading;  // Meter reads 0 to 255 equals 0 to 5 volts 
battVolts= map(battVolts, 0, 30, 0, 255);
newSensorReading  = (int) (battVolts);    
m.updateMeter (newSensorReading);
print("newSensorReading is: ");
println(newSensorReading);   
   
     }
 /* mmmmmmmmmmmmmmmmmmmm Voltmeter Ends mmmmmmmmmmmmmmmmmmmmmmm  */    
     
     
/* these codes will convert the left motors forward signal to a String variable and then send it  with a numerial identifier separeted by a comma to the Mega.
 * it sends the number 190 and the value -10 to 10 which is the left motors fwd speeed setting.
*/ 
 int o = 190;
int p = int (fwdLeftIncDir);
String so = str(o);
String sp = str(p);
String se = (so + "," + sp + "," );
myClient.write(se);     
   
  int q= 195; // sends the no. 195 with the right side motors speed setting to the mega. Both are converted to a String
  int r = int (fwdRightIncDir);
  String sq = str(q);
  String sr = str(r);
  String sf = (sq + "," + sr + ",");
  myClient.write(sf);
  
  
   int s= 198; // sends the no. 198 with the LeftDriveMotion as the camera movement to the mega. Both are converted to a String
  int t = int (LeftDriveMotion);
  String ss = str(s);
  String st = str(t);
  String su = (ss + "," + st + ",");
  myClient.write(su);
  //print("Left DriveMotion is: ");
  //println(LeftDriveMotion);
  
  
    
int m = 80; // sends the No. 80 and Up down motor speed to the mega
int n = int(upDownDir);
String sm = str(m);
String sn = str(n);
String sd = (sm + "," + sn + "," );
myClient.write(sd); 
       
        dataIn = myClient.read();
        print("dataIn is: "); 
        println(dataIn);
     }

  fwdFastUpdate(xButton, (dataIn == FWD_FAST_ON), (dataIn == FWD_FAST_OFF));
  revFastUpdate(yButton, (dataIn == REV_FAST_ON), (dataIn == REV_FAST_OFF));
  
 
 
lightsOnUpdate(aButton, (dataIn == LIGHTS_ON), (dataIn == LIGHTS_OFF));
 
  fwdLeftIncDir = map(fwdLeftIncDir, -10, 10, -10,10); // maps the value of the fwd motors speed  from -10 to 10
  fwdLeftIncDir = constrain(fwdLeftIncDir, -10, 10); // the constrain is used to stop the signal input from going below -10 and above 10
   fwdRightIncDir = map(fwdRightIncDir, -10,10,  -10,10);
 fwdRightIncDir = constrain(fwdRightIncDir, -10, 10);
 
 upDownDir = map(upDownDir, -10,10, -10, 10);
 upDir = constrain(upDownDir, -10, 0);
 downDir = constrain(upDownDir, 0,10);

 
 /* the hat up down sets the up down speed . the value is incremented by .1  evrytime the code runs runs by this step  and stores it in ots own variable
   * the first time through upDown is 0, so UpDownDir = 0+0.1, upDownDir = 0.1. he second through upDownDir = 0.1 + 0.1 = 0.2
   * as long as the hat up button is pressed, the value of upDownDir increases by 0.1
  */ 
 if (hat_up)
    {
     upDownDir = upDownDir + 0.1;
    // upDownDir = upDownDir;
     
    }
 /* When the hat_down button is pressed, the value in upDownDir is subtracted by0.1. On thew first scan no button pressed upDownDir=0
  * upDownDir = 0 -0.1, so UpDownDir = -0.1. After the hat_Down is pressed again, upDownDir = -0.1 - 0.1
  * upDownDiir = -0.2
  */
 if (hat_down)
 {
    upDownDir = upDownDir - 0.1;
    //upDownDir = upDownDir;
 }
 /* mmmmmmmmmmmmmmmmm Up Down Buttons mmmmmmmmmmmmmmmmm */
 
 
 image(RedSm2, 650, 100);
  fill(0);
  textSize(13);
   String str100="DOWN"; 
   text(str100, 660, 130, 100,50); 
   
  if (upDown >0.1)
    { 
  image(RedSm2, 650, 100);
   fill(0);
  textSize(13);
   String str111="DOWN"; 
   text(str111, 660, 130, 100,50);
    }
 
 
 else if (upDownDir >= 0.1)
  {
    image(GreenSm2, 650,100);
     fill(0);
     textSize(13);
   String str300="UP";
   text(str300, 660, 130, 100,50);
  }
 
   else if (upDownDir > -.1 && upDownDir <.1)
  {
    image(YellowSm3,650,100);
    fill(0);
    textSize(14);
   String str302="OFF";
   text(str302, 660, 130, 100,50); 
  }
 
 fill(0);
  stroke(0);
  textSize(40); 
  text(nf(upDownDir, 0,1),710, 180);
  textSize(20);
 
  
  /* mmmmmmmmmmmmmmmmmmmmmmmmmmm FWD Rev Speed Control Speed Control  speed control mmmmmmmmmmmmmmmmmmmmmmmmmm  */
  
 /* mmmmmmmmmmmmmmmmmm assigning the speed switches begin mmmmmmmmm */ 
 if (LfrontSw)
{
fwdLeftIncDir = fwdLeftIncDir +.1;
//fwdLeftIncDir = fwdLeftIncDir;

} 

if (LJstick)
{
 
fwdLeftIncDir = fwdLeftIncDir -.1;
//fwdLeftIncDir = fwdLeftIncDir;
} 

if(RfrontSw)
{
 fwdRightIncDir = fwdRightIncDir +.1;
//fWdRightIncDir = fwdRightIncDir;
}

 if(RJstick)
{
 fwdRightIncDir = fwdRightIncDir -.1;
//fwdRightIncDir = fwdRightIncDir;
}
  

  
  
 /* mmmmmmmmmmmmmmmmmmmmmmmmmmm  left Motor speed controls mmmmmmmmmmmmmmmmmmm */ 
if (fwdLeftIncDir <= -.1) 
{
image(RedSm6, 829, 100);
 fill(0);
 
  textSize(13);
   String str301="Rev Speed"; 
   text(str301, 840, 125, 100,50); 
  }
  
 else if (fwdLeftIncDir >= .1)
  {
    image(GreenSm6, 829,100);
     fill(0);
     textSize(15);
   String str300="Fwd Speed";
   text(str300, 840, 125, 100,50);
   
   
  }
  else if (fwdLeftIncDir > -.1 && fwdLeftIncDir <.1)
  {
    image(YellowSm1,829,100);
    fill(0);
    textSize(14);
   String str302="OFF";
   text(str302, 887, 135); 
  }
  fill(0);
  stroke(0);
  textSize(40); 
  text(nf(fwdLeftIncDir, 0,1),890, 180);
  textSize(20);
  
 
 /* mmmmmmmmmmmmmmmmmmmmmmmmmm Right motors speed control  mmmmmmmmmmmmmmmmmmm*/ 
 if (upDownDir <= 0.1)
 {
 image(RedSm3, 1000, 100);
 fill(0);
  textSize(13);
   String str25="Rev Speed"; 
   text(str25,1010, 125, 100,50);
 }
 
  
  if (fwdRightIncDir <= -.1) 
{
//fill(255, 0, 0); 
image(RedSm5, 1000, 100);
 fill(0);
  textSize(13);
   String str25="Rev Speed"; 
   text(str25,1010, 125, 100,50); 
  }
  
 else if (fwdRightIncDir >= .1)
  {
    image(GreenSm5, 1000,100);
     fill(0);
     textSize(15);
   String str26="Fwd Speed";
   text(str26, 1010, 125, 100,50);
   
   
  }
  else if (fwdRightIncDir > -.1  && fwdRightIncDir < .1)
  {
    image(YellowSm2,1000,100);
    fill(0);
    textSize(15);
   String str302="OFF"; 
   text(str302, 1065, 140); 
  }
  fill(0);
  stroke(0);
  textSize(40); 
  text(nf(fwdRightIncDir, 0,1), 1065, 180);
  textSize(20);
 /* mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm FWD Rev speed contro ends   fwd rev speed control ends  mmmmmmmmmmmmmmmmmmm  */ 
 /*nf sets the no of decimal places to fwdRightIncDir.
 * The 0, is the no. of digits added before the decimal place, so none.
 * The 1, means 1 digit after the decimal place.
 */
  if (dataIn == 260)
    {
      println("returned 260 Right3 fwd ");
    }
    if (dataIn == 270)
    {
      println("returned 270 Right4 fwd");
    }
    
    if(dataIn == 255)
    {
      println("Retrned 255 Right3 rev");
    }
    if(dataIn == 265)
    {
      println("retirned 265 Right 4 rev");
    }  
    
  
 if (backButton)
 {
   home = !home;
 
   if (home)
   {
 fwdLeftIncDir = 0;
 fwdRightIncDir =0;
   }
 } 
  
 
 if (startButton)
 {
   home1 = !home1;
 
   if (home1)
   {
upDownDir = 0;
   }
 } 
 

  /* mmmmmmmmmmmmmmm  Left and Right side motors Fwd  left fwd and right  left and right    fwd  fwd  fwd  fwd  fwd left fwd fwd  mmmmmmmmmmmmmm */
  
  if (fwdLeftIncDir > -0.5 && fwdLeftIncDir <=0.5) // && dataIn ==280)
  {
    image(OrOnOff1, 1200,100);
    textSize(20);
    String str2= "Left1";
      fill(0);
      text(str2 ,1175, 75, 100,150);
          
   image(OrOnOff3, 1200,250);
   textSize(20);
   String str22= "Left2";
       fill(0);
       text(str22 ,1175,220, 100,150);
  }  
      
   if (fwdLeftIncDir > 0.5)
   {
  image(GrOnOff5, 1200,100);
    textSize(20);
  String str12= "Left1";
      fill(0);
      text(str12, 1175, 75, 100, 150);
     }
       
       
  if   (fwdRightIncDir > -0.5 && fwdRightIncDir <= 0.5) // dataIn ==300)
  {    
     image(OrOnOff2, 1300,100);
    textSize(20);
    String str55= "Right3";
       fill(0);
       text(str55 ,1275, 75, 100,150);
             
    image(OrOnOff4, 1300,250);
    textSize(20);
    String str53= "Right4";
       fill(0);
      text(str53 ,1275,220, 100,150);  
     }
           
   if (fwdRightIncDir >0.5)
     {  
     image(GrOnOff7, 1300,100);
    textSize(20);
    String str44= "Right3"; 
      fill(0);
      text(str44 ,1275, 75, 100,150);
      }
   
if (fwdLeftIncDir > 5.0)
   {
    image(GrOnOff6, 1200,250);
    textSize(20);
    String str23= "Left2";
    fill(0);
    text(str23 ,1175,220, 100,150);
   }  
   
    if (fwdRightIncDir > 5.0)
   {    
    image(GrOnOff8, 1300,250);
    textSize(20);
    String str52= "Right4";
     fill(0);
     text(str52 ,1275,220, 100,150);
     } 
    

 if (fwdLeftIncDir > 0 && fwdLeftIncDir  < 4.5)
      {
       image(OrOnOff3, 1200,250);
       textSize(20);
       String str24= "Left2";
       fill(0);
       text(str24 ,1175,220, 100,150);
       println("Hello");
      }
 
  if (fwdRightIncDir > 0  && fwdRightIncDir < 4.5)
  {
  image(OrOnOff4, 1300,250);
    textSize(20);
    String str53= "Right4";
       fill(0);
      text(str53 ,1275,220, 100,150);
  }  
  /* mmmmmmmmmm Left and Right Fwd Motors end mmmmm*/
  
  /* mmmmmmmmmm Left and Right Reverse  reverse  revsers  Motors begin mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm */
  
   if (fwdLeftIncDir < -0.5 )                   // Left Motor 1 < -0.5
   {    
     image(RdOnOff5, 1200,100);
     textSize(20);
     String str2= "Left1";
      fill(0);
      text(str2, 1175, 75, 100, 150); 
     }
   
       
   if (fwdRightIncDir < -0.5)                 // Right motor 3 <-0.5
     { 
      image(RdOnOff6, 1300,100);
      textSize(20);
      String str44= "Right3";
      fill(0);
      text(str44 ,1275, 75, 100,150);
     }
     
    
 if (fwdLeftIncDir < -5.0 )                    // Left Motor 2 < -5.0
 {
  image(RdOnOff7, 1200,250);
  textSize(20);
  String str22= "Left2";
   fill(0);
   text(str22 ,1175,220, 100,150);  
 }
  
   if (fwdRightIncDir < -5.0)                // Right motor 4 < -   
   {
    image(RdOnOff5, 1300,250);
    textSize(20);
    String str52= "Right4";
     fill(0);
     text(str52 ,1275,220, 100,150);
   }
  
  
  if (fwdLeftIncDir > -4.5 && fwdLeftIncDir < 0)                // Left motor 2 > -4.5
   {
    image(OrOnOff3, 1200,250);
    textSize(20);
    String str23= "Left2";
    fill(0);
    text(str23 ,1175,220, 100,150);
   }  

    if (fwdRightIncDir > -4.5  && fwdRightIncDir < 0)               // Right motor 4 > -4. 
   {
    image(OrOnOff4, 1300,250);
    textSize(20);
    String str52= "Right4";
     fill(0);
     text(str52 ,1275,220, 100,150);
   }
  
}  //37

  else
  {  //38
     println("Client is not active");
  } //38
} //3


void mousePressed()
   {  //39
  
  if 
  (mousePressed == true)
  {  //40
    myClient.write('1');
  } //41
  
  if(mouseButton == CENTER)
  {   //41
    myClient.write('0');
  } //42
  
  
   if (mouseButton ==LEFT && dist(mouseX, mouseY, 310, 570) <30) 
     {   //  42 FwdFast  
       button = !button; // Light1
     } //43
     
     
     
  if (mouseButton == LEFT && dist(mouseX, mouseY,135,270) < 30)
    {  //43
       button1 = !button1; //Button X Fast Fwd Select
       fwdFastUpdate(true, button1, !button1);
    } //44
     
      if (mouseButton ==LEFT && dist(mouseX, mouseY, 535, 270) <30)
      {   //44
        button2 = !button2; // Button Y Fast Rev Select 
        revFastUpdate(true, button2, !button2);
      } //45
     
  
       
   } //49