/*
 *_________________________________________________________________________*
 *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
 *      DESCRIPTION: SEE READ-ME                                           *
 *      FILE NAME: system.cpp                                              *
 *      AUTHORS: See Author List                                           *
 *      GRANTS: See Grants List                                            *
 *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
 *      LICENSE: Please see License Agreement                              *
 *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
 *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
 *                     Computational Dynamics Lab                          *
 *                     Rensselaer Polytechnic Institute                    *
 *                     110 8th St. Troy NY 12180                           *
 *      CONTACT:        anderk5@rpi.edu                                    *
 *_________________________________________________________________________*/


#include "system.h"
#include "body.h"
#include "joint.h"
#include <cmath>


System::System(){
  mappings = nullptr;
}

System::~System(){
  Delete();
}

void System::Delete(){
  delete [] mappings;
  bodies.DeleteValues();
  joints.DeleteValues();
}

int System::GetNumBodies(){
  return bodies.GetNumElements();
}

int * System::GetMappings()
{
  return mappings;
}

void System::AddBody(Body* body){
  bodies.Append(body);
}

void System::AddJoint(Joint* joint){
  joints.Append(joint);
}

void System::SetTime(double t){
  time = t;
}

double System::GetTime(){
  return time;
}

void System::ComputeForces(){
  // NOT DOING ANYTHING AT THIS TIME
  }

bool System::ReadIn(istream& in){
  int numbodies;
  Body* body;
  int bodytype;
  char bodyname[256];
  int index;

  // get number of bodies
  in >> numbodies;

  // bodies loop
  for(int i=0;i<numbodies;i++){

    // error check
    in >> index;
    if(index != i){
      cerr << "Error reading bodies" << endl;
      return false;
    }

    in >> bodytype >> bodyname;
    body = NewBody(bodytype);

    // type check
    if(!body){
      cerr << "Unrecognized body type '" << bodytype << "'" << endl;
      return false;
    }

    // add the body
    AddBody(body);

    // set generic body info
    body->ChangeName(bodyname);

    // read in the rest of its data
    if(!body->ReadIn(in)) return false;
  }

  // create a temporary array for fast indexed access
  Body** bodyarray = bodies.CreateArray();

  int numjoints;
  int jointtype;
  char jointname[256];
  Joint* joint;
  int body1, body2;
  int point1, point2;

  // get number of joints
  in >> numjoints;

  // joints loop
  for(int i=0;i<numjoints;i++){

    // error check
    in >> index;
    if(index != i){
      cerr << "Error reading joints" << endl;
      return false;
    }

    in >> jointtype >> jointname;
    joint = NewJoint(jointtype);

    // joint type check
    if(!joint){
      cerr << "Unrecognized joint type '" << jointtype << "'" << endl;
      return false;
    }

    // add the joint
    AddJoint(joint);

    // set the generic joint info
    joint->ChangeName(jointname);

    in >> body1 >> body2;
    if( !(body1<numbodies) || !(body2<numbodies) ){
      cerr << "Body index out of range" << endl;
      delete [] bodyarray;
      return false;
    }

    joint->SetBodies(bodyarray[body1], bodyarray[body2]);

    bodyarray[body1]->AddJoint(joint);
    bodyarray[body2]->AddJoint(joint);

    in >> point1 >> point2;

    joint->SetPoints(bodyarray[body1]->GetPoint(point1),bodyarray[body2]->GetPoint(point2));

    // read in the rest of its data
    if(!joint->ReadIn(in)){
      delete [] bodyarray;
      return false;
    }
  }

  // delete the temporary array
  delete [] bodyarray;

  return true;
}

