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

// demux
//Pin connected to ST_CP of 74HC595
int latchPin = 2;
//Pin connected to SH_CP of 74HC595
int clockPin = 4;
////Pin connected to DS of 74HC595
int dataPin = 7;
int maxLED = 1000;
int perLED = maxLED/8;
byte data = 0;

// servo
Servo futaba;
int servoMax = 300;
int servoMin = 150;

// LCD
const int LCDlength = 16;
const int LCDwidth = 2;

// baud rate
const int commRate = 9600;

// 10bit to Volt conversion
const double volt = 0.0049;

////////////////////////////
// allocate Arduino ports //
////////////////////////////

// analog
/////////

const int potHand = A3;
const int potMotor = A0;
const int mux = A2;
const int buttonMin = A4;
const int buttonPlus = A5;


// digital pins
///////////////

// lcd
const int RS = 12;
const int E = 11;
const int DB4 = 10;
const int DB6 = 9;
const int DB7 = 8;
const int V0 = 5;
const int DB5 = 0;
// servo
const int servo = 3;

// now map the Arduino ports used for the lcd by creating an instance of the LiquidCrystal library
//LiquidCrystal lcd(RS,E,DB4,DB5,DB6,DB7);
LiquidCrystal lcd(12,11,5,4,3,2);

// declare variables
int level;
int oldlevel;
int levelMax = 20;
int levelMin = 1;
int score;
int scoreMax = 100;
int scoreMin = 0;
// positions on LCD
int levelPos = 0;
int scorePos = 4;
int posRow = 0;

// processing score
int lowerBound = 20;
int higherBound = 80;
boolean rising = true;

// EEPROM addresses
int levelAddress = 0;
int scoreAddress = 1;
int firstStartAddress = 2;

// timers denounced with "t" prefix
// write to EEPROM every x 10 seconds
unsigned long tStorage = 1;
// delay for dialog text
int tDialog = 2000;
int tDialogSteps = 100;
long tReset = 0;
long tDiff = 0;

boolean resetFlag = false;
boolean diffFlag = false;

// initializing sequence
int firstStart = 1;

// define progress bar characters
byte progressChars[8][8] = {
  
  {
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  },

  {
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B11111,
  },
  
  {
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B11111,
  B11111,
  },
  
  {
  B00000,
  B00000,
  B00000,
  B00000,
  B11111,
  B11111,
  B11111,
  },
  
  {
  B00000,
  B00000,
  B00000,
  B11111,
  B11111,
  B11111,
  B11111,
  },
  
  {
  B00000,
  B00000,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  },
  
  {
  B00000,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  },
  
  {
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  }
};

void setup()
{
  Serial.begin(commRate);
  lcd.begin(LCDlength,LCDwidth);
  
  pinMode(latchPin, OUTPUT);
  
  futaba.attach(servo);
  
  welcome();
  
  //initServo();
  
  processEEPROM();
  
  if(firstStart)
  {
    tutorial();
    firstStart = 0;
    EEPROM.write(firstStartAddress,firstStart);
  }
  
  initialize();
}

// process data from EEPROM
void processEEPROM()
{
  // read previous level from EEPROM
  level = EEPROM.read(levelAddress);
  
  // read previous score from EEPROM
  score = EEPROM.read(scoreAddress);
  
  // started before?
  firstStart = EEPROM.read(firstStartAddress);
  
  if(level > levelMax)
  {
    level = levelMax;
  }
    
  if(level < levelMin)
  {
    level = levelMin;
  }
  
  if(score > scoreMax)
  {
    score = scoreMax;
  }
  
  if(score < scoreMin)
  {
    score = scoreMin;
  }
}

// store level & score
void storeVariables()
{
  EEPROM.write(levelAddress, level);
  EEPROM.write(scoreAddress, score);
}

void welcome()
{
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Welcome to the");
  delay(2000);
  
  lcd.clear();
  
  lcd.setCursor(1,0);
  lcd.print("rehabilitation");
  lcd.setCursor(5,1);
  lcd.print("robot!");
  
  delay(2000);
  lcd.clear();
}

