#include <stdio.

h>
#include <math.h>

/* Function declarations */
void simCell_RC(double *ik, int len_ik, double T, double *model, double z0, double
rck, double *zcell, double *OCV);
void getParamESC(char *param, double T, double *model, double *result);
void initSPKF_2RC(double *voltage, double T, double *SigmaX0, double SigmaV, double
*SigmaW, double *model, double *spkfData);
void iterSPKF_2RC(double *vk, double ik, double Tk, double deltat, double
*spkfData, double *sochat, double *socbound, double *dsochat, double *dsocbound,
double *bias, double *biasBound);

int main() {

double model_R0Param[100];
double model_QParam[100];
double R0[5] = {2.6, 2.5, 2.4, 2.3, 2.2};
double Q0[4] = {14, 15, 16, 17};

// Load udds.txt
FILE *udds_file = fopen("udds.txt", "r");
double udds[1000][2];
int udds_length = 0;
while (fscanf(udds_file, "%lf %lf", &udds[udds_length][0], &udds[udds_length]
[1]) != EOF) {
udds_length++;
}
fclose(udds_file);

double z0[4] = {0.7, 0.6, 0.5, 0.4};

int T = 25;
int len_ik = 10000;
double ik[10000];
double vk[10000][4];
double zk[1000][4];

/* Create current profile: rest/udds/rest/udds/rest */

for (int i = 0; i < 300; i++) {
ik[i] = 0;
ik[i + 600] = 0;
ik[i + 1200] = 0;
}
for (int i = 0; i < udds_length; i++) {
ik[i + 300] = udds[i][1];
ik[i + 900] = udds[i][1];
ik[i + 1500] = udds[i][1];
}
for (int i = 0; i < 200; i++) {
ik[i + 1200] = 0;
}

/* Perform the cell simulations */

for (int k = 0; k < 4; k++) {
for (int i = 0; i < 100; i++) {
model_R0Param[i] = R0[k] * 1e-3; // Overwrite R0
 model_QParam[i] = Q0[k]; // Overwrite Q
}

double vcell[len_ik];
double rck[len_ik];
double zcell[len_ik];
double OCV[len_ik];

simCell_RC(ik, len_ik, T, model_R0Param, z0[k], rck, zcell, OCV);

for (int i = 0; i < len_ik; i++) {

vk[i][k] = vcell[i];
zk[i][k] = zcell[i];
}
}

/* Clear variables */

/* Load cell-test data */

/* Initialize variables for SPKF */

/* Perform SPKF iterations */

for (int k = 0; k < len_ik; k++) {
double vk[4]; // "measure" voltage
double ik = ik[k]; // "measure" current
double Tk = T; // "measure" temperature

/* Update SOC (and other model states) of bar filter */

/* Update waitbar periodically */

printf(" Completed %d out of %d iterations\n", k + 1, len_ik);

}

/* Plot the results using a plotting library or write custom code */

return 0;
}

void simCell_RC(double *ik, int len_ik, double T, double *model, double z0, double
rck, double *zcell, double *OCV) {
// Implementation of simCell_RC function in C
}

void getParamESC(char *param, double T, double *model, double *result) {

// Implementation of getParamESC function in C
}

void initSPKF_2RC(double *voltage, double T, double *SigmaX0, double SigmaV, double

*SigmaW, double *model, double *spkfData) {
// Implementation of initSPKF_2RC function in C
}
void iterSPKF_2RC(double *vk, double ik, double Tk, double deltat, double
*spkfData, double *sochat, double *socbound, double *dsochat, double *dsocbound,
double *bias, double *biasBound) {
// Implementation of iterSPKF_2RC function in C
}

typedef struct {
double *xhat; // State estimate
double *SigmaX; // State covariance
double *SigmaV; // Sensor noise covariance
double *SigmaW; // Process noise covariance
double *Snoise; // Square root of process and sensor noise covariance
double Qbump; // Bump in process noise covariance
int irInd1; // Index of ir1 in state vector
int irInd2; // Index of ir2 in state vector
int zkInd; // Index of SOC0 in state vector
int biasInd; // Index of ib0 in state vector
int Nx; // Length of state vector
int Ny; // Length of measurement vector
int Nu; // Length of input vector
int Nw; // Length of process noise vector
int Nv; // Length of sensor noise vector
int Na; // Length of augmented state vector
double h; // SPKF/CDKF tuning factor
double *Wm; // Mean weightings
double *Wc; // Covariance weightings
double priorI; // Previous value of current
int signIk; // Sign of current
// Add model data structure here
double *model;
// Delta filter variables
int dNx; // Length of delta state vector
int dNw; // Length of delta process noise vector
int dNa; // Length of augmented delta state vector
double dh; // Delta filter tuning factor
double *dWm; // Delta filter mean weightings
double *dWc; // Delta filter covariance weightings
double *celldz; // Storage for celldz
double *cellSdz; // Storage for cellSdz
} spkfData_2RC;

