/***************************************************************************************************************************************
*                                                                                          Lance Simms, LLNL 2011
*
*NAME: 
*  Detumble.c
*
*PURPOSE: 
#  To provide the control torque that will bring the CXBN CubeSat from its initial 
#  state of torque-free precession after being released from its Ppod to a 
#  state where its only angular velocity is along the z-axis.  In other words, 
#  this is the control torque that will detumble satellite.
#
#  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:
#  B_Field_cur: 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).
#  B_Field_prev: float 3-vector
#    The 3-vector that represents the magnetic field measured by the magnetometers 
#    in the previous cycle (in Gauss; could be in Teslas since value is normalized).
#  K_B: float
#    The control loop gain constant.
#  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 K_B_DEFAULT 20.0  //Default value for control loop gain constant (verified in simulations)
#define MAG_FULL_SC 255   //Full range of input to magnetorquers (8 bit system now)
#include <stdio.h> 
#include <math.h>

typedef struct {
  float x;
  float y;
  float z;
} vec3;

//Function Declarations
int Detumble(vec3 B_Field_cur, vec3 B_Field_prev, float K_B, 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 B Field
  unsigned char  m_x_byte;
  unsigned char  m_y_byte;
  unsigned char  m_z_byte;
  vec3  B_Field_cur;
  vec3  B_Field_prev;
  float K_B;

  //Assign values to these
  K_B = K_B_DEFAULT;
  B_Field_cur.x = 1.25e-3;   //X component of B Field at t minus 0 in Gauss
  B_Field_cur.y = 1.00e-3;   //Y component of B Field at t minus 0 in Gauss
  B_Field_cur.z = 1.00e-3;   //Z component of B Field at t minus 0 in Gauss
  B_Field_prev.x = -1.25e-3;  //X component of B Field at t minus 1 in Gauss
  B_Field_prev.y = -1.00e-3;  //Y component of B Field at t minus 1 in Gauss
  B_Field_prev.z = -4.00e-3;  //Z component of B Field at t minus 1 in Gauss

  //Execute the function
  RetVal = Detumble(B_Field_cur, B_Field_prev, K_B, &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 Detumble(vec3 B_Field_cur, vec3 B_Field_prev, float K_B, unsigned char* m_x_byte, unsigned char* m_y_byte, unsigned char* m_z_byte){

  //Declare variables for calculation
  float B_dot_x = 0.0;               //x component of magnetic field derivative
  float B_dot_y = 0.0;               //y component of magnetic field derivative
  float B_dot_z = 0.0;               //z component of magnetic field derivative
  float B_dot_l = 0.0;               //length of magnetic field vector

  //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

  //Set the control loop gain constant for the moment
  if (K_B == 0.0) K_B = K_B_DEFAULT;
 
  //Calculate the length of the magnetic field vector (actually, the length squared)
  B_dot_l = sqrt(B_Field_cur.x*B_Field_cur.x + B_Field_cur.y*B_Field_cur.y + B_Field_cur.z*B_Field_cur.z);
 
  //Make sure we are not dividing by zero
  if (B_dot_l == 0) return -1;

  //Now evaluate B_dot
  B_dot_x = (1/B_dot_l)*(B_Field_cur.x-B_Field_prev.x);
  B_dot_y = (1/B_dot_l)*(B_Field_cur.y-B_Field_prev.y);
  B_dot_z = (1/B_dot_l)*(B_Field_cur.z-B_Field_prev.z);

  //Calculate the magnetic moment
  m_x_temp = -K_B * B_dot_x;
  m_y_temp = -K_B * B_dot_y;
  m_z_temp = -K_B * B_dot_z + M_MAX;

  //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 success
  return 1;

}

