/* Déclaration des ressources externes */
#include "mbed.h"


/* Déclaration des constantes */
#define r 0.005 // Rayon photodiode (m)
#define d 0.45 // Distance caractéristique (m)
#define Kx 29.47 // Conversion miroirs angle -> tension (rad/V)
#define Ky 42.12
#define Te 0.05 // Période d'échantillonage (s)


/* Déclaration des entrées/sorties */
AnalogIn measureA(PA_0); // Signaux photodiode
AnalogIn measureB(PA_1);
AnalogIn measureC(PB_0);
AnalogIn measureD(PC_1);
AnalogOut commandX(PA_4); // Commandes des miroirs, X = cable haut
AnalogOut commandY(PA_5);

Serial matlab(USBTX, USBRX);
Ticker sampling;


/* Déclaration de fonctions */
void sample(void);

void getParameters(void);

void getPosition(void);

void Asservissement(void);

void posWrite(void);

void sendData(void);


/* Déclaration des variables globales */
double errx[2] = {0,0}; // Erreurs à k et k-1 (V)
double erry[2] = {0,0};

double px = 0; // Positions (m)
double py = 0;

double consx = 0; // Consignes (m)
double consy = 0;

double comx = 0; // Commandes (V)
double comy = 0;

double Kp = 1; // Paramètres d'asservissement
double Ki = 1;
double Kd = 0;

double uix[2] = {0,0}; // Commandes intégrales k et k-1 (V)
double uiy[2] = {0,0};

double A,B,C,D; // Tensions des cadrants

int bool_graph = false; // Tensions ABCD (true) ou position (px,py) (false)


/* Fonction principale */
int main(){
    matlab.baud(115200);
    matlab.attach(&getParameters); // Récéption par interruption (pas scrutation!)
    sampling.attach(&sample,Te); // Action à intervalle Te régulier
    
    while (1) {
        
    }
}


/* Fonction d'interruption du ticker */
void sample() {
    
    getPosition();
    
    errx[1] = errx[0];
    erry[1] = erry[0];
    errx[0] = d*Kx*(px - consx); // A tester
    erry[0] = d*Ky*(py - consy);
    
    Asservissement(); // Kp,Ki,Kd,errx,erry,
    posWrite();
    
    sendData(); // Attention avec Te trop bas!
}


/* Fonction d'interruption série */
void getParameters() {
        
    matlab.scanf("%lf,%lf,%lf,%lf,%lf,%d",&Kp,&Ki,&Kd,&consx,&consy,&bool_graph); // Pas besoin de vérifier matlab.readable()
}


/* Fonctions secondaires */


void getPosition() {
    double A,B,C,D,tempx,tempy;
    
    A = measureA.read();
    B = measureB.read();
    C = measureC.read();
    D = measureD.read();
    
    tempx = r*((A+D)-(C+B))/(A+B+C+D);  // Axes photodiode alignés avec axes de déplacement 
    tempy = r*((A+B)-(C+D))/(A+B+C+D);  // Normalisation par le signal global
    
    if ((!isnan(tempx))&&(!isnan(tempy))&&(A+B+C+D>0.1)) { // Ignorer les NaNs + Seuillage position
        px = tempx;
        py = tempy;
    }
}


void Asservissement() {
    double upx, upy, udx, udy;
    
    upx = Kp*errx[0]; // Action proportionelle
    upy = Kp*erry[0];
    
    uix[1] = uix[0]; // Action intégrale + anti-windup à arbitrairement 1.3V < 1.65V
    uiy[1] = uiy[0];
    uix[0] = ((uix[1] > 0) - (uix[1] <= 0))*fminf(fabs(uix[1]),1.3) + Te*Ki*errx[0];
    uiy[0] = ((uiy[1] > 0) - (uiy[1] <= 0))*fminf(fabs(uiy[1]),1.3) + Te*Ki*erry[0];
    
    udx = (Kd/Te)*(errx[0] - errx[1]); // Action dérivée
    udy = (Kd/Te)*(erry[0] - erry[1]);
    
    comx = upx + uix[0] + udx;
    comy = upy + uiy[0] + udy;
}


void posWrite() {
    
    commandX.write(0.5 + comx/3.3); // [0,1] <- [-1.65,1.65]
    commandY.write(0.5 + comy/3.3);
}


void sendData() {
    
    if (matlab.writeable()) {
        if (bool_graph == 1) {
            matlab.printf("%lf,%lf,%lf,%lf\r\n",A,B,C,D);
        } else {
            matlab.printf("%lf,%lf\r\n",px,py);
        }
    }
}