#include <Wire.h>
#include <LiquidCrystal.h>
#include <Adafruit_PWMServoDriver.h>
#include "Adafruit_MCP23017.h"

Adafruit_MCP23017 mcp;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
LiquidCrystal lcd(2, 3, 4, 5, 6, 7);


#define SERVOMIN  150
#define SERVOMAX  600

#define FRULEG 0
#define FLULEG 1
#define RRULEG 2
#define RLULEG 3

#define FRLLEG 4
#define FLLLEG 5
#define RRLLEG 6
#define RLLLEG 7

#define FTURN 8
#define RTURN 9



const int way1Pin = 8;                //check
const int way2Pin = 9;
const int leverjoy = 8;
const int leftjoy = 9;
const int rightjoy = 10;               ///mcp   inputs
const int middlejoy = 11;
const int frontjoy = 12;
const int frontsensPin = 13;
const int rearsensPin = 14;
const int tempsens = A0;

int fufframes[4] = {- 5, - 2, + 2, + 10};                                   //FrontUpForward
int fubframes[11] = {+ 5, + 4, + 3, + 4, + 5, 0, - 1, - 2, - 3, - 4, - 5}; //FrontUpBackward
int fdfframes[4] = {10, 20, 30, 10};                                          //FrontDownForward
int fdbframes[11] = {0, 0, - 2, - 4, - 6, - 4, - 2, -4, - 8, - 3, 0};                       //FrontDownBackward

int rufframes[4] = {- 5, - 2, + 2, + 10};                    //               //RearUpForward
int rubframes[11] = {+ 5, + 4, + 3, + 4, + 5, 0, - 1, - 2, - 2, - 4, - 5}; //RearUpBackward
int rdfframes[4] = {10, 20, 30, 10};                                          //RearDownForward
int rdbframes[11] = {0, 0, - 2, - 4, - 6, - 4, - 2, - 4, - 8, - 3, 0};                       //RearDownBackward


int servomid[13] = {82, 90, 85, 83,   87, 106, 90, 83,   90, 90,   90, 90,   90};/// in degrees
int turndeg = 0;
int walkspeed = 0;
int startpos[13];                ////in puselenght
int curservopos[13];             //// in degrees
int inbyte = 0;
int lastturndeg = 0;
int legcounter = 0;
int key = -1;

long temptime = 0;

byte passwordlength = 5;
byte slavePassword[5];
byte masterPassword[5] = {3, 9, 8, 1, 5};
byte denailcount = 5;
byte firstmenucounter  = 1;
byte walkwidth = 6;

boolean butstate = 0;
boolean fronttouch = 0;
boolean reartouch = 0;
boolean kneeing = 0;
boolean legup = 0;
boolean autopilot = 0;
boolean stopwalk = 0;
boolean laying = 0;
boolean loginok = 0;