spkfData_2RC* initSPKF_2RC(double *v0, double T0, double *SigmaX0, double SigmaV,

double *SigmaW, double *model) {

spkfData_2RC *spkfData = (spkfData_2RC*)malloc(sizeof(spkfData_2RC));

// Initialize variables for the "bar" filter

int Nx = 4;
int Ny = 1;
int Nu = 1;
int Nw = 2;
int Nv = 1;
int Na = Nx + Nw + Nv;
double h = sqrt(3);
double Weight1 = (h * h - Na) / (h * h);
double Weight2 = 1 / (2 * h * h);

spkfData->xhat = (double*)malloc(Nx * sizeof(double));

 spkfData->SigmaX = (double*)malloc(Nx * sizeof(double));
spkfData->SigmaV = (double*)malloc(Nv * sizeof(double));
spkfData->SigmaW = (double*)malloc(Nw * sizeof(double));
spkfData->Snoise = (double*)malloc((Nw + Nv) * (Nw + Nv) * sizeof(double));
spkfData->Wm = (double*)malloc((2 * Na + 1) * sizeof(double));
spkfData->Wc = (double*)malloc((2 * Na + 1) * sizeof(double));
spkfData->model = (double*)malloc(/* size of model */);
spkfData->celldz = (double*)malloc(/* size of v0 */);
spkfData->cellSdz = (double*)malloc(/* size of v0 */);

spkfData->irInd1 = 1;
spkfData->irInd2 = 2;
spkfData->zkInd = 3;
spkfData->biasInd = 4;

double SOC0 = 0; // Compute initial SOC0 using SOCfromOCVtemp

double ib0 = 0; // Initial current sensor bias

// Initialize xhat
spkfData->xhat[0] = 0; // ir1
spkfData->xhat[1] = 0; // ir2
spkfData->xhat[2] = SOC0; // SOC0
spkfData->xhat[3] = ib0; // ib0

// Initialize SigmaX
for (int i = 0; i < Nx; i++) {
spkfData->SigmaX[i] = SigmaX0[i];
}

// Initialize SigmaV
spkfData->SigmaV[0] = SigmaV;

// Initialize SigmaW
for (int i = 0; i < Nw; i++) {
spkfData->SigmaW[i] = SigmaW[i];
}

// Initialize Snoise
for (int i = 0; i < Nw + Nv; i++) {
for (int j = 0; j < Nw + Nv; j++) {
spkfData->Snoise[i * (Nw + Nv) + j] = 0; // Set elements to the
correct values
}
}

// Initialize Wm and Wc
spkfData->Wm[0] = Weight1;
for (int i = 1; i < 2 * Na + 1; i++) {
spkfData->Wm[i] = Weight2;
spkfData->Wc[i] = Weight2;
}

// Initialize priorI and signIk

spkfData->priorI = 0;
spkfData->signIk = 0;

// Store model data structure

// Copy model data to spkfData->model
 // Initialize delta filter variables
int dNx = 1;
int dNw = 1;
int dNa = dNx + dNw;
double dWeight1 = (h * h - dNa) / (h * h);
double dWeight2 = 1 / (2 * h * h);

spkfData->dNx = dNx;
spkfData->dNw = dNw;
spkfData->dNa = dNa;
spkfData->dh = h;
spkfData->dWm = (double*)malloc((2 * dNa + 1) * sizeof(double));
spkfData->dWc = (double*)malloc((2 * dNa + 1) * sizeof(double));

spkfData->dWm[0] = dWeight1;
for (int i = 1; i < 2 * dNa + 1; i++) {
spkfData->dWm[i] = dWeight2;
spkfData->dWc[i] = dWeight2;
}

// Initialize celldz and cellSdz

// Set elements to the correct values

return spkfData;
}

#include <stdio.h>
#include <math.h>

#define Nx 4
#define Nw 1
#define Nv 1
#define Na (Nx + Nw + Nv)
#define dNx 1
#define dNw 1
#define dNa (dNx + dNw)

typedef struct {
double xhat[Nx];
double SigmaX[Nx][Nx];
double celldz[...]; // Size to be determined
double cellSdz[...]; // Size to be determined
// Additional fields as needed
} SPKFData;

void iterSPKF_2RC(double vk[], double ik, double Tk, double deltat, SPKFData*
spkfData, double zkbar[], double zkbarbnd[], double dzk[], double dzkbnd[], double
ib[], double ibbnd[]);
double getParamESC(char param[], double Tk, double model);
double SQRT(double x);

