//////////////////////////////////////////////////////////////////////////////
//
//       Title           : MathTut2.cpp
//       Project         : OSCAR
//       Created         : 7/23/2003
//       Author          : Peter S. March
//       Platforms       : All
///       Copyright      : Copyright© The University of Texas at Austin, 2002. All rights reserved.
///                 
///          This software and documentation constitute an unpublished work
///          and contain valuable trade secrets and proprietary information
///          belonging to University.  None of the foregoing material may be
///          copied  or duplicated or disclosed without the express, written
///          permission of University.  UNIVERSITY EXPRESSLY DISCLAIMS ANY
///          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
///          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
///          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
///          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
///          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
///          THE SOFTWARE OR DOCUMENTATION.  Under no circumstances shall
///          University be liable for incidental, special, indirect, direct or
///          consequential damages or loss of profits, interruption of business,
///          or related expenses which may arise from use of software or documentation,
///          including but not limited to those resulting from defects in software
///          and/or documentation, or loss or inaccuracy of data of any kind.
//       Access          : Company Confidential
//       Purpose         :
//
//----------------------------------------------------------------------------
//
//       Classes:
//               <none>
//
//       Global Functions:
//               <none>
//
//       Global Variables:
//               <none>
//
//----------------------------------------------------------------------------
//
//       $Revisions$
//
//       $Log: MathTut2.cpp,v $
//       Revision 1.3  2006/08/25 19:42:10  jknoll
//       now compatible with current OSCAR
//
//       Revision 1.2  2005/01/11 17:41:16  pmarch
//       no message
//
//       Revision 1.1  2003/08/14 16:47:21  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////

#include "Math/Vector.h"
#include "Math/FixedSizeMatrix.h"
#include "Math/JointVector.h"
#include "Math/MathUtilities.h"

using namespace OSCAR;
using namespace std;


int main(void){

  //Create a FixedSizeArray object of size 4.  This is
  //a template class that takes a class name and size as
  //arguments.  This line creates an object that derives 
  //a 4x1 vector from Vector.
  FixedSizeArray<Vector, 4> vec4;

  vec4.at(0)=1.0;
  vec4.at(1)=9.0;
  vec4.at(2)=8.0;
  vec4.at(3)=0.0;

  cout << "Vector vec4:" << endl;
  cout << vec4 << endl << endl;

  //Create a FixedSizeMatrix object of size 3x4.  This is
  //a template class that takes the number of rows and columns
  //as arguments.
  FixedSizeMatrix<3,4> fixedMatrix;

  fixedMatrix.at(0,0)=1.0;  fixedMatrix.at(0,1)=-0.3; fixedMatrix.at(0,2)=3.4;  fixedMatrix.at(0,3)=1.7;
  fixedMatrix.at(1,0)=0.5;  fixedMatrix.at(1,1)=1.3;  fixedMatrix.at(1,2)=5.2;  fixedMatrix.at(1,3)=-1.2;
  fixedMatrix.at(2,0)=3.3;  fixedMatrix.at(2,1)=0.5;  fixedMatrix.at(2,2)=1.1;  fixedMatrix.at(2,3)=1.0;
  
  cout << "3x4 matrix:" << endl;
  cout << fixedMatrix << endl;

  //Multiply these two objects and display result.
  cout << "matrix * vec4:" << endl;
  cout << (fixedMatrix*vec4) << endl << endl;
  
  //Create an 8x1 JointVector and populate it.
  JointVector jointVec(8);

  jointVec.at(0)=1; jointVec.at(1)=3; jointVec.at(2)=-3;  jointVec.at(3)=2;
  jointVec.at(4)=0; jointVec.at(5)=7; jointVec.at(6)=4;   jointVec.at(7)=-10;

  cout << "8x1 JointVector:" << endl;
  cout << jointVec << endl << endl;

  //Set the 1st and 3rd elements to be Inactive.  This
  //tells certain classes to ignore these elements.  This
  //will be explained more later.
  jointVec.SetCoordinateStatus(0,Inactive);
  jointVec.SetCoordinateStatus(2,Inactive);

  //This for loop cycles through the elements of the
  //joint vector and uses GetCoordinateStatus() to check
  //if a particular element is Active or Inactive.  If
  //the element is Active, it is displayed to the screen.
  cout << "values for 'Active' JointVector elements: (1st and 3rd set to 'Inactive')" << endl;
  for (unsigned int i=0;i<jointVec.GetSize();i++){
    if (jointVec.GetCoordinateStatus(i) == Active)
      cout << jointVec.at(i) << endl;
  }
  return 0;
}