unsigned long lasttime = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("---------------------------------------------");
  Serial.println("Starting Arduino Setup...");
  Serial.println("");
  pwm.begin();
  lcd.begin(16, 2);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Booting");

  mcp.begin();



  mcp.pinMode(0, INPUT);  // reads row 1
  mcp.pullUp(0, HIGH);

  mcp.pinMode(1, INPUT);  // reads row 2
  mcp.pullUp(1, HIGH);

  mcp.pinMode(2, INPUT);  // reads row 3
  mcp.pullUp(2, HIGH);

  mcp.pinMode(3, INPUT);  // reads row 4
  mcp.pullUp(3, HIGH);

  mcp.pinMode(4, INPUT);  // controls column directions
  mcp.pinMode(5, INPUT);  // controls column 147*
  mcp.pinMode(6, INPUT);  // controls column 2580
  mcp.pinMode(7, INPUT);  // controls column 369#


  mcp.pinMode(rightjoy, INPUT);
  mcp.pinMode(leftjoy, INPUT);
  mcp.pinMode(middlejoy, INPUT);
  mcp.pinMode(frontjoy, INPUT);
  mcp.pinMode(leverjoy, INPUT);

  mcp.pullUp(rightjoy, HIGH);    //
  mcp.pullUp(leftjoy, HIGH);     //
  mcp.pullUp(middlejoy, HIGH);   //
  mcp.pullUp(frontjoy, HIGH);    //
  mcp.pullUp(leverjoy, HIGH); //

  mcp.pinMode(frontsensPin, INPUT);
  mcp.pullUp(frontsensPin, HIGH);
  mcp.pinMode(rearsensPin, INPUT);
  mcp.pullUp(rearsensPin, HIGH);


  pinMode(way1Pin, OUTPUT);
  pinMode(way2Pin, OUTPUT);
  pinMode(13, OUTPUT);

  pwm.setPWMFreq(60);

  Serial.println("Choose one option :");
  Serial.println("1. servosetup");
  Serial.println("2. Knee");
  Serial.println("");

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Choose :");

  lasttime = millis();

  do {
    firstmenuupdate();
  } while (mcp.digitalRead(leftjoy) == 1 & mcp.digitalRead(rightjoy) == 1);

  if (mcp.digitalRead(leftjoy) == 0) {
    Serial.println("Starting Servosetup...");
    Serial.println("");
    servosetup();
  } else if (mcp.digitalRead(rightjoy) == 0) {
    kneeing = 1;
    curservopos[4] = servomid[4] + 80;
    curservopos[5] = servomid[5] - 80;
    curservopos[6] = servomid[6] + 80;
    curservopos[7] = servomid[7] - 80;

    for (int x = 0; x < 4; x++) {
      curservopos[x] = servomid[x];
    }
    Serial.println("Starting knee...");
    Serial.println("");
    knee();
  }

  /*
  Serial.println("Starting Login...");
  Serial.println("");
  login();
  lcd.clear();
  if (loginok == 0){
    lcd.setCursor(0, 0);
    lcd.print("Access denailed!");
    lcd.setCursor(0, 1);
    lcd.print("Shutting down!");
    while(0 == 0){
      digitalWrite(13, LOW);
      delay(300);
      digitalWrite(13, HIGH);
      delay(300);
    }
  }
  */

  delay(750);
  lcd.clear();
  Serial.println("Arduino Setup over!");
  Serial.println("---------------------------------------------");
  
  temptime = -1;
  tempupdate();
}
void loop() {
  joyupdate();
  keyupdate();
  tempupdate();

  delay(50);
}

void tempupdate(){
  if ((temptime + 1000) < millis() + 30 & (temptime + 1000) > millis() - 30 || temptime == - 1){
    temptime = millis();
    
    int value = analogRead(tempsens);
  
    float millivolts = (value / 1024.0) * 5000;
    float celsius = millivolts / 10;
  
    lcd.setCursor(0, 0);
    lcd.print("                ");
    lcd.setCursor(0, 0);
    lcd.print("Temp. : ");
    lcd.print(celsius);
  
  }
}

void servosetup() {
  for (int x = 0; x < 13; x++) {
    startpos[x] = map(servomid[x], 0, 180, SERVOMIN, SERVOMAX);
  }
  startpos[0] = startpos[0] + 20;
  startpos[1] = startpos[1] - 20;
  startpos[2] = startpos[2] - 20;
  startpos[3] = startpos[3] + 20;


  pwm.setPWM(0, 0, startpos[0]);
  pwm.setPWM(1, 0, startpos[1]);
  pwm.setPWM(2, 0, startpos[2]);
  pwm.setPWM(3, 0, startpos[3]);

  for (int x = 4; x < 13; x++) {
    pwm.setPWM(x, 0, startpos[x]);
  }



  for (int x = 0; x < 13; x++) {
    curservopos[x] = map(startpos[x], SERVOMIN, SERVOMAX, 0, 180);
  }

  Serial.println("Servomid :");
  Serial.println("");
  Serial.println("");

  for (int x = 0; x < 13; x++) {
    Serial.print(x);
    Serial.print(": ");
    Serial.println(servomid[x]);
  }
  Serial.println("");

  Serial.println("Servo Startpos :");
  Serial.println("");
  Serial.println("");

  for (int x = 0; x < 13; x++) {
    Serial.print(x);
    Serial.print(": ");
    Serial.println(startpos[x]);
  }
  Serial.println("");
  Serial.println("Servo Setup over!");
  Serial.println("");
  Serial.println("");
}


