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

#include "SusConverter.h"

#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>  //for atan2

#include <iostream>

bool SusConverter::checkSunSensorData(lp_vec_t<uint16_t, 6> susChannel) {
  if (susChannel.value[0] <= susChannelValueCheckLow ||
      susChannel.value[0] > susChannelValueCheckHigh ||
      susChannel.value[0] > susChannel.value[GNDREF]) {
    return false;
  }
  if (susChannel.value[1] <= susChannelValueCheckLow ||
      susChannel.value[1] > susChannelValueCheckHigh ||
      susChannel.value[1] > susChannel.value[GNDREF]) {
    return false;
  };
  if (susChannel.value[2] <= susChannelValueCheckLow ||
      susChannel.value[2] > susChannelValueCheckHigh ||
      susChannel.value[2] > susChannel.value[GNDREF]) {
    return false;
  };
  if (susChannel.value[3] <= susChannelValueCheckLow ||
      susChannel.value[3] > susChannelValueCheckHigh ||
      susChannel.value[3] > susChannel.value[GNDREF]) {
    return false;
  };

  susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] +
                                                       susChannel.value[2] + susChannel.value[3]);
  if ((susChannelValueSum < susChannelValueSumHigh) &&
      (susChannelValueSum > susChannelValueSumLow)) {
    return false;
  };
  return true;
}

void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> susChannel) {
  float xout, yout;
  float s = 0.03;  // s=[mm] gap between diodes
  uint8_t d = 5;   // d=[mm] edge length of the quadratic aperture
  uint8_t h = 1;   // h=[mm] distance between diodes and aperture
  int ch0, ch1, ch2, ch3;
  // Substract measurement values from GNDREF zero current threshold
  ch0 = susChannel.value[GNDREF] - susChannel.value[0];
  ch1 = susChannel.value[GNDREF] - susChannel.value[1];
  ch2 = susChannel.value[GNDREF] - susChannel.value[2];
  ch3 = susChannel.value[GNDREF] - susChannel.value[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[0] = atan2(xout, h) * (180 / M_PI);  //[°]
  alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI);  //[°]
}

void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
  uint8_t index, k, l;

  // 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 ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) &&
           alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) &&
          (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) &&
           alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) {
        index = (3 * (k - 1) + l) - 1;  // calculate the index of the datapoint for the right cell
        alphaBetaCalibrated[0] =
            coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +
            coeffAlpha[index][2] * alphaBetaRaw[1] +
            coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
            coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
            coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
            coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
            coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
            coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
            coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
        alphaBetaCalibrated[1] =
            coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] +
            coeffBeta[index][2] * alphaBetaRaw[1] +
            coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
            coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
            coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
            coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
            coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
            coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
            coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
      }
    }
  }
}

float* SusConverter::calculateSunVector() {
  // Calculate the normalized Sun Vector
  sunVectorBodyFrame[0] = (tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
                           (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
                                 powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
  sunVectorBodyFrame[1] = (tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
                           (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
                                 powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
  sunVectorBodyFrame[2] =
      (-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
                  powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));

  return sunVectorBodyFrame;
}

float* SusConverter::getSunVectorSensorFrame(lp_vec_t<uint16_t, 6> susChannel,
                                             const float coeffAlpha[9][10],
                                             const float coeffBeta[9][10]) {
  calcAngle(susChannel);
  calibration(coeffAlpha, coeffBeta);
  return calculateSunVector();
}

bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; }

float* SusConverter::TransferSunVector() {
  float* sunVectorEIVE = 0;
  sunVectorEIVE = new float[3];

  uint8_t susAvail = 12;
  int8_t basisMatrixUse[3][3];
  float sunVectorMatrixEIVE[3][12] = {0};
  float sunVectorMatrixBodyFrame[3][12];

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

  for (uint8_t susNumber = 0; susNumber < 12; susNumber++) {
    if (getValidFlag(susNumber) == returnvalue::FAILED) {
      susAvail -= 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 (susNumber) {
          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][susNumber] +=
            (basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]);
      }
    }
  }

  if (susAvail > 0) {  // Calculate one sun vector out of all sun vectors from the different SUS
    for (uint8_t i = 0; i < 3; i++) {
      float sum = 0;
      for (uint8_t susNumber = 0; susNumber < 12; susNumber++) {
        if (getValidFlag(susNumber) == returnvalue::OK) {
          sum += sunVectorMatrixEIVE[i][susNumber];
          // printf("%f\n", SunVectorMatrixEIVE[i][susNumber]);
        }
      }
      // ToDo: decide on length on sun vector
      sunVectorEIVE[i] = sum;
    }
    VectorOperations<float>::normalize(sunVectorEIVE, sunVectorEIVE, 3);
  } else {
    // No sus is valid
    throw std::invalid_argument("No sun sensor is valid");  // throw error
  }

  return sunVectorEIVE;
}