#include <Servo.h>

String command; 

short         servo_Read         = 90 ;     // Potentiometer READ variable
short         servo_Read_Old     = 90;     // Potentiometer READ OLD variable
unsigned long  servo_Timer        = 0;     // Timer for update time of write of potentiometer
unsigned long servo_Timer_Delay  = 175;   // Timer delay for writing potmeter value 

int potPosition;
int servoPosition1 = 90;
int servoPosition2 = 90;

int STATE = 0;
int STATE_KNAPPER = 1;
int STATE_SERIAL = 2;

int numberFromCommand;
int numberFromCommandM2;


const int redButton = 11;
const int grnButton = 13;

Servo M1;
Servo M2;

void setup() {

  Serial.begin(9600);
   Serial.println("Vil du styre med knapper eller serial?");
 pinMode (redButton, INPUT_PULLUP);
 pinMode (grnButton, INPUT_PULLUP);
 M1.attach(9);
 M2.attach(6);

}

void loop() {
checkState();

if (STATE == STATE_KNAPPER)
{

stateKnapper();
}

else if (STATE == STATE_SERIAL)
{
  stateSerial();
}








}

void stateKnapper()
{
  potControl();
buttonControl();

}

void stateSerial(){
int x;
x = (numberFromCommand);

   while (Serial.available())
 {
   
   command = Serial.readString(); 
 }
 
 if (command.length() > 0)
 {
   int lastIndex1 = command.length() - 1;
   command.remove(lastIndex1); 
   numberFromCommand = atoi(command.c_str());
 }
 
if (command.length() > 0)
{
  M1.write(numberFromCommand);


command=""; 


if (numberFromCommand = !x)
{
  Serial.println ("Skriv inn servo 2 sin nye posisjon (0 - 180 grader)");

 while (Serial.available())
 {
   
   command = Serial.readString(); 
 }
 
 if (command.length() > 0)
 {
   int lastIndex2 = command.length() - 1;
   command.remove(lastIndex2); 
   numberFromCommandM2 = atoi(command.c_str());
 }
 
if (command.length() > 0)
{
  M2.write(numberFromCommandM2);


command=""; 


}
}

}


 
}


void checkState()
{
  while (Serial.available())
{
  command = Serial.readString();
}

 
 if (command.length() > 0)
 {
   int lastIndex = command.length() - 1;
   command.remove(lastIndex); 
 }
 
if (command.length() > 0)


  if (command == "knapper" ||  command == "knapp")
  {
STATE = STATE_KNAPPER;
Serial.println ("State = Knapper");
}
else if (command == "serial" || command == "seriell")
{
  STATE = STATE_SERIAL;
  Serial.println ("State = Serial");
  Serial.println("Skriv inn servoene sin nye posisjon (0-180 grader)");
}
else
{
  Serial.println("Skriv 'knapper' eller 'serial'");
}
command=""; 

}

void potControl()
{
  potPosition = analogRead(A0);

servoPosition2 = map(potPosition, 0, 1023, 0, 180);

M2.write(servoPosition2);
servo2_Write();
}

void buttonControl()
{
  while (digitalRead(grnButton)== LOW and servoPosition1 <= 179)
{
  
  servoPosition1 = servoPosition1+1;
  servo1_Write();
  delay(10);
  M1.write(servoPosition1);


}
while (digitalRead(redButton)== LOW and servoPosition1 >= 1)
{
  
  servoPosition1 = servoPosition1-1;
  servo1_Write();
  delay(10);
  M1.write(servoPosition1);

 
}

}

void servo1_Write()
{
  servo_Read = (servoPosition1);  // Set potmeter_Read value = to potentiometer real value
  
  // Write the potentiometer value to monitor with a delay and only if potmeter read value is different to the old potmeter value
  if (servo_Timer + servo_Timer_Delay <= millis() && (servo_Read - 2 >= servo_Read_Old || servo_Read + 2 <= servo_Read_Old))
  {
    servo_Timer = millis();                    // Set timer to millis()
    servo_Read_Old = servo_Read;            // Set potmeter_read_old to the potmeter_read value
    Serial.print("Servo 1 Position: ");                  
    Serial.println(servo_Read);
}
}

void servo2_Write()
{
  servo_Read = (servoPosition2);  // Set potmeter_Read value = to potentiometer real value
  
  // Write the potentiometer value to monitor with a delay and only if potmeter read value is different to the old potmeter value
  if (servo_Timer + servo_Timer_Delay <= millis() && (servo_Read - 2 >= servo_Read_Old || servo_Read + 2 <= servo_Read_Old))
  {
    servo_Timer = millis();                    // Set timer to millis()
    servo_Read_Old = servo_Read;            // Set potmeter_read_old to the potmeter_read value
    Serial.print("Servo 2 Position: ");                  
    Serial.println(servo_Read);
}
}