void joyupdate() {
  turn();
  walkspeed = analogRead(A2);

  if (walkspeed > 550) {
    schritt();
  }

  if (mcp.digitalRead(rightjoy) == 0) {
    
  }
  if (mcp.digitalRead(leftjoy) == 0) {
    bridge();

  }
  if (mcp.digitalRead(middlejoy) == 0) {
    knee();
  }
  if (mcp.digitalRead(leverjoy) == 0) {
    autopilot = 1;
  }
  else {
    autopilot = 0;
  }
}

void keyupdate() {

}

void turn() {
  int frontpos = 0;
  int rearpos = 0;
  
  turndeg = analogRead(A3);

  if (autopilot == 0) {
    frontpos = map(turndeg, 0, 1023, 310, 440);
    rearpos = map(turndeg, 0, 1023, 440, 310);
    pwm.setPWM(FTURN, 0, frontpos);
    pwm.setPWM(RTURN, 0, rearpos);

    lastturndeg = turndeg;
  }
  else if (autopilot == 1) {
    frontpos = map(lastturndeg, 0, 1023, 300, 450);
    rearpos = map(lastturndeg, 0, 1023, 450, 300);
    pwm.setPWM(FTURN, 0, frontpos);
    pwm.setPWM(RTURN, 0, rearpos);
  }


  lcd.setCursor(0, 1);
  lcd.print("Turn Deg: ");
  lcd.setCursor(9, 1);
  lcd.print("      ");
  delay(10);
  lcd.setCursor(9, 1);
  lcd.print(map(frontpos, 300, 450, -45, 45));

}

void turnservo(int Degrees, int servonum, int delaytime, int Speed) {
  int Aim = map(Degrees, 0, 180, SERVOMIN, SERVOMAX);
  int curpulse = map(curservopos[servonum], 0, 180, SERVOMIN, SERVOMAX);

  if (curpulse < Aim) {
    for (int pulselength = curpulse; pulselength < Aim; pulselength++) {
      pwm.setPWM(servonum, 0, pulselength);
      delay(Speed);
    }
  }
  else if (curpulse > Aim) {
    for (int pulselength = curpulse; pulselength > Aim; pulselength--) {
      pwm.setPWM(servonum, 0, pulselength);
      delay(Speed);
    }
  }
  curservopos[servonum] = Degrees;

  if (delaytime > 0) delay(delaytime);
}

int getkey() {
  int x = 0;
  int y = 0;

  for (int i = 0; i < 4; i++) {
    mcp.pinMode(i + 4, OUTPUT);
    mcp.digitalWrite(i + 4, LOW);  // create a GND connection for columns

    if (LOW == mcp.digitalRead(0)) {
      x = i + 1;
      y = 1;
    }
    if (LOW == mcp.digitalRead(1)) {
      x = i + 1;
      y = 2;
    }
    if (LOW == mcp.digitalRead(2)) {
      x = i + 1;
      y = 3;
    }
    if (LOW == mcp.digitalRead(3)) {
      x = i + 1;
      y = 4;
    }

    mcp.pinMode(i + 4, INPUT);  // disconnect GND from columns
  }

  if (x == 1 && y == 1) {
    return 12;
  }
  else if (x == 1 && y == 2) {
    return 13;
  }
  else if (x == 1 && y == 3) {
    return 14;
  }
  else if (x == 1 && y == 4) {
    return 15;
  }
  else

    if (x == 2 && y == 1) {
      return 1;
    }
    else if (x == 2 && y == 2) {
      return 4;
    }
    else if (x == 2 && y == 3) {
      return 7;
    }
    else if (x == 2 && y == 4) {
      return 10;
    }
    else

      if (x == 3 && y == 1) {
        return 2;
      }
      else if (x == 3 && y == 2) {
        return 5;
      }
      else if (x == 3 && y == 3) {
        return 8;
      }
      else if (x == 3 && y == 4) {
        return 0;
      }
      else

        if (x == 4 && y == 1) {
          return 3;
        }
        else if (x == 4 && y == 2) {
          return 6;
        }
        else if (x == 4 && y == 3) {
          return 9;
        }
        else if (x == 4 && y == 4) {
          return 11;
        }

  if (x == 0 && y == 0) {
    return -1;
  }
}