// progress bar method
void showProgress(int progressTmp)
{
  for(int i=0; i<=7; i++){    
    byte temp[8];
     
    for(int k=0; k<=7; k++) temp[k] = progressChars[i][k];
     
    lcd.createChar(i, temp);
  }
    
  // lcd has 2 rows (0-1) and 16 columns (0-15)
  lcd.begin(LCDlength, LCDwidth);
  
  // set cursor at row 1, column 0 where progress bar starts
  lcd.setCursor(0,1);
  
  // only lower row is needed
  if(progressTmp<=7)
  {
    // write all 8 characters of progress bar to first 8 columns of row 1
    for(int i=0; i<=progressTmp;i++){
      lcd.write(i); 
    }
  }
  
  // upper row is also needed
  if(progressTmp>7 && progressTmp <=14)
  {  
    // write all 8 characters of progress bar to first 8 columns of row 1
    for(int i=0; i<=7;i++){
      lcd.write(i); 
    }
      
    // write rest of progress bar with combination of second and first row
    for(int i=0; i<=(progressTmp-7);i++){
      // for row 1, fill the appropriate black rectangles
      // set cursor at right place on row 0
      lcd.setCursor(8+i,1);
      // write char (full rectangle)
      lcd.write(7);
      
      // for row 0, column 8 - 15 fill with progress bar characters 0 - 7
      // set cursor at first row
      lcd.setCursor(8+i,0);
      // write char
      lcd.write(i+1);
      
      if(progressTmp==14)
      {
        lcd.setCursor(15,0);
        lcd.write(7);
      }
    }
  }
}

// show on two lines if string larger than 16 characters
void multiLine(char* cOne,char* cTwo)
{
  String one = String(cOne);
  String two = String(cTwo);
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(one);
  lcd.setCursor(0,1);
  lcd.print(two);
}
  
  
// scroll text if string larger than 16 characters
void scroll(char* string)
{
  boolean exitLoop = false;
  int scrollDelay = 250;
  String scrollString = String(string);
  int StrLength = scrollString.length();
  
  lcd.print(scrollString);
  
  while(true)
  {
    if(StrLength > 16)
    {
      int Overflow = StrLength - 16;
      
      for(int i=0; i<Overflow; i++)
      {
        lcd.scrollDisplayLeft();
        delay(scrollDelay);
        if(analogRead(buttonMin)*volt > (double)5)
        {
          exitLoop = true;
          break;
        }   
      }
      
      if(exitLoop)
      {
        break;
      }
      
      delay(1000);
      
      for(int i=1; i<=Overflow; i++)
      {
        lcd.scrollDisplayRight();
        delay(scrollDelay);
        if(analogRead(buttonMin)*volt > (double)5)
        {
          exitLoop = true;
          break;
        }   
      }
            
      if(exitLoop)
      {
        break;
      }
    }
  }
}

void writeDisplay(int posCol, char* whichone, int value)
{
  lcd.setCursor(posCol,posRow);
  lcd.print((uint8_t)* whichone);
  lcd.print(value);
}

void checkDisplay(char* whichone)
{
  if(whichone == "L")
  {
      writeDisplay(levelPos, "L", level);
  }
    
  if(whichone == "S")
  {
      writeDisplay(scorePos, "S", score);
  }    
}

// "getters"
int getHandPos(int lower, int higher)
{
  int pos = analogRead(potHand);
  pos = map(pos, 0, 1023, lower, higher);
  
  return pos;
}

int getMotorPos()
{
  int pos = analogRead(potMotor);
  pos = map(pos, 0, 1023, 1, 10);
  
  return pos;
}

///////////
// Demux //
///////////

void demux()
{
  int pos = analogRead(potHand);
  
  if(pos<maxLED && pos>(maxLED-perLED))
  {
    digitalWrite(latchPin, 0);
    lightShiftPin(0);
    digitalWrite(latchPin, 1);
  }
  
  for(int i=7;i>=1;i--)
  {
    int counter = 8-i;
    int upper = maxLED-perLED*counter;
    int lower = maxLED-perLED*(counter+1);
    if(pos<(upper) && pos>(lower))
    {
      digitalWrite(latchPin, 0);
      lightShiftPin(i);
      digitalWrite(latchPin, 1);
    }
  }
}

// externally imported procedure
void lightShiftPin(int p) {
  //defines a local variable
  int pin;

  //this is line uses a bitwise operator
  //shifting a bit left using << is the same
  //as multiplying the decimal number by two. 
  pin = 1<< p;

  //move 'em out
  shiftOut(dataPin, clockPin, pin);   
}

