import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.Pin;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;


class Schrittmotor implements Runnable{

  boolean direction;//true = cw false = ccw;
  double  acceleration;
  double speed;
  double maxSpeed;
  long start_pos;
  long target_pos;
  long current_pos;
  long step_counter;
  double ini_step_size;
  double last_step_size;
  double min_step_size;
  long stepInterval;
  long lastStepTime;
  long minPulseWidth;
  final GpioController gpio;
  final GpioPinDigitalOutput step_pin;
    final GpioPinDigitalOutput dir_pin;
  
  public void moveTo(long absolute) {
    if(target_pos!=absolute) {
      target_pos=absolute;
      computeNewSpeed();
    }
  }
  
  public void move(long relative) {
    moveTo(current_pos+relative);
  }
  
  private boolean runSpeed() {
    if(stepInterval==0) {
      return false;
    }
    long time = System.nanoTime()/1000;
    long nextStepTime= lastStepTime+stepInterval;
     if (   ((nextStepTime >= lastStepTime) && ((time >= nextStepTime) || (time < lastStepTime)))|| ((nextStepTime < lastStepTime) && ((time >= nextStepTime) && (time < lastStepTime)))) {
       if (direction) {
         current_pos++;
       }
       else {
         current_pos--;
         
       }
       step();
       lastStepTime=time;
       return true;
       
     }
     else {
       return false;
     }
        
    
  }

  public long distanceToGo()
  {
      return target_pos - current_pos;
  }
  
  public long targetPosition() {
    return target_pos;
  }
  
  public long currentPosition() {
    return current_pos;
    
  }
  
  public void setCurrentPosition(long Position) {
    target_pos = Position;
    current_pos = Position;
    step_counter=0;
    stepInterval=0;
    
    
  }
    
  private void computeNewSpeed() {
    long distanceTo=distanceToGo();
    long stepToStop=(long)((speed*speed)/(2.0*acceleration));
    if(distanceTo==0&&stepToStop<=1) {
      stepInterval=0;
      speed=0.0;
      step_counter=0;
      return;
      
    }
    if(distanceTo>0) {
      if(step_counter>0) {
        if((stepToStop>=distanceTo)||direction==false) {
          step_counter=-stepToStop;
        }
        
      }
      else {
        if(step_counter<0) {
          if((stepToStop<distanceTo)&&direction==true) {
            step_counter=-step_counter;
          }
        }
      }
      
    }
    else {
      if(distanceTo<0) {
        if(step_counter>0) {
          if((stepToStop>=-distanceTo)||direction==true) {
            step_counter=-stepToStop;
          }
        }
        else {
          if(step_counter<0) {
            if((stepToStop<-distanceTo)&&direction==false)  {
              step_counter=-step_counter;
            }
          }
        }
          
            
            
          
        
        
      }
    }
    if(step_counter==0) {
      last_step_size=ini_step_size;
      if(distanceTo>0) {
        direction=true;
        
      }
      else {
        direction=false;
      }
      
    }
    else {
      last_step_size=last_step_size-((2.0*last_step_size)/((4.0*step_counter)+1));
      if(last_step_size<min_step_size) {
        last_step_size=min_step_size;
      }
    }
    step_counter++;
    stepInterval=(long)(last_step_size);
    speed=1000000.0/last_step_size;
    if(!direction) {
      speed=-speed;
    }
    
    
  }
  
  public boolean lauf()
  {
      if (target_pos == current_pos)
    return false;
      
      if (runSpeed()) 
    computeNewSpeed();
      return true;
      
  }
  
  public Schrittmotor( Pin Step_pin, Pin Dir_pin ) {
    current_pos = 0;
      target_pos = 0;
      speed = 0.0;
      maxSpeed = 1.0;
      acceleration = 0.0;
      stepInterval = 0;
      minPulseWidth = 10;
      lastStepTime = 0;
      
      step_counter = 0;
      ini_step_size = 0.0;
      last_step_size = 0.0;
      min_step_size = 1.0;
      direction = false;
      gpio = GpioFactory.getInstance();
      step_pin=gpio.provisionDigitalOutputPin(Step_pin,PinState.LOW);
      dir_pin = gpio.provisionDigitalOutputPin(Dir_pin, PinState.LOW);

    
    
  }
  