void login() {
  int key = getkey();
  byte lcdcursor = 0;

  for (int x = 0; x < 5; x++) {
    lcd.setCursor(0, 0);
    lcd.print("Please enter");
    lcd.setCursor(0, 1);
    lcd.print("your Password");
    while (key == -1) {
      key = getkey();
    }
    lcd.clear();

    for (int keycount = 0; keycount < passwordlength; keycount++) {
      loginok = 1;
      while (key == -1) {
        key = getkey();
      }
      lcd.setCursor(lcdcursor, 0);
      lcd.print("*");
      lcdcursor++;
      slavePassword[keycount] = key;
      key = -1;
      delay(200);
    }

    for (int control = 0; control < passwordlength; control++) {
      if (slavePassword[control] != masterPassword[control]) {
        loginok = 0;
      }
    }
    if (loginok == 1) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Login succesful!");
      lcd.setCursor(0, 1);
      lcd.print("Welcome!");
      while (key == -1) {
        key = getkey();
      }
      return;
    } else if (loginok == 0) {
      denailcount--;
      if (denailcount == 0) {
        loginok = 0;
        return;
      }
      lcdcursor = 0;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Wrong Password");
      lcd.setCursor(0, 1);
      lcd.print("Left trys:");
      lcd.setCursor(11, 1);
      lcd.print(denailcount);
      while (key == -1) {
        key = getkey();
      }
      lcd.clear();
    }
  }
}

void knee() {
  if (kneeing == 1) {
    Serial.println("Standing up...");
    Serial.println("");

    for (int x = 0; x < 8; x++) {
      turnservo(servomid[x] , x, 0, 2);
    }

    servosetup();

    kneeing = 0;
  }
  else if (kneeing == 0) {
    Serial.println("Knee down...");
    Serial.println("");
    for (int x = 0; x < 4; x++) {
      turnservo(servomid[x], x, 0, 2);
    }
    for (int x = 0; x < 80; x++) {
      turnservo(servomid[4] + x, 4, 0, 1);
      turnservo(servomid[5] - x, 5, 0, 1);
      turnservo(servomid[6] + x, 6, 0, 1);
      turnservo(servomid[7] - x, 7, 0, 1);
    }


    kneeing = 1;
  }


  Serial.println("Knee over!");
  Serial.println("");
}



