#include <AccelStepper.h>
#include <MultiStepper.h>

long gittSteg = 0;
long gittFart = 0;
long gittAkkselerasjon = 0;
char gittKommando;

int retningsGiver = 1; // 1 er + retning og -1 er - retning
bool newData, runAllowed=false; // flag for nydata og tillating av kjøring 

const int stepperAmount = 2;
int positionArray[]={2000};


byte ledPin = 13;   // the onboard LED

// stepper ( AccelStepper::Drifttyper(eks:fullstep, Driver),Pin for puls, Pin for Retnings skifte)
AccelStepper stepper1 (AccelStepper::DRIVER,2,5);
AccelStepper stepper2 (AccelStepper::DRIVER,3,6);



AccelStepper* steppers[stepperAmount] ={
    
    &stepper1,
    &stepper2,
};

//-----------------------------------------------------------------------------------------------------

void setup(){
  Serial.begin(115200);
  Serial.println("Hello Arduino\n");
  // Default Hastighet 400 steps per sekund; Default Akkselerasjon 100 steps per sekund^2;
  for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++){
        steppers[stepperNumber]->setMinPulseWidth(20);
        steppers[stepperNumber]->setMaxSpeed(400);
        steppers[stepperNumber]->setAcceleration(100);
        steppers[stepperNumber]->disableOutputs(); //disable outputs
    }    
}

void loop(){
  
    recvWithStartEndMarkers();
    RunMotor();
   
}


//===============

void RunMotor()//Motor Driver
{
  if(runAllowed==true)
  {
      for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++){
          steppers[stepperNumber]->enableOutputs();
          steppers[stepperNumber]->run();
      }
  }  
  else
  {
    for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++){
       steppers[stepperNumber]->disableOutputs();
    }
  return; 
  }
 } 


//-----------------


//===============

void replyToPython() {
    if (newData == true) {
        Serial.print("<recieved: ");
        Serial.print(gittKommando);
        Serial.print("   ");
        Serial.print(millis());
        Serial.print('>');
            // change the state of the LED everytime a reply is sent
        digitalWrite(ledPin, ! digitalRead(ledPin));
        newData = false;
    }
}

//===============

void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;

    if (Serial.available() > 0)
    {
      gittKommando = Serial.read();
      newData = true; 

      if (newData == true)
      {
        switch (gittKommando)
        {
          case 'P': 

          gittSteg = Serial.parseFloat();
          gittFart = Serial.parseFloat();
          retningsGiver = 1;
          Serial.println("Beveger i + retning");
          RelativBevegelse();
          break; 

          //----------------------------//
          
          case 'N':
          gittSteg = Serial.parseFloat();
          gittFart = Serial.parseFloat();
          retningsGiver = -1;
          Serial.println("Beveger i - retning");
          RelativBevegelse();
          break;
          
          //----------------------------//

          case'A':
          gittSteg = Serial.parseFloat();
          gittFart = Serial.parseFloat();
          retningsGiver = 1;
          Serial.println("Beveges til oppgitt posisjon");
          AbsoluttBevegelse();
          break;
          
          //----------------------------//

          case 'S':
          for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++){
          steppers[stepperNumber]->stop();
          steppers[stepperNumber]->disableOutputs();
          Serial.println("Stopped");
          }

          default:

            break;
          

  
        }   
      }
  
        newData=false;

      
     }
}


//-----------------

void RelativBevegelse()
{
    runAllowed = true;

    for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++)
    {
        steppers[stepperNumber]->setMaxSpeed(gittFart);
        steppers[stepperNumber]->move(gittSteg*retningsGiver);
    }
 }
  
//-----------------
//-----------------

void AbsoluttBevegelse()
{
  runAllowed = true;
  for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++)
    {
        steppers[stepperNumber]->setMaxSpeed(gittFart);
        steppers[stepperNumber]->moveTo(gittSteg*retningsGiver);
     }
  
}

//-----------------
//-----------------


void BevegHjem()
{

   for(int stepperNumber = 0; stepperNumber < stepperAmount; stepperNumber++)
    {
      if (steppers[stepperNumber]->currentPosition()==0)
      {
        Serial.println("Er hjemme.");
        steppers[stepperNumber]->disableOutputs();
      }
        steppers[stepperNumber]->setMaxSpeed(1000);
        steppers[stepperNumber]->moveTo(0);
     }
}
  


  
//-----------------
//-----------------