////////////////////////////////
/////
///// Circuit-version-2 nonlinar 2
/////
/////
////////////////////////////////

int torquePin=DAC0;

unsigned long lastTriggerMillis=0;
int triggerIntervalMillis=1000;

boolean serial=true;
boolean autonomous=true;

int rotationDirection=0;

//For intinal position
int zeroTorque=2048;
int zeroGravity=1735;
int nextVelocityTicks = 0;

double stiffness=16.0/600.0;

int lastTorque=zeroTorque;

//veclocity calculation
int calculatedVelocityTicks;
int velocityLastPos;
unsigned long lastVelocitySampleMillis;
int velocitySampleTime=1;

double coulombFactor=0;
double c1Factor=0;
double c2Factor=0;
double c3Factor=0;
double cLinearFactor=0;


//Sigmoid Curve Parameters
double sigmoidScalingFactor=0.0007;
double sigmoidAmplitude=500.0;

boolean overspeed=false;

int zeroPosition=0;
int cpuPosition=0;
int previousCpuPosition = 0;
int positions[10] = {0};
int memoryCounter = 0;

int error=0;

int torqueOverride=zeroTorque;

unsigned long cachedMillis;

#define forward 0
#define reverse 1

void setup()
{
  delay(1000);
  if(serial) Serial.begin(115200);

  setupQam();

  analogWriteResolution(12);

  pinMode(torquePin, OUTPUT);
  analogWrite(torquePin, zeroTorque);
}

void setupQam()
{
  // Setup Quadrature Encoder with Marker
  REG_PMC_PCER0 = (1<<27); // activate clock for TC0
  REG_PMC_PCER0 = (1<<28); // activate clock for TC1

  // select XC0 as clock source and set capture mode
  REG_TC0_CMR0 = 5;

  // activate quadrature encoder and position measure mode, no filters
  REG_TC0_BMR = (1<<9)|(1<<8); //|(1<<12)

  // select XC0 as clock source and set capture mode
  REG_TC0_CCR0 = (1<<2) | (1<<0) | (1<<14);
}

void loop()
{
  cachedMillis=millis();
  updatePosition();

  calculateVelocity();
  applyTorque();
  previousCpuPosition = cpuPosition;
  if(serial) performOutput();
  if(serial) processSerialInput();
}

void updatePosition()
{

  cpuPosition=((int)REG_TC0_CV0)-zeroPosition;
  rotationDirection=((REG_TC0_QISR>>8)&0x1);


  //error=(REG_TC0_QISR>>2)&0x1;
}

//int sign(int x)
//{
//  return x>0?1:(x<0?-1:0);
//}

int sign(double x)
{
  return x>0?1:(x<0?-1:0);
}

void applyTorque()
{
  //scale: 1/1
  int adjPos=cpuPosition;

  double yDot=((double)calculatedVelocityTicks)/25;
  double y=adjPos;

  double coulombComponent=0;
  //double linearComponent=y; //This is replaced by piecewiseLinearComponent
  double yComponent=0;
  double yDotComponent=0;
  double yDot2Component=0;
  double yDot3Component=0;

  int torque=zeroTorque;

  if(autonomous)
  {
    double yDot2=yDot*yDot;
    double yDot3=yDot2*yDot;

    if(sign(yDot)>0) //down
    {
      coulombComponent=10.955;
      yDotComponent=30.28 * yDot;
      yDot2Component=-9.8495 * yDot2;
      yDot3Component=1.1143* yDot3;
    }
    else //up
    {
      coulombComponent=-10.265;
      yDotComponent=30.28 * yDot;
      yDot2Component=10.2495 * yDot2;
      yDot3Component=1.1143 * yDot3;
    }

    double sigmoidComponent=0.0;
    sigmoidComponent=(1.0/(1+pow(M_E, -sigmoidScalingFactor*y)))*2.0-1.0;


    double output=
      -sigmoidComponent*sigmoidAmplitude
      +yDotComponent*(c1Factor-cLinearFactor) *5.64
        +yDot2Component*(c2Factor)  *5.64
          +yDot3Component*(c3Factor) *5.64
            +coulombComponent*(coulombFactor) *5.64;


    torque=max(1.0, min(2048.0 + output, 4094.0));
  }
  else
  {
    torque=torqueOverride;
  }

  //safety checks
  if(torque>=4000 || torque<=100 || cpuPosition>11000 || cpuPosition<-11500 || error !=0 || calculatedVelocityTicks>1900)
  {
    overspeed=true;
  }

  //if(overspeed)
  //{
 //   torque=zeroTorque;
 // }

  //save us the extra write
  if(torque!=lastTorque)
  {
    analogWrite(torquePin, torque);
    lastTorque=torque;
  }

}