void System::WriteOut(ostream& out){
  // number of bodies
  out << bodies.GetNumElements() << endl;

  // bodies loop
  int i = 0;
  Body* body;
  ListElement<Body>* b_ele = bodies.GetHeadElement();
  while(b_ele !=nullptr){
    out << i << ' ';

    body = b_ele->value;

    // set the body ID for later identification
    body->SetID(i);

    // write out the data
    body->WriteOut(out);

    i++; b_ele = b_ele->next;
  }

  // number of joints
  out << joints.GetNumElements() << endl;

  // joints loop
  i = 0;
  Joint* joint;
  ListElement<Joint>* j_ele = joints.GetHeadElement();
  while(j_ele !=nullptr){
    out << i << ' ';
    joint = j_ele->value;

    // set the joint ID for later identification
    joint->SetID(i);

    // write out the data
    joint->WriteOut(out);

    i++; j_ele = j_ele->next;
  }
}

void System::ClearBodyIDs(){
  ListElement<Body>* current = bodies.GetHeadElement();

  while(current){
    current->value->SetID(0);
    current = current->next;
  }
}

void System::ClearJointIDs(){
  ListElement<Joint>* current = joints.GetHeadElement();

  while(current){
    current->value->SetID(0);
    current = current->next;
  }
}


void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space){

 //-------------------------------------------------------------------------//
  // Declaring Temporary Entities
  //-------------------------------------------------------------------------//
     Body* body = nullptr;
     Body* prev;
     Body* Inertial;
     Point* origin;
     Joint* joint;
     Point* point_CM;
     Point* point_p;
     Point* point_k;
     Point* point_ch = nullptr;
     Vect3 r1,r2,r3,v1,v2,v3;
     Mat3x3 IM, N, PKCK,PKCN ;
     ColMatrix qo, uo, q, qdot,w;

   mappings = new int[nfree];
   for(int i = 0; i < nfree; i++)
   {
     mappings[i] = freelist[i];
   }
     qo.Dim(4);
     uo.Dim(3);
     q.Dim(4);
     qdot.Dim(4);
     PKCN.Identity();
     PKCK.Identity();
     w.Dim(3);

//-------------------------------------------------------------------------//
  // Setting up Inertial Frame, gravity and Origin
  //-------------------------------------------------------------------------//
     Inertial= new InertialFrame;
     AddBody(Inertial);

     Vect3 temp1;
     temp1.Zeros();
     ((InertialFrame*) Inertial)->SetGravity(temp1);
     origin= new FixedPoint(temp1);
     Inertial->AddPoint(origin);
//-------------------------------------------------------------------------//
  double ** xh1 = new double*[nfree];
  double ** xh2 = new double*[nfree];

  for (int i=0; i<nfree; i++){
    xh1[i] = new double[3];
    xh2[i] = new double[3];
  }
  for (int i=0; i<nfree; i++){
    for (int j=0; j<3; j++){
      xh1[i][j] = xcm[mappings[i]-1][j];
    }
  }

//-------------------------------------------------------------------------//
// Begin looping over each body for recursive kinematics
//-------------------------------------------------------------------------//
    for(int i=0;i<nfree;i++){
      prev=Inertial;
      point_p=origin;

     body = new RigidBody;
     body->mass=masstotal[mappings[i]-1];
     IM(1,1)=inertia[mappings[i]-1][0];
     IM(2,2)=inertia[mappings[i]-1][1];
     IM(3,3)=inertia[mappings[i]-1][2];
     IM(1,2)=0.0;
     IM(1,3)=0.0;
     IM(2,3)=0.0;
     IM(2,1)=IM(1,2);
     IM(3,1)=IM(1,3);
     IM(3,2)=IM(2,3);
     body->inertia = IM;

//-------------------------------------------------------//


      for (int k=0;k<3;k++){
          r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];
       r3(k+1) = xcm[mappings[i]-1][k];
       r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k];
      }

     r2.Zeros();

     for (int k=1;k<=3;k++){
          N(k,1)=ex_space[mappings[i]-1][k-1];
          N(k,2)=ey_space[mappings[i]-1][k-1];
          N(k,3)=ez_space[mappings[i]-1][k-1];
         }

     PKCK=T(N);
     PKCN=T(N);

     q.Zeros();
     EP_FromTransformation(q,N);

     r1=PKCN*r1;
     r3=PKCN*r3;

     for (int k=1;k<=3;k++){
     w(k)=omega[mappings[i]-1][k-1];
     }

     Vect3 cart_r, cart_v;
     for (int k=1;k<=3;k++){
       cart_r(k)=xcm[mappings[i]-1][k-1];
       cart_v(k)=vcm[mappings[i]-1][k-1];
       }

       w=PKCN*w;
       EP_Derivatives(q,w,qdot);


