// Arduino Flush-Less (1.0) _ TV Miller
// CapacitiveSensor Library _ Paul Badger
// PFOD _ Dr. Matthew Ford

#include "CapacitiveSensor.h"
#include "Servo.h"
#include "pfodParser.h"
#include "Stream.h"
pfodParser parser;

// Pin 6 (10M) and 5 (1K)
CapacitiveSensor deucetouch = CapacitiveSensor(6,5);
// Pin 7 (10M) and 8 (1K)
CapacitiveSensor flushtouch = CapacitiveSensor(8,7);

Servo tankservo;
Servo flushservo;

int tankOFF = 120; // Tank Servo Position OFF
int tankON = 0; // Tank Servo Position ON
int flushOFF = 180; // Flush Servo Position OFF
int flushON = 0; // Flush Servo Position ON
int bowlvalve = 10; // Bowl Pin 10
int pooLED = 9; // LED Pin 9

void setup()                    
{
Serial.begin(9600);
for (int i=5; i>0; i--) { // Wait
Serial.print(i);
delay(1000);
}
parserSetup();
Serial.println();
Serial1.begin(9600); // Serial1 BlueTooth (Micro)
  
pinMode(bowlvalve, OUTPUT);
pinMode(pooLED, OUTPUT);

tankservo.attach(3); // Tank Pin 3
flushservo.attach(4); // Flush Pin 4

digitalWrite(pooLED, HIGH); // Setup Alert
delay(1000);
digitalWrite(pooLED, LOW);
delay(1000);
digitalWrite(pooLED, HIGH);
delay(1000);
}

void loop()                    
{
// Turn OFF All
flushservo.write(flushOFF); // Flush CLOSED
tankservo.write(tankOFF); // Tank CLOSED
digitalWrite(bowlvalve, LOW); // Bowl CLOSED
digitalWrite(pooLED, LOW); // LED OFF

// Read Sensors
long tank_touch = deucetouch.capacitiveSensor(30);
long flush_touch = flushtouch.capacitiveSensor(30);

// Deuce, Flush or Stand By
     if (tank_touch > 1500 && flush_touch < 500){ numbertwo(); }
else if (flush_touch > 1500 && tank_touch < 500){ emptybowl(); } 
else ( delay(10) );

// Print Sensors (Testing)
Serial.print(tank_touch);
Serial.print("\t");
Serial.println(flush_touch);

// Write Bluetooth Menu
if (Serial1.available()) {
byte in = Serial1.read();
byte cmd = parser.parse(in);
if (cmd != 0) { 
if ('.' == cmd) {
 Serial1.print(F("{."));
 Serial1.print(F("HackADay Prize"));
 Serial1.print(F("|A~<+8>Deuce"));
 Serial1.print(F("|B~<+8>Flush"));
 Serial1.print(F("}"));
 
// Handle Bluetooth Buttons
} else if('A'==cmd) { // Deuce
numbertwo();
 Serial1.print(F("{}")); // Update
} else if('B'==cmd) { // Flush
emptybowl();
 Serial1.print(F("{}")); // Update
} else {
 Serial1.print(F("{}"));
}
}
}

}

// Deuce
void numbertwo() {
digitalWrite(pooLED, HIGH); // LED ON
digitalWrite(bowlvalve, HIGH); // Bowl OPEN
tankservo.write(tankON); // Tank OPEN
delay(13000); /* Adjust Per Model */
return;
//Code To Lock Out NumberTwo()?
}

// Flush
void emptybowl() {
digitalWrite(pooLED, HIGH); // LED ON
flushservo.write(flushON); // Flush OPEN
digitalWrite(bowlvalve, HIGH); // Bowl OPEN
tankservo.write(tankON); // Tank OPEN
delay(6000);
flushservo.write(flushOFF); // Flush CLOSED
delay(2000);
digitalWrite(bowlvalve, LOW); // Bowl CLOSED
delay(15000); /* Adjust Per Model */
return;
}

// BlueTooth Tasks
byte cmdByte;
byte parserByteCounter;
byte parserState;
static const byte pfodMaxMsgLen = 0xff;
static const byte pfodStartMsg = (byte)'{';
static const byte pfodEndMsg = (byte)'}';
static const byte pfodWaitingForStart = 0xff;
static const byte pfodInMsg = 0;

void parserSetup() {
parserByteCounter = 0;
cmdByte = 0;
parserState = pfodWaitingForStart;
}

byte parse(byte in) {
parserByteCounter++;
if (parserState == pfodWaitingForStart) {
if (in == pfodStartMsg) {
parserSetup();
parserState = pfodInMsg;
}
parserByteCounter = 1;
return 0;
}    
  
if ((parserByteCounter == pfodMaxMsgLen) &&
(in != pfodEndMsg)) {
parserSetup();
return 0;
}
  
if (in == pfodEndMsg) {
byte pfodMsgCmd = cmdByte;
parserSetup();
return pfodMsgCmd;
} else if (cmdByte == 0) {
cmdByte = in;
} 
return 0;
}