void calculateVelocity()
{
  unsigned long currentTimeMillis=cachedMillis;
  if( abs(currentTimeMillis-lastVelocitySampleMillis) >= velocitySampleTime )
  {
    lastVelocitySampleMillis=currentTimeMillis;
    calculatedVelocityTicks=(cpuPosition-velocityLastPos);

    velocityLastPos=cpuPosition;

    //int cpuDelta = cpuPosition - previousCpuPosition;
    //nextVelocityTicks = calculatedVelocityTicks - 6 *cpuDelta*velocitySampleTime;
    //calculatedVelocityTicks = nextVelocityTicks*4; //should be checked !!!!
  }


//  unsigned long currentTimeMillis=cachedMillis;
//  if( abs(currentTimeMillis-lastVelocitySampleMillis) >velocitySampleTime )
//  {
//    lastVelocitySampleMillis=currentTimeMillis;
//    calculatedVelocityTicks=(cpuPosition-velocityLastPos);
//    velocityLastPos=cpuPosition;
//  }

}

void processSerialInput()
{
  if(Serial.available()>0)
  {
    char x=Serial.read();
    if(x=='v')
    {
      int incomingDutyCycle = Serial.parseInt();
      autonomous=false;
      torqueOverride=incomingDutyCycle;
    }
    else if(x=='r')
    {
      zeroPosition=cpuPosition+zeroPosition;
      velocityLastPos=0;
      cpuPosition=0;
      torqueOverride=zeroTorque;
      autonomous=true;
      overspeed=false;
    }
    else if(x=='s')
    {
      double stiffnessIncoming=Serial.parseInt();
      stiffness = 16.0/stiffnessIncoming;
    }
    else if(x=='p')
    {
      int posOffset=Serial.parseInt();
      zeroPosition+=posOffset;
    }
    else if(x=='d')
    {
      c1Factor=getPercentageFromInput();
    }
    else if(x=='c')
    {
      coulombFactor=getPercentageFromInput();
    }
    else if(x=='q')
    {
      c2Factor=getPercentageFromInput();
    }
    else if(x=='b')
    {
      c3Factor=getPercentageFromInput();
    }
    else if(x=='l')
    {
      cLinearFactor=getPercentageFromInput();

    }
    Serial.read();  //newline
  }
}

double getPercentageFromInput()
{
  int value=Serial.parseInt();
  double percentage=((double)value)/100.0;
  return percentage;
}

int perfSpeedTicks=0;
void performOutput()
{
  perfSpeedTicks++;
  unsigned long currentMillis=cachedMillis;
  if((currentMillis-lastTriggerMillis)>=triggerIntervalMillis)
  {
    if(perfSpeedTicks<1000) overspeed=true;
    if(overspeed) Serial.print("!ERR ");
    Serial.print(cpuPosition);
    Serial.print(" T:");
    Serial.print(lastTorque);
    Serial.print(" D:");
    Serial.print(rotationDirection);
    Serial.print(" S:");
    Serial.print(calculatedVelocityTicks);
    Serial.print(" H:");
    Serial.print(perfSpeedTicks);
    Serial.print("\n");
    lastTriggerMillis=currentMillis;
    perfSpeedTicks=0;
  }
}