/*
 * SusConverter.cpp
 *
 *  Created on: 17.01.2022
 *      Author: Timon Schwarz
 */

#include <math.h>  //for atan2
#include <iostream>
#include <SusConverter.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>

void SunSensor::setSunSensorData() {
  // Creates dummy sensordata, replace with SUS devicehandler / channel readout
  susChannelValues[0] = {3913, 3912, 3799, 4056};
  susChannelValues[1] = {3913, 3912, 3799, 4056};
  susChannelValues[2] = {3913, 3912, 3799, 4056};
  susChannelValues[3] = {3913, 3912, 3799, 4056};
  susChannelValues[4] = {3913, 3912, 3799, 4056};
  susChannelValues[5] = {3913, 3912, 3799, 4056};
  susChannelValues[6] = {3913, 3912, 3799, 4056};
  susChannelValues[7] = {3913, 3912, 3799, 4056};
  susChannelValues[8] = {3913, 3912, 3799, 4056};
  susChannelValues[9] = {3913, 3912, 3799, 4056};
  susChannelValues[10] = {3913, 3912, 3799, 4056};
  susChannelValues[11] = {3913, 3912, 3799, 4056};
}

void SunSensor::checkSunSensorData(uint8_t Sensornumber) {
  uint16_t ChannelValueSum;

  // Check individual channel values
  for (int k = 0; k < 4; k++) {  // iteration above all photodiode quarters

    if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow ||
        susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) {  // Channel values out of range for 12 bit SUS
                                                    // channel measurement range?
      ValidityNumber[Sensornumber] = false;                       // false --> Data not valid
    /*printf(
          "The value of channel %i from sun sensor %i is not inside the borders of valid data with "
          "a value of %i \n",
          k, Sensornumber, ChannelValue[k]);*/
    } else if (susChannelValues[Sensornumber][k] >
               susChannelValues[Sensornumber][4]) {  // Channel values higher than zero current threshold GNDREF?
      ValidityNumber[Sensornumber] = false;
    /*printf(
          "The value of channel %i from sun sensor %i is higher than the zero current threshold "
          "GNDREF\n",
          k, Sensornumber);*/
    };
  };

  // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is
  // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon
  // reflection or earth albedo)
  ChannelValueSum =
      4 * susChannelValues[Sensornumber][4] - (susChannelValues[Sensornumber][0] +
      susChannelValues[Sensornumber][1] + susChannelValues[Sensornumber][2] +
	  susChannelValues[Sensornumber][3]);
  if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) {
    ValidityNumber[Sensornumber] = false;
    //printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber);
  };
}

void SunSensor::AngleCalculation(uint8_t Sensornumber) {
  float xout, yout, s = 0.03;  // s=[mm]
  uint8_t d = 5, h = 1;        // d=[mm]  h=[mm]
  int ch0, ch1, ch2, ch3;
  // Substract measurement values from GNDREF zero current threshold
  ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0];
  ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1];
  ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2];
  ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3];

  // Calculation of x and y
  xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
  yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]

  // Calculation of the angles
  AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI);  //[°]
  AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI);  //[°]
}

void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
  switch (Sensornumber) {  // search for the correct calibration coefficients for each SUS

    case 0:
      for (uint8_t row = 0; row < 9;
           row++) {  // save the correct coefficients in the right SUS class
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column];
        }
      }
      break;

    case 1:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column];
        }
      }
      break;

    case 2:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column];
        }
      }
      break;

    case 3:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column];
        }
      }
      break;

    case 4:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column];
        }
      }
      break;

    case 5:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column];
        }
      }
      break;

    case 6:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column];
        }
      }
      break;

    case 7:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column];
        }
      }
      break;

    case 8:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column];
        }
      }
      break;

    case 9:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column];
        }
      }
      break;

    case 10:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column];
        }
      }
      break;

    case 11:
      for (uint8_t row = 0; row < 9; row++) {
        for (uint8_t column = 0; column < 10; column++) {
          CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column];
          CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column];
        }
      }
      break;
  }
}