// externally imported procedure
void shiftOut(int myDataPin, int myClockPin, byte myDataOut) {
  // This shifts 8 bits out MSB first, 
  //on the rising edge of the clock,
  //clock idles low

  //internal function setup
  int i=0;
  int pinState;
  pinMode(myClockPin, OUTPUT);
  pinMode(myDataPin, OUTPUT);

  //clear everything out just in case to
  //prepare shift register for bit shifting
  digitalWrite(myDataPin, 0);
  digitalWrite(myClockPin, 0);

  //NOTICE THAT WE ARE COUNTING DOWN in our for loop
  //This means that %00000001 or "1" will go through such
  //that it will be pin Q0 that lights. 
  for (i=7; i>=0; i--)  {
    digitalWrite(myClockPin, 0);

    //if the value passed to myDataOut and a bitmask result 
    // true then... so if we are at i=6 and our value is
    // %11010100 it would the code compares it to %01000000 
    // and proceeds to set pinState to 1.
    if ( myDataOut & (1<<i) ) {
      pinState= 1;
    }
    else {	
      pinState= 0;
    }

    //Sets the pin to HIGH or LOW depending on pinState
    digitalWrite(myDataPin, pinState);
    //register shifts bits on upstroke of clock pin  
    digitalWrite(myClockPin, 1);
    //zero the data pin after shift to prevent bleed through
    digitalWrite(myDataPin, 0);
  }

  //stop shifting
  digitalWrite(myClockPin, 0);
}

///////////
// Servo //
///////////

void initServo()
{
  int pos = analogRead(potMotor);

  while(pos > 150)
  {
    goDown();
  }
  
  stay();  
}

void setMotorPos()
{
  int pos = analogRead(potMotor);
  pos = constrain(pos,servoMin,servoMax);
  pos = map(pos,servoMin,servoMax,1,10);
  
  int target = servoMin + level*(levelMax-levelMin);
  
  int counter = 0;
  
  while(analogRead(potMotor) < target)
  {
    goUp();
    
    // counter++;
    // failsafe
    // if(counter > limit)
    // {
    //   break;
    // }
  }
  
  stay();
}

void stay()
{
  futaba.write(90);
}

void goUp()
{
  futaba.write(0);
}

void goDown()
{
  futaba.write(180);
}

// process hand position and thus update score
void processScore(int progressTmp)
{  
  if((progressTmp>higherBound) && rising == true)
  {
    score++;
    rising = false;
  }
  
  if((progressTmp<lowerBound) && rising == false)
  {
    rising = true;
  }
}

void checkDiff()
{
  Serial.println(millis());
  Serial.println(tDiff);
  if(diffFlag == false && (millis()-tDiff)>500)
  {
    diffFlag = true;
    tDiff = millis();
  }
  
  if((diffFlag == true) && (millis()-tDiff)<500)
  {
    diffFlag = false;
  }
}

// check for button activity
void checkbuttons()
{   
  double buttonMinValue = analogRead(buttonMin)*volt;
  double buttonPlusValue = analogRead(buttonPlus)*volt;
  
//  Serial.println(buttonMinValue);
//  Serial.println(buttonPlusValue);
//  delay(200);
  
  if((buttonMinValue > (double)5) && level > levelMin)
  {
    level--;
  }
  
  if((buttonPlusValue > (double)5) && level < levelMax)
  {
//    checkDiff();
//    if(diffFlag)
//    {
    level++;
//    }
  }
}

// function to set difficulty
void setDiff()
{
  lcd.clear();
  lcd.setCursor(0,0);
  multiLine("OK, please set the","difficulty...");
  delay(1000);
  
  long timeOut = 0;
  long timeOutSteps = 100;
  
  while(timeOut<(long)5000)
  { 
    checkbuttons();   
    if(level!=oldlevel)
    {
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Difficulty: ");
      lcd.setCursor(14,1);
      lcd.print(level);
      timeOut=0;
    }
    delay(timeOutSteps);
    timeOut+=timeOutSteps;
    oldlevel = level;
  }
}

