#include <LiquidCrystal.h>
#include <Servo.h>

//*********************************************
//Nano Ports
//*********************************************

#define BTNok 17    //A3
#define BTNmode 14  //A0   //time pattern speed reset
#define BTNplus 15  //A1
#define BTNminus 16 //A2
#define buzzer 6    //Was 5
#define ledPin 18
#define LCDrs 12
#define LCDen 11 
#define LCDd4 2
#define LCDd5 3
#define LCDd6 4
#define LCDd7 7
#define SVpin1 5
#define SVpin2 10

//*********************************************
//Misc definitions
//*********************************************

#define maxModes 8
#define pauseflash 250        //
#define stoppingDelay 1500    //delay to let waves die down
#define delayMultiplier 10
#define bounceBackDelay 200   //delay to stop bounceback
#define beepDelay 500

//*********************************************
//Flags
//*********************************************
#define NOTRUNNING 0
#define RUNNING 1
#define PAUSED 2

//*********************************************
//MODE definitions
//*********************************************
#define modeSPEED 0
#define modeINTENSITY 1
#define modeTIME 2
#define modePATTERN 3
#define modeSTART 4
#define modeSTOP 5
#define modePAUSE 6
#define modeTEST 7

#define patRANDOM 5



//*********************************************
//Global Program Variables
//*********************************************
byte JIGmode=-1;
byte runstate=NOTRUNNING;  //0=not running 1=running 2=paused
bool modeDisplay=0;        //Is the "mode" display on screen
bool haschanged=false;     //so we know if a setting has been changed whilst in running mode

//*********************************************
//Timer Variables
//*********************************************

unsigned long timeoutTarget;
unsigned long timeoutDelay=5000;    //no button press for x seconds, go back to default display.
unsigned long finaltime;
unsigned long startMil;
unsigned long displayUpdateFlag;

//*********************************************
//Display and value arrays
//*********************************************
String modes[maxModes]={"Set Speed:","Set Intensity:","Set Time:","Set Pattern:","Start","Stop","Pause","Test"}; //Modes
byte maxval[maxModes]={10,10,60,5,-1,-1,-1,-1};           //max values for each of the modes. -1=irrelevant
byte minval[maxModes]={1,1,1,0,-1,-1,-1,-1};             //min values for each of the modes. -1=irrelevant
byte incval[maxModes]={1,1,5,1,-1,-1,-1,-1};             //increment values for each of the modes. -1=irrelevant
byte modeval[maxModes]={5,5,15,0,-1,-1,-1,-1};           //stored values for each of the modes. -1=irrelevant
byte xco[maxModes]={11,14,9,0,-1,-1,-1,-1};              //display Xco-ords of the mode values (eg where to display x)-1=irrelevant
byte yco[maxModes]={0,0,0,1,-1,-1,-1,-1};                //display Yco-ords of the mode values (eg where to display x)-1=irrelevant

//*********************************************
//Pattern def arrays
//*********************************************
String patterns[6]={"Left to Right","Back to Front","Circular CW","Circular Anti-CW","Jiggle","Random"};
String shortpat[6]={"L->R","B->F","CLOCK","ACLOCK","JIG","RAND"};

//*********************************************
//loop working arrays and variables
//*********************************************
int xrange[101];
int yrange[101];
byte xstart[7]={0,0,0,0,0,0};   //where in the array does the counter start
byte ystart[7]={0,0,50,75,0};   //this allows us to offset the paddles to ceate a wave motion
int servoDelay;
long waitForMils;
int lpctrx;             //we have two counters, so that we can offset one from the other
int lpctry; 
int beepFreq[4]={750,750,750,750};
int beepDuration[4]={125, 250,500,750};

//*********************************************
//Graphics for the percentage bar
//**********************************************

byte barGraphic [8][8];


//*********************************************
//Initialise the LCD
//*********************************************
LiquidCrystal lcd(LCDrs, LCDen, LCDd4, LCDd5, LCDd6, LCDd7);

//*********************************************
//Initialise the Servos
//*********************************************

static const int servosPins[2] = {SVpin1,SVpin2};

Servo servos[2];