//-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS
//-------------------------------------------------------------------------//

     point_CM = new FixedPoint(r2);
     point_k = new FixedPoint(r1);
     point_ch = new FixedPoint(r3);
     body->AddPoint(point_CM);
     body->AddPoint(point_k);
     body->AddPoint(point_ch);
     AddBody(body);

     Mat3x3 One;
     One.Identity();
    ColMatrix qq=Stack(q,cart_r);
          ColMatrix vv=Stack(qdot,cart_v);
          joint=new FreeBodyJoint;
          AddJoint(joint);
          joint->SetBodies(prev,body);
          body->AddJoint(joint);
          prev->AddJoint(joint);
          joint->SetPoints(point_p,point_k);
          joint->SetZeroOrientation(One);
          joint->DimQandU(7,6);
          joint->SetInitialState(qq,vv);
          joint->ForwardKinematics();
  }
  for(int i = 0; i < nfree; i++) {
    delete [] xh1[i];
    delete [] xh2[i];
  }
  delete [] xh1;
  delete [] xh2;
}


void System::Create_System_LAMMPS(int numbodies, double *mass,double **inertia, double ** xcm, double ** xjoint,double **vcm,double **omega,double **ex_space, double **ey_space, double **ez_space, int b, int * mapping, int count){

  //-------------------------------------------------------------------------//
  // Declaring Temporary Entities
  //-------------------------------------------------------------------------//

  Body* body = nullptr;
  Body* prev;
  Body* Inertial;
  Point* origin;
  Joint* joint;
  Point* point_CM;
  Point* point_p;
  Point* point_k;
  Point* point_ch = nullptr;
  Vect3 r1,r2,r3,v1,v2,v3;
  Mat3x3 IM, N, PKCK,PKCN ;
  ColMatrix qo, uo, q, qdot,w;
  Vect3 cart_r, cart_v;
  mappings = new int[b];
  for(int i = 0; i < b; i++){
    mappings[i] = mapping[i];
  }


  qo.Dim(4);
  uo.Dim(3);
  q.Dim(4);
  qdot.Dim(4);
  PKCN.Identity();
  PKCK.Identity();
  w.Dim(3);

  //-------------------------------------------------------------------------//
  // Setting up Inertial Frame, gravity and Origin
  //-------------------------------------------------------------------------//
  Inertial= new InertialFrame;
  AddBody(Inertial);

  Vect3 temp1;
  temp1.Zeros();
  ((InertialFrame*) Inertial)->SetGravity(temp1);
  origin= new FixedPoint(temp1);
  Inertial->AddPoint(origin);
  //-------------------------------------------------------------------------//

  double ** xh1;
  double ** xh2;

  xh1 = new double*[b];
  xh2 = new double*[b];


  for (int i=0; i<b; i++){
    xh1[i] = new double[3];
    xh2[i] = new double[3];
  }



  for (int j=0; j<3; j++){
    xh1[0][j] = xcm[mapping[0]-1][j];
    xh2[b-1][j] = xcm[mapping[b-1]-1][j];
  }

  for (int i=0; i<b-1; i++){
    for (int j=0; j<3; j++){
      xh1[i+1][j] = xjoint[mapping[i]-count-1][j];
    }
  }

  for (int i=0; i<b-1; i++){
    for (int j=0; j<3; j++){
      xh2[i][j] = xjoint[mapping[i]-count-1][j];
    }
  }


  //-------------------------------------------------------------------------//
// Begin looping over each body for recursive kinematics
  //-------------------------------------------------------------------------//
  for(int i=0;i<b;i++){
    if (i == 0){
      prev=Inertial;
      point_p=origin;
    }
    else{
      prev = body;
      point_p = point_ch;
    }


    body = new RigidBody;
    body->mass=mass[mapping[i]-1];
    IM(1,1)=inertia[mapping[i]-1][0];
    IM(2,2)=inertia[mapping[i]-1][1];
    IM(3,3)=inertia[mapping[i]-1][2];
    IM(1,2)=0.0;
    IM(1,3)=0.0;
    IM(2,3)=0.0;
    IM(2,1)=IM(1,2);
    IM(3,1)=IM(1,3);
    IM(3,2)=IM(2,3);
    body->inertia = IM;

    //-------------------------------------------------------//

    for (int k=0;k<3;k++){
      r1(k+1)=xh1[i][k]-xcm[mapping[i]-1][k];
      r3(k+1)=xh2[i][k]-xcm[mapping[i]-1][k];
    }
    r2.Zeros();

    for (int k=1;k<=3;k++){
      N(k,1)=ex_space[mapping[i]-1][k-1];
      N(k,2)=ey_space[mapping[i]-1][k-1];
      N(k,3)=ez_space[mapping[i]-1][k-1];
    }


    if (i==0){
      PKCK=T(N);
      PKCN=T(N);

      q.Zeros();
      EP_FromTransformation(q,N);

      r1=PKCN*r1;
      r3=PKCN*r3;

      for (int k=1;k<=3;k++){
        w(k)=omega[mappings[i]-1][k-1];
      }

      for (int k=1;k<=3;k++){
        cart_r(k)=xcm[mappings[i]-1][k-1];
        cart_v(k)=vcm[mappings[i]-1][k-1];
      }
      w=PKCN*w;
      EP_Derivatives(q,w,qdot);

    }
    else{
      PKCK=PKCN*N;
      PKCN=T(N);

      q.Zeros();
      EP_FromTransformation(q,PKCK);

      r1=PKCN*r1;
      r3=PKCN*r3;

      for (int k=1;k<=3;k++){
        w(k)=omega[mapping[i]-1][k-1]-omega[mapping[i-1]-1][k-1];
      }

      w=PKCN*w;
      EP_Derivatives(q, w, qdot);
    }


    //-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS
    //-------------------------------------------------------------------------//

    point_CM = new FixedPoint(r2);
    point_k = new FixedPoint(r1);
    point_ch = new FixedPoint(r3);
    body->AddPoint(point_CM);
    body->AddPoint(point_k);
    body->AddPoint(point_ch);
    AddBody(body);

    Mat3x3 One;
    One.Identity();
    if (i==0){
      ColMatrix qq=Stack(q,cart_r);
      ColMatrix vv=Stack(qdot,cart_v);
      joint=new FreeBodyJoint;
      AddJoint(joint);
      joint->SetBodies(prev,body);
      body->AddJoint(joint);
      prev->AddJoint(joint);
      joint->SetPoints(point_p,point_k);
      joint->SetZeroOrientation(One);
      joint->DimQandU(7,6);
      joint->SetInitialState(qq,vv);
      joint->ForwardKinematics();
    }
    else{
      joint= new SphericalJoint;
      AddJoint(joint);
      joint->SetBodies(prev,body);
      body->AddJoint(joint);
      prev->AddJoint(joint);
      joint->SetPoints(point_p,point_k);
      joint->SetZeroOrientation(One);
      joint->DimQandU(4,3);
      joint->SetInitialState(q,qdot);
      joint->ForwardKinematics();
    }
  }
  for(int i = 0; i < b; i++)
  {
    delete [] xh1[i];
    delete [] xh2[i];
  }
  delete [] xh1;
  delete [] xh2;

}
