/***************************************************************************************************************************************
*                                                                                          Lance Simms, LLNL 2011
*
*NAME: 
*  SunAcquisitionWithSolarPanels.c
*
*PURPOSE: 
#  To provide the control torque necessary to orient the +z axis of the CXBN satellite 
#  toward the sun.  The idea behind this is that the shadow of the CubeSat will introduce an 
#  uneven amount of illumination on the solar panels (the ones facing the sun will see 
#  more intensity than the ones being shaded).  The disparity in current between the +x and 
#  -x panels can be used to assess how much torque is needed about the y axis.  And that for 
#  the +y and -y panels can be used for the x axis.
#
#  This control loop has been tested in the CXBN control simulator and verified to 
#  produce the correct results assuming that the current reads about 525 milliamps at full 
#  illumination and 0-50 milliamps when the panels are not facing the sun (non-negligible 
#  current due to sunlight reflected off the moon and earth).  When the current falls in 
#  latter range, no torques are exerted.  The satellite will not get "stuck" because 
#  the aerodynamic and gravity gradient torques will eventually bring it into a sun 
#  facing position. 
#
#  The magnetorquer command is driven by an 8 bit counter value. The value range 0 to 255. 
#  The value 128 being center and no torque. The values 0 and 255 being the maximum torques 
#  either direction.
#
#INPUTS:
#  SolarPanelCurrentArr: 4x1 float array
#    The current measured by the solar panels.  It is in the order +x, +y, -x, -y
#  B_Field: float 3-vector
#    The 3-vector that represents the magnetic field measured by the magnetometers 
#    in the current cycle (in Gauss; could be in Teslas since value is normalized).
#  omega_m: float 3-vector
#    The current angular velocity vector as measured by the gyros (in degrees/second) 
#  k_t: float
#    The control loop gain constant for the angular displacement (theta)
#  k_w: float
#    The control loop gain constant for the angular displacement (theta dot)
#  m_x_byte: unsigned char*
#    A pointer to the char that will contain the x counter value
#  m_y_byte: unsigned char*
#    A pointer to the char that will contain the y counter value 
#  m_z_byte: unsigned char*
#    A pointer to the char that will contain the z counter value 
#
***************************************************************************************************************************************/

//Declare some constants
#define M_MAX        0.166      //Maximum magnetic moment in A*m^2
#define PI           3.14159    //Pi
#define K_T_DEFAULT  0.001      //Default value for angular control loop gain constant (verified in simulations)
#define K_W_DEFAULT  -0.10      //Default value for angular velocity control gain constant (verified in simulations)
#define SP_0_CURRENT 50         //Value at which we consider the solar panels to be unilluminated
#define MAG_FULL_SC  255        //Full range of input to magnetorquers (8 bit system now)

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

//Typedef for the vector
typedef struct {
  float x;
  float y;
  float z;
} vec3;

//Function Declarations
int SunAcquisitionWithSolarPanels(float* SolarPanelCurrentArr, vec3 B_Field, vec3 omega_m, float k_t, float k_w, \
                                  unsigned char* m_x_byte, unsigned char* m_y_byte, unsigned char* m_z_byte);

//SIMPLE CALLING FUNCTION ===============================================================================================================
int main(){

  //Return value for function
  int   RetVal;

  //Set up variables for magnetorquer control
  unsigned char  m_x_byte;
  unsigned char  m_y_byte;
  unsigned char  m_z_byte;

  //Vector for the current magnetic field
  vec3  B_Field;

  //Vector for the current angular velocity (measured by the gyros in degrees/second)
  vec3  omega_m;

  //Constants for the control loop (t is for theta, w is for omega) 
  float k_t;
  float k_w;
  
  //Array to hold the solar panel currents (ordered as +x,+y,-x,-y)
  //float SolarPanelCurrentArr[4] = {480, 490, 500, 502};
  float SolarPanelCurrentArr[4] = {20, 30, 24, 22};

  //Assign values to these
  k_t = K_T_DEFAULT;
  k_w = K_W_DEFAULT;
  B_Field.x = 1.25e-3;   //X component of B Field at t minus 0 in Gauss
  B_Field.y = 1.00e-3;   //Y component of B Field at t minus 0 in Gauss
  B_Field.z = 1.00e-3;   //Z component of B Field at t minus 0 in Gauss

  //Execute the function
  RetVal = SunAcquisitionWithSolarPanels(SolarPanelCurrentArr, B_Field, omega_m, k_t, k_w, &m_x_byte, &m_y_byte, &m_z_byte);

  //Print out what the values were
  printf("Return Value      : %d\n\r", RetVal);
  printf("m_x counter value : %d\n\r", m_x_byte);
  printf("m_y counter value : %d\n\r", m_y_byte);
  printf("m_z counter value : %d\n\r", m_z_byte);

  //Return something for main
  return 1;

}