void bridge() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Open bridge ...");

  digitalWrite(13, HIGH);

  fronttouch = mcp.digitalRead(frontsensPin);
  while (fronttouch == 1) {
    digitalWrite(way1Pin, HIGH);
    digitalWrite(way2Pin, LOW);
    fronttouch = mcp.digitalRead(frontsensPin);
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Bridge opened!");

  digitalWrite(way1Pin, LOW);
  digitalWrite(way2Pin, LOW);

  while (mcp.digitalRead(leftjoy) == 1);

  reartouch = mcp.digitalRead(rearsensPin);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Close bridge ...");
  while (reartouch == 1) {
    digitalWrite(way2Pin, HIGH);
    digitalWrite(way1Pin, LOW);
    reartouch = mcp.digitalRead(rearsensPin);
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Bridge closed!");

  digitalWrite(13, LOW);

  digitalWrite(way1Pin, LOW);
  digitalWrite(way2Pin, LOW);

  delay(500);

  lcd.clear();
}

void stretchlegs() {
  for (int x = 0; x < 8; x++) {
    turnservo(servomid[x], x, 0, 2);
  }
}

void firstmenuupdate() {
  if (
    ((lasttime + 1000) < millis() + 10)
    &
    ((lasttime + 1000) > millis() - 10)
  ) {
    lasttime = millis();

    if (firstmenucounter == 1) {
      lcd.setCursor(0, 1);
      lcd.print("left: servosetup");
    } else if (firstmenucounter == 2) {
      lcd.setCursor(0, 1);
      lcd.print("right: needown  ");
    }

    if (firstmenucounter == 2) {
      firstmenucounter = 1;
    } else {
      firstmenucounter++;
    }
  }
}

void schritt() {
  int frontcounter = 0;
  int rearcounter = 4;
  int frcounter = 8;
  int flcounter = 0;
  int rrcounter = 12;
  int rlcounter = 4;
  
  for (int c = 0; c < (15); c++){
    Serial.print(c);
    Serial.print(", ");
    Serial.print(frcounter);
    Serial.print(", ");
    Serial.print(flcounter);
    Serial.print(", ");
    Serial.print(rrcounter);
    Serial.print(", ");
    Serial.println(rlcounter);
    
    frleg(frcounter);
    if (frcounter == 14){
      frcounter = 0;
    }else{
      frcounter++;
    }
    
    flleg(flcounter);
    if (flcounter == 14){
      flcounter = 0;
    }else{
      flcounter++;
    }
    
    rrleg(rrcounter);
    if (rrcounter == 14){
      rrcounter = 0;
    }else{
      rrcounter++;
    }
    
    rlleg(rlcounter);
    if (rlcounter == 14){
      rlcounter = 0;
    }else{
      rlcounter++;
    }
  }
  
}

void frleg(int frcounter){
  if (frcounter < 4){
    turnservo(servomid[0] + fufframes[frcounter], 0, 0, 4);
    turnservo(servomid[4] + fdfframes[frcounter], 4, 0, 2);
  }
  
  if (frcounter > 3){
    int counter = frcounter - 4;
    turnservo(servomid[0] + fubframes[counter], 0, 0, 12);
    turnservo(servomid[4] - fdbframes[counter], 4, 0, 6);
  }
}

void flleg(int flcounter){
  if (flcounter < 4){
    turnservo(servomid[1] - fufframes[flcounter], 1, 0, 4);
    turnservo(servomid[5] - fdfframes[flcounter], 5, 0, 2);
  }
  
  if (flcounter > 3){
    int counter = flcounter - 4;
    turnservo(servomid[1] - fubframes[counter], 1, 0, 12);
    turnservo(servomid[5] - fdbframes[counter], 5, 0, 6);
  }
}

void rrleg(int rrcounter){
  if (rrcounter < 4){
    turnservo(servomid[2] + rufframes[rrcounter], 2, 0, 4);
    turnservo(servomid[6] + rdfframes[rrcounter], 6, 0, 2);
  }
  
  if (rrcounter > 3){
    int counter = rrcounter - 4;
    turnservo(servomid[2] + rubframes[counter], 2, 0, 12);
    turnservo(servomid[6] - rdbframes[counter], 6, 0, 6);
  }
}

void rlleg(int rlcounter){
  if (rlcounter < 4){
    turnservo(servomid[3] - rufframes[rlcounter], 3, 0, 4);
    turnservo(servomid[7] - rdfframes[rlcounter], 7, 0, 2);
  }
  
  if (rlcounter > 3){
    int counter = rlcounter - 4;
    turnservo(servomid[3] - rubframes[counter], 3, 0, 12);
    turnservo(servomid[7] - rdbframes[counter], 7, 0, 6);
  }
}