int Test_function_for init() {

// Example usage of the function
SPKFData spkfData;
double vk[...]; // Size to be determined
double ik = 0.0, Tk = 0.0, deltat = 0.0;
double zkbar[...]; // Size to be determined
double zkbarbnd[...]; // Size to be determined
 double dzk[...]; // Size to be determined
double dzkbnd[...]; // Size to be determined
double ib[...]; // Size to be determined
double ibbnd[...]; // Size to be determined

iterSPKF_2RC(vk, ik, Tk, deltat, &spkfData, zkbar, zkbarbnd, dzk, dzkbnd, ib,
ibbnd);

return 0;
}

void iterSPKF_2RC(double vk[], double ik, double Tk, double deltat, SPKFData*
spkfData, double zkbar[], double zkbarbnd[], double dzk[], double dzkbnd[], double
ib[], double ibbnd[]) {
double model = spkfData->model;

// Load the cell model parameters

double RC1 = exp(-deltat / fabs(getParamESC("RCParam", Tk, model)));
double RC2 = exp(-deltat / fabs(getParamESC("RCParam", Tk, model)));
double R1 = getParamESC("RParam", Tk, model);
double R2 = getParamESC("RParam", Tk, model);
double R0 = getParamESC("R0Param", Tk, model);

// Get data stored in spkfData structure

double I = spkfData->priorI;
double (*SigmaX)[Nx] = spkfData->SigmaX;
double (*xhat)[Nx] = spkfData->xhat;
double (*celldz) = spkfData->celldz;
double (*cellSdz) = spkfData->cellSdz;

// Step 1a : State estimate time update

// - Create xhatminus augmented SigmaX points
// - Extract xhatminus state SigmaX points
// - Compute weighted average xhatminus(k)

// Step 1a-1: Create augmented SigmaX and xhat

double sigmaXa[Nx + Nw + Nv][Nx + Nw + Nv];
double xhata[Nx + Nw + Nv][1];
// TODO: Initialize sigmaXa and xhata

// Step 1a-2: Calculate SigmaX points

double Xa[Nx + Nw + Nv][2 * Na + 1];
// TODO: Calculate Xa

// Step 1a-3: Time update from last iteration until now

double Xx[Nx][2 * Na + 1];
// TODO: Calculate Xx

// Step 1b: Error covariance time update

double Xs[Nx][2 * Na + 1];
// TODO: Calculate Xs

// Step 1c: Output estimate

double Y[Nv][2 * Na + 1];
// TODO: Calculate Y

// Step 2a: Estimator gain matrix

double Ys[Nv][2 * Na + 1];
// TODO: Calculate Ys
 // Step 2b: State estimate measurement update
double r = 0.0;
// TODO: Calculate r

// Step 2c: Error covariance measurement update

// TODO: Update SigmaX

// Q-bump code
// TODO: Implement Q-bump code

// Save data in spkfData structure for next time

// TODO: Update spkfData fields

// The "bar" filter update is complete. Now, work on "delta" filter updates
int offset = -R1 * xhat[irInd1] - R2 * xhat[irInd2] - R0 * (ik - ib);
for (int thecell = 0; thecell < length(vk); thecell++) {
// Implement SPKF for delta-soc
// Step 1a - State prediction time update
double cellxa[dNx];
double cellSxa[dNx][dNx];
double cellXa[dNa][2 * spkfData->dNa + 1];
double cellXx[dNx][2 * spkfData->dNa + 1];
// TODO: Update cellxa, cellSxa, cellXa, and cellXx

// Step 1b - Do error covariance time update

// TODO: Update celldz and cellSdz

// Step 1c - output estimate

double cellY[dNv][2 * spkfData->dNa + 1];
double cellyhat[dNv];
// TODO: Calculate cellY and cellyhat

// Step 2a - Estimator gain matrix

double cellYs[dNv][2 * spkfData->dNa + 1];
double cellSy[dNv][dNv];
double cellSxy[dNx][dNv];
double cellL[dNx][dNv];
// TODO: Calculate cellYs and cellL

// Step 2b - State estimate measurement update

// TODO: Update celldz

// Step 2c - Error covariance measurement update

// TODO: Update cellSdz
}

// Save data in spkfData structure for next time

// TODO: Update spkfData fields

// Calculate new states for all of the old state vectors in xold.
// TODO: Implement stateEqn function

// Calculate cell output voltage for all of state vectors in xhat

// TODO: Implement outputEqn function

// "Safe" square root

// TODO: Implement SQRT function
}
double getParamESC(char param[], double Tk, double model) {
// TODO: Implement getParamESC function
}

double OCVfromSOCtemp(double SOC, double T, double model) {

// TODO: Implement OCVfromSOCtemp function
}