void setup() {
  int lp;
  int bar;
  byte shade[8]={128,192,224,240,248,252,254,255};
  byte Graphic[8];

  Serial.begin(115200);
  lcd.begin(16, 2);

  pinMode(BTNmode, INPUT);  
  pinMode(BTNplus, INPUT);
  pinMode(BTNminus, INPUT);
  pinMode(BTNok, INPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(buzzer, OUTPUT);
  
  for(lp = 0; lp < 2; ++lp) {
    if(!servos[lp].attach(servosPins[lp])) {
        Serial.print(F("Servo "));
        Serial.print(lp);
        Serial.println(F(" attach error"));
    }
  }
  
  for (lp=0;lp<8;lp++) {  //which block are we writing
    for (bar=0; bar<8; bar++) { //which bar are we writing
       Graphic[bar]=shade[lp];
    }
    lcd.createChar(lp,Graphic);
  }

  displaySplash();
  displayRest();
  
  randomSeed(analogRead(5));

}

void loop() {

  if (runstate==RUNNING) {
      if (millis()>waitForMils) {
          //Serial.println(String(millis())+"->"+String(waitForMils));
          servos[0].write(xrange[lpctrx]);
          servos[1].write(yrange[lpctry]);
          //Serial.println(String(lpctrx)+" "+String(xrange[lpctrx])+","+String(yrange[lpctry]));
          waitForMils=millis()+servoDelay;
          //Serial.println("wfm "+String(waitForMils));
          lpctrx++;
          lpctry++;
          if (lpctrx>100) {lpctrx=0;}
          if (lpctry>100) {
            lpctry=0;
            if (modeval[modePATTERN]==patRANDOM) {
              processSequence(random(0,4));
            }
          }
      }
    processRun();
  }

  if ((runstate==NOTRUNNING) && (timeoutTarget!=-1) && (millis()>timeoutTarget)) {
      displayRest();
      timeoutTarget=-1;
      modeDisplay=false;
  }


  // put your main code here, to run repeatedly:
  if (digitalRead(BTNmode)!=0) {
      // if (runstate==PAUSED) { // pressing the MODE button in PAUSE mode, stops the run
      //   processStop();
      // } else {
        timeoutTarget=millis()+timeoutDelay;
        JIGmode++;
        if (JIGmode>maxModes-1) {
          JIGmode=0;
        }
        displayMode();
      // }
      delay(bounceBackDelay);
  }

  if (digitalRead(BTNplus)!=0) {
    timeoutTarget=millis()+timeoutDelay;
    modeval[JIGmode]+=incval[JIGmode];
    displayValue(JIGmode);
    delay(bounceBackDelay);
  }

  if (digitalRead(BTNminus)!=0) {
    timeoutTarget=millis()+timeoutDelay;
    modeval[JIGmode]-=incval[JIGmode];
    displayValue(JIGmode);
    delay(bounceBackDelay);
  }

  if (digitalRead(BTNok)!=0) {
    timeoutTarget=millis()+timeoutDelay;
    processOKbutton(JIGmode);
    delay(bounceBackDelay);
  }

}

void displaySplash() {
  lcd.setCursor(4,0);
  lcd.print("Jiggler!");
  lcd.setCursor(0,1);
  lcd.print("Richard Salvage");
  delay(2000);
  for (int positionCounter = 0; positionCounter < 16; positionCounter++) {
    // scroll one position right:
    lcd.scrollDisplayRight();
    // wait a bit:
    delay(50);
  }
}

void displayRest() {

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SPEED:"+String(modeval[modeSPEED]));
  lcd.setCursor(9, 0);
  lcd.print("INT:"+String(modeval[modeINTENSITY]));
  lcd.setCursor(0, 1);
  lcd.print("TIME:"+String(modeval[modeTIME]));
  lcd.setCursor(8, 1);
  lcd.print("PT:"+shortpat[modeval[modePATTERN]]);

  JIGmode=0;
}

void displayMode() {
int special;
int colonval;
int temp=-1;

  modeDisplay=true;
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(modes[JIGmode]);
  displayValue(JIGmode);
}

void displayValue(int whichone) {

  if (maxval[whichone]!=-1) {
    if (modeval[whichone]<minval[whichone]) {
        modeval[whichone]=maxval[whichone];
    }

    if (modeval[whichone]>maxval[whichone]) {
        modeval[whichone]=minval[whichone];
    }
  
    lcd.setCursor(xco[whichone],yco[whichone]);
    switch (whichone) {
      case modeSPEED:
        haschanged=true;
        lcd.print(String(modeval[whichone])+" ");
        break;
      case modePATTERN:
        haschanged=true;
        lcd.print(patterns[modeval[whichone]]+"                 ");
        break;
      case modeTIME:
        haschanged=true;
        lcd.print(String(modeval[whichone])+" ");
        break;
      case modeINTENSITY:
        haschanged=true;
        lcd.print(String(modeval[whichone])+" ");
        break;
    }
  }
}

void processPause() { //returns 255 to cancel the jiggle or 0 to continue
  lcd.clear();
  lcd.print("Paused. Press OK");
  lcd.setCursor(0,1);
  lcd.print(" to continue");
  digitalWrite(ledPin,0);
  runstate=PAUSED;
}

void processStop() {
  runstate=NOTRUNNING;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("STOPPING");//Display status
  resetServos();        //Put them to their resting position
  delay(stoppingDelay);          //Wait for any waves to calm down
  displayRest();        //Display the standard rest screen
  digitalWrite(ledPin,0);
}

void processOKbutton(int whichone) {
unsigned long tempTIME;
      
      switch (whichone) {
      case modeSPEED:     // do nothing in this mode
      case modeTIME:     // do nothing in this mode
      case modeINTENSITY:     // do nothing in this mode
      case modePATTERN:     // do nothing in this mode
        timeoutTarget=millis();
        break;
      case modeSTART:     //Start
        switch (runstate) {
          case NOTRUNNING:  //we are not running, so we need to start
            runstate=RUNNING;
            lcd.clear();
            modeDisplay=false;
            JIGmode=0;
            processSequence(modeval[modePATTERN]);      //we always have to do this here
            startMil=millis();    //I had to do it this way due to the way Arduino works.  Remember the start millis,
            tempTIME=modeval[modeTIME];   //I need to convert this into a long first...Strange!
            finaltime=(tempTIME*60)*1000;
            // Serial.println(String(modeval[modeTIME])+"mv  ");
            // Serial.println(String(startMil)+"smill  ");
            // Serial.println(String(finaltime)+"ft  ");
            digitalWrite(ledPin,1);
            //*****************
            //Here we need to set the start point, end point and delay time
            //*******************

            break;
          case PAUSED:      //we are paused, so we need to start
            if (haschanged==true) {
              processSequence(modeval[modePATTERN]);  //if we have changed something during the pause, re-sequence then continue
            }
            runstate=RUNNING;
            break;
          case RUNNING:     //we are running already, so nothing to do
            break;
        } 
        break;
      case modeSTOP:     //Stop
        //we have pressed stop.  if we are in idle mode, show the message
        //if we are already in pause mode, unpause
        switch (runstate) {
          case NOTRUNNING:  //we are not running, so nothing to do
            break;
          case PAUSED:      //we are paused, so process stop
            processStop();
            break;
          case RUNNING:     //we are running, so process stop
            processStop();
            break;
        } 
        break;
      case modePAUSE:     //Pause
        //we have pressed pause.  if we are in idle mode, show the message
        //if we are already in pause mode, unpause
        switch (runstate) {
          case NOTRUNNING:  //we are not running, so nothing to do
            break;
          case PAUSED:      //we are paused, so unpause
            if (haschanged==true) {
              processSequence(modeval[modePATTERN]);  //if we have changed something during the pause, re-sequence then continue
            }
            runstate=RUNNING;
            lcd.clear();
            modeDisplay=false;
            break;
          case RUNNING:     //we are running, so pause with a message
            processPause();
            break;
        } 
        break;
      case modeTEST: //Test
        if (runstate==NOTRUNNING) {
          lcd.clear();
          lcd.print("Testing");
          processTest();
          displayRest();
        }
    }
}

void resetServos() {
    servos[0].write(90);    //start point 
    servos[1].write(90);    //start point     
}

void processRun() { //returns 255 to cancel the jiggle or 0 to continue
char pformat[12];
unsigned long timeleft;
unsigned long tempMil;
unsigned long mins;
unsigned long secs;
int percent;
float tlfloat;
float ftfloat;
float temp;
int blocks;
int bars;
int lp;

  if (modeDisplay==false) {   //Dont update the screen if the mode is being displayed
    tempMil=millis();
    if (tempMil-startMil>=finaltime) {
      //we have finished
      lcd.clear();
      lcd.print("Process Finished");
      lcd.setCursor(0,1);
      lcd.print("------ x -------");
      beep(); //Wait for the waves to stop
      processStop();
    } else if (tempMil>displayUpdateFlag) {
        displayUpdateFlag=millis()+1000;          //update the display every second
        timeleft=((startMil+finaltime)-tempMil)/1000; //seconds
        mins=timeleft/60;
        secs=timeleft-(mins*60);
        tlfloat=timeleft;
        ftfloat=finaltime/1000;
        percent=tlfloat/ftfloat*100;     
        lcd.setCursor(0,0);
        sprintf(pformat, "Time: %02d:%02d", mins, secs);
        lcd.print(pformat);
        lcd.setCursor(13,0);
        sprintf(pformat, "%02d\%", percent);
        lcd.print(pformat);

        temp=percent;         //this has to be a float or it will not calculate
        tlfloat=temp/100;  //re-use this float as we do not need it any more
        ftfloat=128*tlfloat;
        blocks=(128*tlfloat)/8;   //gives us the number of solid blocks to black out
        bars=ftfloat-(blocks*8);  //gives us the number of bars (and therefore which graphic)
        //Serial.println(String(percent)+"% = " + String(blocks)+","+String(bars)+"..."+String(tlfloat)+"/"+String(ftfloat));
        
        for (int lp=0;lp<blocks;lp++) {
          lcd.setCursor(lp,1);
          lcd.write(byte(7));
        }
        if (bars!=0 && bars<9) {    //just a double safety check
          lcd.setCursor(blocks,1);
          lcd.write(byte(bars));
          lcd.setCursor(blocks+1,1);
          lcd.write(byte(0));
          }

        //beep for the last 4 seconds
        if (mins==0 && secs<=4 && secs>0) {
            tone(buzzer, beepFreq[4-secs],beepDuration[4-secs]); // Send 1KHz sound signal...
        }
    }
  }
}

void processTest() {
  int lp;
  int x;
  int y;

  processSequence(modeval[modePATTERN]);      //we always have to do this here
  x=xstart[modeval[modePATTERN]];
  y=ystart[modeval[modePATTERN]];

  for (lp = 0; lp <100; lp++) { // goes from 0 degrees to 180 degrees
        servos[0].write(xrange[x]);
        servos[1].write(yrange[y]);
        delay(15);
        x++;
        y++;
        if (x>100) {x=0;}
        if (y>100) {y=0;}
  }
}

void processSequence(int whichone) {  //here we will the arrays with the angles and intensity
  int lp;
  float target;   //we need to use floats here as if we use a non float, the result is a non-float
  float temp;
  float calc;
  float tp;
  float a;
  float b;
  float maxDegrees;
  float maxval;
  float minval;
  float modint;
 
  //100% intensity is a sweep from 180 to 0, so we need to adjust it accordingly.
  //BUT we want to go up and down equally, so we calculate on 90 degrees, then take it up 25 steps,then down
  lpctrx=xstart[whichone];
  lpctry=ystart[whichone];
  modint=modeval[modeINTENSITY];  //put it in a float so that it will add up
  maxDegrees=(90.0/10.0)*modint;   //maximum flex
  a=90.0;                          //start point for calcs is always 90 degrees
  b=25.0;                          //there are 100 steps, giving 25 per "quarter"
  target=maxDegrees/b;             //gives us value per step
  maxval=(target*b)+90.0;
  minval=90-(target*b);
  servoDelay=5+(delayMultiplier*(10/modeval[modeSPEED]));   //how long are we delaying in miliseconds
  waitForMils=15;               //ensure it fires for the first time

 // Serial.println("mdeg "+String(maxDegrees));
 // Serial.println("del "+String(servoDelay));

  for (lp=0;lp<100;lp++) {
    yrange[lp]=90;
    xrange[lp]=90;
  }

  switch (whichone) {
    case 0:
      //we need to go from 180 to 0, then back again
      for (lp=0;lp<26;lp++) {
        tp=lp;
        calc=tp*target;  
        xrange[lp]=90-calc;
        xrange[lp+25]=minval+calc;
        xrange[lp+50]=90+calc;
        xrange[lp+75]=maxval-calc;
      }
      break;
    case 1:
      for (lp=0;lp<26;lp++) {
        tp=lp;
        calc=tp*target;  
        yrange[lp]=90-calc;
        yrange[lp+25]=minval+calc;
        yrange[lp+50]=90+calc;
        yrange[lp+75]=maxval-calc;
      }
      break;
    case 2: //let this drop through as we are using the same numbers but offsetting the start
    case 3:
      for (lp=0;lp<26;lp++) {
        tp=lp;
        calc=tp*target; 
        xrange[lp]=90-calc;
        xrange[lp+25]=minval+calc;
        xrange[lp+50]=90+calc;
        xrange[lp+75]=maxval-calc; 

        yrange[lp]=90-calc;
        yrange[lp+25]=minval+calc;
        yrange[lp+50]=90+calc;
        yrange[lp+75]=maxval-calc;
      }
      break;
    case 4:   //jiggle
        maxval=100+(2*modint);
        minval=80-(2*modint);
        for (lp=0;lp<100;lp++) {
        xrange[lp]=random(minval,maxval);
        yrange[lp]=random(minval,maxval);
      }
      break;
    case 5:   //random
      processSequence(random(0,4)); //a recursive call, but should be OK
      break;
  }
  resetServos();
}

void beep() {
  tone(buzzer, 1000); // Send 1KHz sound signal...
  delay(1000);        // ...for 1 sec
  noTone(buzzer);     // Stop sound...
  delay(1000); 
}