#include <SoftPWM.h>

#define POTPIN A0    // select the input pin for the potentiometer
#define PWMPIN A1 // Output pin to control PWM for motor controller
#define DIRPIN A2 // Output direction for motor controller
#define SENSORPIN A3 // position sensor pin
//#define DIRSW A3 // direction switch
//int dirSwState = 0; // last state of the direction switch
//int dirLast = 0; // last state to compare direction switch against
int potValue = 0;  // variable to store the value coming from the sensor
int sensorValue = 0; // value of position sensor
long prevPos1 = 0;
long prevPos2 = 0;
long curPos = 0;
long prevSPos1 = 0;
long prevSPos2 = 0;
long curSPos = 0;

#define MIN_POT_VAL 0
#define MAX_POT_VAL 1023
#define MIN_SENSOR_VAL 32
#define MAX_SENSOR_VAL 1018
#define SLOW_RANGE 51 // number of encoder values away before decel begins

int mspeed = 0;
int newspeed = 0;

void setup() {

  SoftPWMBegin();
  SoftPWMSet(PWMPIN, 0);
  SoftPWMSetFadeTime(ALL, 100, 400);
  //pinMode(PWMPIN, OUTPUT);

  pinMode(DIRPIN, OUTPUT);
 // pinMode(DIRSW, INPUT);

  Serial.begin(9600);
}

void loop() {
  // read the value from the sensor:
  long newPos = analogRead(POTPIN);
  long newSPos = analogRead(SENSORPIN);

  // debounce pot
  if (newPos != prevPos1 && newPos != prevPos2) {
    prevPos1 = prevPos2;
    prevPos2 = newPos;
    curPos = map(newPos, MIN_POT_VAL, MAX_POT_VAL, MIN_SENSOR_VAL, MAX_SENSOR_VAL);
  }

  // debounce encoder
  if (newSPos != prevSPos1 && newSPos != prevSPos2) {
    prevSPos1 = prevSPos2;
    prevSPos2 = newSPos;

    curSPos = map(newSPos, MIN_SENSOR_VAL, MAX_SENSOR_VAL, MIN_POT_VAL, MAX_POT_VAL);
  }

  if(curPos <= curSPos){ // Backwards
    digitalWrite(DIRPIN, LOW);

    unsigned int in_range = curPos + SLOW_RANGE;

    if(curSPos > in_range){ // Max speed
        SoftPWMSet(PWMPIN, 255);
    } else {

      if((curPos+50) == curSPos) // 75%
      {
        SoftPWMSet(PWMPIN,190);
      }
      if((curPos+40) == curSPos) // 50%
      {
        SoftPWMSet(PWMPIN, 125);
      }
      if((curPos+30) == curSPos) // 25%
      {
        SoftPWMSet(PWMPIN, 60);
      }
      if((curPos+20) == curSPos) // 12%
      {
        SoftPWMSet(PWMPIN, 31);
      }
       if((curPos+10) == curSPos) // 12%
      {
        SoftPWMSet(PWMPIN, 15);
      }
      if((curPos+3) == curSPos) // 6%
      {
        SoftPWMSet(PWMPIN, 0);
      }
    }
  }

  if(curPos >= curSPos){ // Forward
    digitalWrite(DIRPIN, HIGH);

    unsigned int in_range = curPos - SLOW_RANGE;

    if(curSPos < in_range){ // Max speed
        SoftPWMSet(PWMPIN, 255);
    } else {

      if((curPos-50) == curSPos) // 75%
      {
        SoftPWMSet(PWMPIN,190);
      }
      if((curPos-40) == curSPos) // 50%
      {
        SoftPWMSet(PWMPIN, 125);
      }
      if((curPos-30) == curSPos) // 25%
      {
        SoftPWMSet(PWMPIN, 60);
      }
      if((curPos-20) == curSPos) // 12%
      {
        SoftPWMSet(PWMPIN, 31);
      }
       if((curPos-10) == curSPos) // 12%
      {
        SoftPWMSet(PWMPIN, 15);
      }
      if((curPos-3) == curSPos) // 6%
      {
        SoftPWMSet(PWMPIN, 0);
      }

    }
  }

  if(curPos == curSPos){ // Stop
    SoftPWMSet(PWMPIN, 0);
  }

 // if(dirLast != dirSwState){
 //   Serial.println(dirSwState);
 //   dirLast = dirSwState;
 //}

  //analogWrite(pwmPin, val);
  //SoftPWMSet(PWMPIN, val);
}