void SunSensor::Calibration(uint8_t Sensornumber) {
  float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l;
  uint8_t index;

  alpha_m = AlphaBetaRaw[Sensornumber][0];  //[°]
  beta_m = AlphaBetaRaw[Sensornumber][1];   //[°]

  // while loop iterates above all calibration cells to use the different calibration functions in
  // each cell
  k = 0;
  while (k < 3) {
    k = k + 1;
    l = 0;
    while (l < 3) {
      l = l + 1;
      // if-condition to check in which cell the data point has to be
      if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) &&
           alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) &&
          (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) &&
           beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) {
        index = (3 * (k - 1) + l) - 1;  // calculate the index of the datapoint for the right cell
                                        // -> first cell has number 0
        AlphaBetaCalibrated[Sensornumber][0] =
            CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m +
            CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m +
            CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m +
            CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m +
            CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m +
            CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m +
            CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m;  //[°]
        AlphaBetaCalibrated[Sensornumber][1] =
        	CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m +
            CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m +
            CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m +
            CoeffBeta[Sensornumber][index][5] * beta_m * beta_m +
            CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m +
            CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m +
            CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m +
            CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m;  //[°]
      }
    }
  }
}

void SunSensor::CalculateSunVector(uint8_t Sensornumber) {
  float alpha, beta;
  alpha = AlphaBetaCalibrated[Sensornumber][0];  //[°]
  beta = AlphaBetaCalibrated[Sensornumber][1];   //[°]

  // Calculate the normalized Sun Vector
  SunVectorBodyFrame[Sensornumber][0] =
      (tan(alpha * (M_PI / 180)) /
       (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1))));
  SunVectorBodyFrame[Sensornumber][1] =
      (tan(beta * (M_PI / 180)) /
       (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1))));
  SunVectorBodyFrame[Sensornumber][2] =
      (-1 /
       (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1))));
}

float* SunSensor::getSunVectorBodyFrame() {
  // return function for the sun vector in the body frame
  float* SunVectorBodyFrameReturn = 0;
  SunVectorBodyFrameReturn = new float[3];

  SunVectorBodyFrameReturn[0] = SunVectorBodyFrame[0];
  SunVectorBodyFrameReturn[1] = SunVectorBodyFrame[1];
  SunVectorBodyFrameReturn[2] = SunVectorBodyFrame[2];

  return SunVectorBodyFrameReturn;
}

bool SunSensor::getValidityNumber(uint8_t Sensornumber) {
  return ValidityNumber[Sensornumber];
}

float* SunSensor::TransferSunVector() {
  float* SunVectorEIVE = 0;
  SunVectorEIVE = new float[3];

  uint8_t counter = 0;
  int8_t BasisMatrixUse[3][3];
  float SunVectorMatrixEIVE[3][12] = {0}, sum;
  float SunVectorMatrixBodyFrame[3][12];

  for (uint8_t Sensornumber = 0; Sensornumber < 12;
       Sensornumber++) {  // save the sun vector of each SUS in their body frame into an array for
                          // further processing
    float* SunVectorBodyFrame = SunVectorBodyFrame[Sensornumber];
    SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0];
    SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1];
    SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2];
  }

  for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) {
    if (getValidityNumber(Sensornumber) == false) {
      counter = counter + 1;
    }  // if the SUS data is not valid ->

    for (uint8_t c1 = 0; c1 < 3; c1++) {
      for (uint8_t c2 = 0; c2 < 3; c2++) {
        switch (Sensornumber) {  // find right basis matrix for each SUS

          case 0:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2];
            break;
          case 1:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
            break;
          case 2:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
            break;
          case 3:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
            break;
          case 4:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
            break;
          case 5:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
            break;
          case 6:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
            break;
          case 7:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
            break;
          case 8:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
            break;
          case 9:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
            break;
          case 10:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
            break;
          case 11:
            BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
            break;
        }
      }
    }

    // matrix multiplication for transition in EIVE coordinatesystem
    for (uint8_t p = 0; p < 3; p++) {
      for (uint8_t q = 0; q < 3; q++) {
        // normal matrix multiplication
        SunVectorMatrixEIVE[p][Sensornumber] +=
            (BasisMatrixUse[p][q] * SunVectorMatrixBodyFrame[q][Sensornumber]);
      }
    }
  }

  if (counter < 12) {  // Calculate one sun vector out of all sun vectors from the different SUS
    for (uint8_t i = 0; i < 3; i++) {
      sum = 0;
      for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) {
        if (getValidityNumber(Sensornumber)){
          sum += SunVectorMatrixEIVE[i][Sensornumber];
          //printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]);
        }
      }
      // ToDo: decide on length on sun vector
      SunVectorEIVE[i] =
          sum/* / (12 - counter)*/;  // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm
                                 // liegen, vermutlich aber an den falschen ChannelValues da die
                                 // transformierten Sonnenvektoren jedes SUS plausibel sind
    }
    VectorOperations<float>::normalize(SunVectorEIVE, SunVectorEIVE, 3);
  } else {
    // No sus is valid
    throw std::invalid_argument("No sun sensor is valid");  // throw error
  }

  return SunVectorEIVE;
}