  public void setMaxSpeed(float Speed) {
    if(maxSpeed!=Speed) {
      maxSpeed=Speed;
      min_step_size=1000000.0/Speed;
      if(step_counter>0) {
        step_counter=(long)((speed*speed)/(2.0*acceleration));
        computeNewSpeed();
      }
    }
    
  }
  
  public void setAcceleration(double Acceleration) {
    if(Acceleration==0.0) {
      return;
    }
    if(acceleration!=Acceleration) {
      step_counter=(long) (step_counter*(acceleration/Acceleration));
      ini_step_size=Math.sqrt(2.0/Acceleration)*1000000.0;
      acceleration=Acceleration;
      computeNewSpeed();
    }
  }
  
  public void setSpeed(double Speed) {
    if(Speed==speed) {
      return;
    }
    if(Speed<-maxSpeed) {
      Speed=-maxSpeed;
    }
    if(Speed>maxSpeed) {
      Speed=maxSpeed;
    }
    if(Speed==0.0) {
      stepInterval=0;
    }
    else {
      stepInterval=(long) Math.abs(1000000/Speed);
      if(Speed>0.0) {
        direction=true;
      }
      else {
        direction=false;
      }
    }
    speed=Speed;
    
  }
  
  public double speed() {
    return speed;
  }
  
  public void runToPosition() {
    while(speed != 0 || distanceToGo()!=0) {
      lauf();
    }
  }
  
  public boolean runSpeedToPosition() {
    if(target_pos==current_pos) {
      return false;
    }
    if(target_pos>current_pos) {
      direction =true;
    }
    else {
      direction = false;
    }
    return runSpeed();
  }
  
  public void runToNewPosition(long position) {
    moveTo(position);
    runToPosition();
  }
  
  public void stop() {
    if (speed != 0.0)
      {    
    long stepsToStop = (long)((speed * speed) / (2.0 * acceleration)) + 1; // Equation 16 (+integer rounding)
    if (speed > 0)
        move(stepsToStop);
    else
        move(-stepsToStop);
      }
    
    
  }
  
  public void step() {
    if(direction) {
      dir_pin.high();
    }
    else {
      dir_pin.low();
      
    }
    step_pin.high();
    long merker_time;
    merker_time=System.nanoTime();
while(System.nanoTime()<(merker_time+minPulseWidth)||System.nanoTime()<merker_time){
  
}
    step_pin.low();
    
  }
  
  
  


  @Override
  public void run() {
    while(lauf()) {
      
      
    }
    System.out.println("Fertig");
      
    
    // TODO Auto-generated method stub
    
  }
  
}

public class Stepper {
  
  

    public static void main(String[] args) throws InterruptedException {
      
      Schrittmotor Stepper1 = new Schrittmotor(RaspiPin.GPIO_01,RaspiPin.GPIO_00);
      Schrittmotor Stepper2 = new Schrittmotor(RaspiPin.GPIO_03,RaspiPin.GPIO_02);
      Thread t1 = new Thread(Stepper1);
      Thread t2 = new Thread(Stepper2);
      Stepper1.setAcceleration(3000.0);
      Stepper1.setMaxSpeed(50000);
      Stepper2.setAcceleration(3000.0);
      Stepper2.setMaxSpeed(50000);
      Stepper1.runToNewPosition(3000);
      Stepper2.runToNewPosition(3000);
    
      Stepper1.runToNewPosition(8000);
      
      
      Stepper2.runToNewPosition(8000);
      Stepper1.moveTo(0);
      Stepper2.moveTo(0);
      t1.start();
      t2.start();

     

     

        
    }
}