//DETUMBLE ==============================================================================================================================
int SunAcquisitionWithSolarPanels(float* SolarPanelCurrentArr, vec3 B_Field, vec3 omega_m, float k_t, float k_w, \
                                  unsigned char* m_x_byte, unsigned char* m_y_byte, unsigned char* m_z_byte){

  //Magnitude of B Field and the factor for normalization
  float B_Field_l = 0.0;
  float B_Field_Normalization = 0.0;

  //Set up a vector for the desired angular velocity
  vec3  omega_d;
  omega_d.x = 0.0;
  omega_d.y = 0.0;
  omega_d.z = 0.0;

  //Declare variables for magnetic moment
  float m_x_temp = 0.0;              //temporary value to hold x component of magnetic momemnt
  float m_y_temp = 0.0;              //temporary value to hold x component of magnetic momemnt
  float m_z_temp = 0.0;              //temporary value to hold x component of magnetic momemnt
  float m_s_temp = 0.0;              //sign of magnetic field

  //Declare values for the 8-bit values that control the torque
  unsigned char  m_x_byte_temp = 0;  //0-255 value for the x magnetorquer
  unsigned char  m_y_byte_temp = 0;  //0-255 value for the y magnetorquer
  unsigned char  m_z_byte_temp = 0;  //0-255 value for the z magnetorquer

  //Difference between panels 
  float DiffXPanelsCurrent = 0.0;    //Difference between the +x and -x panel current
  float DiffYPanelsCurrent = 0.0;    //Difference between the +y and -y panel current

  float XTorqueProxy = 0.0;          //A proxy for the torque about the x axis
  float YTorqueProxy = 0.0;          //A proxy for the torque about the y axis 

  //Set the control loop gain constant for the moment
  if (k_t == 0.0) k_t = K_T_DEFAULT;
  if (k_w == 0.0) k_w = K_W_DEFAULT;

  //First determine whether we are even getting illumination
  if ((SolarPanelCurrentArr[0] < SP_0_CURRENT) & (SolarPanelCurrentArr[1] < SP_0_CURRENT) & \
      (SolarPanelCurrentArr[2] < SP_0_CURRENT) & (SolarPanelCurrentArr[3] < SP_0_CURRENT)){
    //Apply no torque since there is no illumination
    *m_x_byte = 128; 
    *m_y_byte = 128;
    *m_z_byte = 128;
    //It was a success, technically
    return 1;
  } 

  //Convert the angular velocities to radians per second
  omega_m.x = omega_m.x*PI/180;
  omega_m.y = omega_m.y*PI/180;

  //Now determine the appropriate amount of torque   
  DiffXPanelsCurrent = SolarPanelCurrentArr[0]-SolarPanelCurrentArr[2];
  DiffYPanelsCurrent = SolarPanelCurrentArr[1]-SolarPanelCurrentArr[3];
 
  //And calculate the angular velocity we want (none along the z axis)
  omega_d.x = k_t * DiffYPanelsCurrent + k_w * omega_m.x;
  omega_d.y = k_t * DiffXPanelsCurrent + k_w * omega_m.y;
  omega_d.z = 0.0;

  //Calculate the length of the magnetic field vector (actually, the length squared)
  B_Field_l = sqrt(B_Field.x*B_Field.x + B_Field.y*B_Field.y + B_Field.z*B_Field.z);

  //Make sure we are not dividing by zero
  if (B_Field_l == 0) return -1;

  //Calculate the normalization for the magnetic field
  B_Field_Normalization = (1/(B_Field_l*B_Field_l));

  //Now evaluate the cross product
  m_x_temp = -B_Field_Normalization*B_Field.z*omega_d.y;
  m_y_temp = +B_Field_Normalization*B_Field.z*omega_d.x;
  m_z_temp = +B_Field_Normalization*(B_Field.x*omega_d.y-B_Field.y*omega_d.x); 

  //Now make sure these dipole moment components are not bigger than what is allowable
  if (fabs(m_x_temp) > M_MAX){
    if (m_x_temp < 0) 
      m_s_temp = -1.0; 
    else 
      m_s_temp = 1.0;
    m_x_temp = m_s_temp * M_MAX; 
  }
  if (fabs(m_y_temp) > M_MAX){
    if (m_y_temp < 0) 
      m_s_temp = -1.0; 
    else 
      m_s_temp = 1.0;
    m_y_temp = m_s_temp * M_MAX;
  }
  if (fabs(m_z_temp) > M_MAX){
    if (m_z_temp < 0) 
      m_s_temp = -1.0; 
    else 
      m_s_temp = 1.0;
    m_z_temp = m_s_temp * M_MAX;
  }

  //Convert these to integer values in the 0-255 bit range
  m_x_byte_temp = (unsigned char)(roundf(((m_x_temp + M_MAX)/(2*M_MAX))*255));
  m_y_byte_temp = (unsigned char)(roundf(((m_y_temp + M_MAX)/(2*M_MAX))*255));
  m_z_byte_temp = (unsigned char)(roundf(((m_z_temp + M_MAX)/(2*M_MAX))*255));
 
  //Now return them
  *m_x_byte = m_x_byte_temp;
  *m_y_byte = m_y_byte_temp;
  *m_z_byte = m_z_byte_temp;
 
  //Return indication of success
  return 1;

}

