#include <Stepper.h>
#include <Keypad.h>

String verdi = "0";
float currentPosition1 = 0;
float currentPosition2 = 0;
int StepperValg = 0;

const int dirPin1 = 8;
const int stepPin1 = 9;
const int dirPin2 = 10;
const int stepPin2 = 11;
const int stepsPerRevolution = 1600;
int stepDelay = 500;
float VinkRat = ((stepsPerRevolution * 1.875) / 45); //1.875 er antall runder som blir 45grader

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '-', '.' }
};

uint8_t colPins[COLS] = { 5, 4, 3, 2 };
uint8_t rowPins[ROWS] = { 13, 12, 7, 6 };

Stepper myStepper1(stepsPerRevolution, stepPin1, dirPin1);
Stepper myStepper2(stepsPerRevolution, stepPin2, dirPin2);

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

void setup() {
  Serial.begin(9600);
  verdi = "0";
  currentPosition1 = -2;
  currentPosition2 = -2;
  StepperValg = 0;
  float goalPosition = verdi.toFloat();
  float stepsToMove1 = goalPosition - currentPosition1; // Beregn antall trinn som skal beveges
  float stepsToMove2 = goalPosition - currentPosition2; // Beregn antall trinn som skal beveges

  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(stepPin2, OUTPUT);

  if (stepsToMove1 < 0) {
    digitalWrite(dirPin1, LOW); // Sett retningen til lav for motsatt retning
    digitalWrite(dirPin2, LOW); // Sett retningen til lav for motsatt retning
    stepsToMove1 = -stepsToMove1; // Konverter negativ avstand til positiv
    } else {
    digitalWrite(dirPin1, HIGH); // Sett retningen til høy for normal retning
    digitalWrite(dirPin2, HIGH); // Sett retningen til høy for normal retning
    }

  for (int i = 0; i < stepsToMove1; i++) {
    digitalWrite(stepPin1, HIGH);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(stepDelay);
    digitalWrite(stepPin1, LOW);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(stepDelay);
  }
  currentPosition1 = -0;
  currentPosition2 = -0;
  verdi = "";
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    if (key == '*') {
      float goalPosition = verdi.toFloat();
      Serial.print("goalPosition: ");
      Serial.println(goalPosition);
      float stepsToMove1 = goalPosition - currentPosition1;
      float stepsToMove2 = ((goalPosition - currentPosition2) * VinkRat);

      if (StepperValg == 0 && stepsToMove1 < 0) {
        digitalWrite(dirPin1, LOW);
        digitalWrite(dirPin2, LOW);
        stepsToMove1 = -stepsToMove1;
      } else if (StepperValg == 1 && stepsToMove2 < 0) {
        digitalWrite(dirPin1, LOW);
        digitalWrite(dirPin2, LOW);
        stepsToMove2 = -stepsToMove2;
        stepsToMove2 = round(stepsToMove2);
      } else {
        digitalWrite(dirPin1, HIGH);
        digitalWrite(dirPin2, HIGH);
        stepsToMove2 = ceil(stepsToMove2);
      }

      if (StepperValg == 0) {
        for (int i = 0; i < stepsToMove1; i++) {
          digitalWrite(stepPin1, HIGH);
          delayMicroseconds(stepDelay);
          digitalWrite(stepPin1, LOW);
          delayMicroseconds(stepDelay);
          currentPosition1 = goalPosition;
        }
      } else {
        for (int i = 0; i < stepsToMove2; i++) {
          digitalWrite(stepPin2, HIGH);
          delayMicroseconds(stepDelay);
          digitalWrite(stepPin2, LOW);
          delayMicroseconds(stepDelay);
          currentPosition2 = goalPosition;
        }
      }


      verdi = "";
    } else if (key == 'A') {
      StepperValg = 0;
      Serial.println(StepperValg);
    } else if (key == 'B') {
      StepperValg = 1;
      Serial.println(StepperValg);
    } else {
      if (key == '.') {
        verdi += key; // Legg til punktum i strengen
      } else {
        verdi += key;
      }
      Serial.println(verdi);
    }
  }
}