// displayed at first run
void tutorial()
{  
  boolean exitLoop = false;
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Welcome!");
  delay(1000);  
  lcd.clear();
  lcd.setCursor(0,0);

  while(true)
  {
    multiLine("Press the","+-button to...");
    
    for(int i=0;i<=tDialog;i=i+tDialogSteps)
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    lcd.clear();
    
    if(exitLoop)
    {
      break;
    }
      
    multiLine("continue during","this tutorial.");
    for(int i=0;i<=tDialog;i=i+tDialogSteps)
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
  }
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Great!");
  delay(tDialog);
  
  lcd.setCursor(0,1);
  multiLine("Let's move on","to the buttons.");
  delay(tDialog*3);
  lcd.clear();
  
  exitLoop = false;
  
  while(true)
  {
    multiLine("While using","the device...");
    for(int i=0;i<=tDialog;i=i+tDialogSteps)
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
    
    multiLine("use the buttons","below the screen");
    for(int i=0;i<=tDialog;i=i+tDialogSteps)
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
    
    multiLine("to set the", "difficulty.");
    for(int i=0;i<=tDialog;i=i+tDialogSteps)
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      exitLoop = false;
      break;
    }
    
    lcd.clear();
  }
  
  while(true)
  {
    multiLine("Hold buttons","simultaneously");
    for(int i=0;i<=tDialog*100;i=i+(tDialogSteps))
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
    
    multiLine("to reset","your score.");
    for(int i=0;i<=tDialog*100;i=i+(tDialogSteps))
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
  }
  
  lcd.clear();
  multiLine("Let's talk","about the screen.");
  delay(tDialog);
  
  while(true)
  {
    multiLine("The screen","shows 3 things:");
    for(int i=0;i<=tDialog*100;i=i+(tDialogSteps))
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
    
    multiLine("Score, level &","current progress");
    for(int i=0;i<=tDialog*100;i=i+(tDialogSteps))
    {
      if(analogRead(buttonMin)*volt > (double)5)
      {
        exitLoop=true;
        break;
      }   
      delay(tDialogSteps);
    }
    
    if(exitLoop)
    {
      break;
    }
    
    lcd.clear();
  }
}

void initialize()
{
  boolean exitLoop = false;
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Initializing...");
  delay(500);
  lcd.clear();
  
  if(level!=0)
  {
    multiLine("Previous set","difficulty:");
    lcd.setCursor(14,1);
    lcd.print(level);
    
    delay(tDialog*2);
    
    while(true)
    {
      multiLine("\'-\': use this","\'+\': define new");
      
      for(int i=0;i<=tDialog;i=i+tDialogSteps)
      {
        Serial.println(analogRead(buttonMin)*volt);
        delay(500);
        if(analogRead(buttonMin)*volt > (double)5)
        { 
          exitLoop=true;
          break;
        }   
        
        Serial.println(analogRead(buttonPlus)*volt);
        delay(500);
        if(analogRead(buttonPlus)*volt > (double)5)
        {
          setDiff();
          setMotorPos();
          exitLoop=true;
          break;
        }   
        delay(tDialogSteps);
      }
      
      if(exitLoop)
      {
        storeVariables();
        break;
      }
    }
  }
  
  lcd.clear();
  delay(1000);
}

void loop() 
{  
  oldlevel = level;
//  Serial.print("score:"); Serial.println(score);
//  Serial.print("difficulty: "); Serial.println(level);
//  delay(2000);
  
  int temp = random(0,100);
  
  showProgress(map(temp,0,100,0,15));
  
//  showProgress(getHandPos(0,15));

//  processScore(getHandPos(0,100));

  demux();

  processScore(temp);
    
  checkbuttons();
  
  checkDisplay("L");
  checkDisplay("S");
  
  if((analogRead(buttonMin)*volt>(double)5)&&(analogRead(buttonPlus)*volt>(double)5))
  {
    if(!resetFlag)
    {
      resetFlag = true;
      tReset = millis();
    }

    if(resetFlag && (millis() - tReset) > 2000)
    {
      score = 0;
    }
  }
  
  if(level!=oldlevel)
  {
    setMotorPos();
  }
    
  if((millis()%(tStorage*10000))<=1000)
  {
    storeVariables();
    Serial.println("variables written to EEPROM");
  }
  
  delay(500);
}