// Ball balance using state space using kalman estimator
// version 0.2

#include <Servo.h>
#include <BasicLinearAlgebra.h>

// All the functions in BasicLinearAlgebra are wrapped up inside the namespace BLA, so specify that we're using it like so:
using namespace BLA;

#define Tstep 0.1  // Tstep for discrete time system update. in seconds
#define delayTime 95 // time in ms to wait for next cycle. This is a close guestimation

Servo servoLeft;
Servo servoRight;

Matrix<2,2> kalman_A; // Filter 
Matrix<2,2> kalman_B;  // Filter 

Matrix<2,1> xhatNew;  // X  --- > Filter ---- >XHat
Matrix<2,1> kalman_U;
Matrix<1,2> feedback_K; //X(dot) = (A-BK)X + Br

// variables that need are needed for each axis.
Matrix<2,1> Xxhat; //First row position, second row velocity
Matrix<2,1> Yxhat; //First row position, second row velocity
Matrix<1,1> Xu;  // Control signal - motor command to first motor
Matrix<1,1> Yu;  // Control signal - motor command to second motor


float Xsigma =0.;  //Accumulated error in X direction, Integral component
float Ysigma =0.;  //Accumulated error in Y direction, Integral component
float Ksigma;

int loopcounter = 0;
int index = 0;

float Xdesired = 0; //Reference in X direction
float Ydesired = 0; //Reference in Y direction

//float Xpositions[] = {300, -300, 300, -300};
float Xpositions[] = {0, -0, 0, -0}; 
float Ypositions[] = {0, -0, 0, -0};


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // digitizer pins A2/pin8 (for Y gradient) and A3/pin7 (for X gradient)
  // 
  pinMode(A3, INPUT); //Position Sensor
  pinMode(7, INPUT);
  pinMode(A2, INPUT);
  pinMode(8, INPUT);

  //Servos Right pin 10, Left pin 9
  // write(0) will rotate counterclockwise and write(180) clockwise
  servoLeft.attach(9); //Motor
  servoRight.attach(10);

 //The below is filter
  kalman_A ={0.37908378887363425 , 0.06653266336609243 , -1.8818278974632858 , 0.8912925854841883} ;
  kalman_B= {1.3451856244638154 , 0.6209162111263659 , 33.642487009924565 , 1.8818278974632863};
  
  xhatNew = {0.,0.};
  
  ///Gain
  feedback_K = {0.03417143, 0.01714286}; //   take care of last part manually!!!!!
 
   //Ingegral gain
 
  Ksigma = 0.02262857  ; //set this to zero to get rid of integrator

}

void loop() {
  // put your main code here, to run repeatedly:
  int sensorY, sensorX;

  
  sensorX = readX(); //Reading velocity 
  sensorY = readY();


  //moveTable( map(sensorX, -450, 450, -10,10), map(sensorY, -400, 400, -10,10));

  /// DO X PART here
  kalman_U(0) = Xu(0);     // load values into U for kalman filter, u(0) is last value sent to table, sensorX is measured y
  kalman_U(1) = sensorX;
  
  xhatNew = kalman_A * Xxhat + kalman_B * kalman_U;    // xhatNew now contains Kalman estimate of position and velocity

  Xsigma += ( Xxhat(0,0) - Xdesired) * Tstep;
  
  Xu =  -feedback_K * xhatNew;
  Xu(0) -= Ksigma * Xsigma;        // manually take care of Kfeedback term for sigma (accumulated error)
  
  Xxhat = xhatNew;

  

  /// DO Y PART here
  kalman_U(0) = Yu(0);     // load values into U for kalman filter, u(0) is last value sent to table, sensorX is measured y
  kalman_U(1) = sensorY;
  
  xhatNew = kalman_A * Yxhat + kalman_B * kalman_U;    // xhatNew now contains Kalman estimate of position and velocity

  Ysigma += ( Yxhat(0,0) - Ydesired)* Tstep;
  
  Yu = -feedback_K * xhatNew;
  Yu(0) -= Ksigma * Ysigma;        // manually take care of Kfeedback term for sigma (accumulated error)
  
  Yxhat = xhatNew;

  moveTable(Xu(0) ,Yu(0));//+ Xdesired/dcgain - Ksigma*Xsigma

  Serial.print(millis());
  Serial.print(" :sensor ");
  Serial.print(sensorX);
  Serial.print(" :xhat X0 ( position)");
  Serial.print(Xxhat(0,0));
  Serial.print(" :xhat X1 (velocity)");
  Serial.print(Xxhat(1,0));
//  Serial.print(", ");
//  Serial.println(sensorY);
  Serial.print(" :sigma");
  Serial.println(Xsigma);
  Serial.print(" :u");
  Serial.println(Xu(0));
  delay(delayTime);

  loopcounter += 1;
  if (loopcounter >= 50) {
    loopcounter = 0;
    index++;
    index %= 4;
    Xdesired = Xpositions[index];
    Ydesired = Ypositions[index];
  }
}

void moveTable(float x,float y) {
  // both x and should be limited to +/- 10
  // positive numbers are to tilt up and right, ie to increase 
  // position in respective axes

  int left, right;

  left = (int) ( 8. * (x + y)) + 90;
  right = (int) (8. * (x - y)) + 90;

//  Serial.print(left);
//  Serial.print(", ");
//  Serial.println(right);

  servoLeft.write(left );
  servoRight.write(right);
}

int readX() {
  int value;
  
  pinMode(A3, OUTPUT);
  pinMode(7, OUTPUT);
  digitalWrite(A3, HIGH);
  digitalWrite(7, LOW);
//  delay(1);
  value = analogRead(A2);
  pinMode(A3, INPUT);
  pinMode(7, INPUT);
  return(value-512);
}


int readY() {
  int value;
  
  pinMode(A2, OUTPUT);
  pinMode(8, OUTPUT);
  digitalWrite(A2, LOW);
  digitalWrite(8, HIGH);
//  delay(1);
  value = analogRead(A3);
  pinMode(A2, INPUT);
  pinMode(8, INPUT);

  return (value-512);
}