//-------------------------------------------------------------------------
//                  Sampling methods for SO(3)
//-------------------------------------------------------------------------
//
// Copyright (c) 2003 University of Illinois and Steven M. LaValle
// All rights reserved.
//
// Developed by:                Motion Strategy Laboratory
//                              University of Illinois
//                              http://msl.cs.uiuc.edu/msl/
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal with the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
// 
//     * Redistributions of source code must retain the above copyright 
//       notice, this list of conditions and the following disclaimers.
//     * Redistributions in binary form must reproduce the above copyright 
//       notice, this list of conditions and the following disclaimers in 
//       the documentation and/or other materials provided with the 
//       distribution.
//     * Neither the names of the Motion Strategy Laboratory, University
//       of Illinois, nor the names of its contributors may be used to 
//       endorse or promote products derived from this Software without 
//       specific prior written permission.
//
// The software is provided "as is", without warranty of any kind,
// express or implied, including but not limited to the warranties of
// merchantability, fitness for a particular purpose and
// noninfringement.  In no event shall the contributors or copyright
// holders be liable for any claim, damages or other liability, whether
// in an action of contract, tort or otherwise, arising from, out of or
// in connection with the software or the use of other dealings with the
// software.
//
//-------------------------------------------------------------------------


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

#include "sample.h"

// *********************************************************************
// *********************************************************************
// CLASS:     Sample class
// 
// *********************************************************************
// *********************************************************************

Sample::Sample(int smethod){

  readOrder();
  readV();

  SampleMethod = smethod;

  switch(SampleMethod)
    {
    case RandomQuaternions:
      cout << "Sampling method: random quaternion sequence" << endl;
      break;
    case SukGriSequence:
      cout << "Sampling method: Sukharev grid sequence" << endl;
      break;
    case LaySukGriSequence:
    default:
      cout << "Sampling method: layered Sukharev grid sequence" << endl;
      break;
    }   

}


//!------------Generating sequence element with index i-----------------------//
MSLVector Sample::ChooseState(int i) 
{
  int dim = 3;
  switch(SampleMethod)
    {
    case RandomQuaternions:  //random Quaternion sequence
      return RandomState(dim);
      break;
    case SukGriSequence:     //Sukharev Grid Sequence
    case LaySukGriSequence:  //layered Sukharev Grid Sequence
    default:
      return QuatState(i);
      break;
    }    
  return RandomState(dim);
}

//!-----------------------Random quaternion sequence-----------------------//
MSLVector Sample::RandomState(int dim) { 
  double r; 
  double Theta1, Theta2, r1, r2;
  MSLVector rx = MSLVector(dim); 
  MSLVector qrsample(4);

  for (int i = 0; i < dim; i++) { 
      R >> r;  
      rx[i] = r; 
    } 
      
  Theta1 = 2*PI*rx[1];
  Theta2 = PI*rx[2] - PI/2;
  r1 = sqrt(1 - rx[0]);
  r2 = sqrt(rx[0]);
      
  qrsample[0] = sin(Theta1)*r1;
  qrsample[1] = cos(Theta1)*r1;
  qrsample[2] = sin(Theta2)*r2;
  qrsample[3] = cos(Theta2)*r2;
  
  return qrsample;
} 

//!-----------------------Deterministic grid sequences-----------------------//
MSLVector Sample::QuatState(int i) { 
  MSLVector a(3);
  MSLVector qrsample(4);
  
  int faceIndex = (int)fmod(i, (double) 4);
  int index = (int)floor(i/ (double) 4);

  switch(SampleMethod)
    {
    case SukGriSequence:        //Sukharev Grid Sequence
      a = DiscGridSample(index, 3);
      break;    
    case LaySukGriSequence:     //layered Sukharev Grid Sequence
    default:
      a = Choose3dState(index, 3);
      break;
    }    
  transform(a, faceIndex, qrsample);
  
  if (qrsample[3] < 0) qrsample = -qrsample;
  return qrsample;
}


//!--------------------layered Sukharev grid sequence------------------------//
//! Written by Steven Lindemann in the context of----------------------------//
//!"Incremental low-discrepancy lattice methods for motion planning"---------//
//! by S. R. Lindemann and S. M. LaValle-------------------------------------//
//! In Proc. IEEE Internation Conference on Robotics and Automation, 2003----//
MSLVector Sample::Choose3dState(int i, int dim){
    double offset = 0.0;
        //calculate offset based on index
        i++;
        int level = 0;
        offset = 0.5;
        double sampIndex = pow(2.0,dim*level);
        while (i > sampIndex)
        {
            i -= (int)sampIndex;
            offset = offset / 2.0;
            level++;
            sampIndex = pow(2.0,dim*level);
        }
        i--;
    //now i is the index into the grid, offset is the offset
    MSLVector blah(dim), sample(dim);
    for (int j = 0; j < dim; j++)
    {
        blah[j] = offset;
    }
    sample = blah + DiscGridSample(i, dim);
    return sample;
}

//!-----------------------Sukharev grid sequence-----------------------------//
//! Written by Steven Lindemann in the context of----------------------------//
//!"Incremental low-discrepancy lattice methods for motion planning"---------//
//! by S. R. Lindemann and S. M. LaValle-------------------------------------//
//! In Proc. IEEE Internation Conference on Robotics and Automation, 2003----//
MSLVector Sample::DiscGridSample(int i, int dim)
{
    int numberCells = order.size();
    MSLVector sample(dim);
    double currentFactor = 0.5;
    int currentIndex = i%numberCells;
    int blah = i/numberCells;
    while (blah > 0)
    {
        for (int j = 0; j < dim; j++)
        {
            sample[j] += currentFactor*(order[currentIndex])[j];
        }
        currentFactor *= 0.5;
        currentIndex = blah%numberCells;
        blah = blah/numberCells;
    }
    for (int j = 0; j < dim; j++)
    {
        sample[j] += currentFactor*(order[currentIndex])[j];
    }
    return sample;
}

//!----spherical sample using baricentric coordinates of the cube vertices-------//
void Sample::transform(MSLVector a, int faceIndex, MSLVector &qrsample) {
  MSLVector x1 = LinearInterpolate(v[faceIndex][0], v[faceIndex][1], a[0]);
  MSLVector x2 = LinearInterpolate(v[faceIndex][2], v[faceIndex][3], a[0]);
  MSLVector x3 = LinearInterpolate(v[faceIndex][4], v[faceIndex][5], a[0]);
  MSLVector x4 = LinearInterpolate(v[faceIndex][6], v[faceIndex][7], a[0]);

  MSLVector y1 = LinearInterpolate(x1, x3, a[1]);
  MSLVector y2 = LinearInterpolate(x2, x4, a[1]);

  qrsample = LinearInterpolate(y1, y2, a[2]);
} 

//!-------------------spherical linear interpolation---------------------------//
MSLVector Sample::LinearInterpolate(MSLVector x1, MSLVector x2, double a){
  
  MSLVector v = MSLVector (4);
  double cosTheta = x1[0]*x2[0] + x1[1]*x2[1] + x1[2]*x2[2] + x1[3]*x2[3];
  double Theta = acos(cosTheta);
    
  double k1 = sin((1 - a)*Theta)/sin(Theta);
  double k2 = sin(a*Theta)/sin(Theta);
  v[0] = k1*x1[0] + k2*x2[0];
  v[1] = k1*x1[1] + k2*x2[1];
  v[2] = k1*x1[2] + k2*x2[2];
  v[3] = k1*x1[3] + k2*x2[3];
  return v;
}

//!-------------------reads order on faces from a file-------------------------//
void Sample::readOrder() {
    int dimension = 3;
    string filename("order_3.out");

    std::ifstream fin;
    fin.open(filename.c_str());
    char thisCharLine[100];
    string thisLine;
    int *thisPerm;
    if (!fin.bad())
    {
        fin.getline(thisCharLine, 100);
        while ((thisCharLine[0] == '1') || (thisCharLine[0] == '0'))
        {
            thisPerm = new int[dimension];
            for (int i = 0; i < dimension; i++)
            {
                if (thisCharLine[i] == '1')
                {
                    thisPerm[i] = 1;
                }
                else
                {
                    thisPerm[i] = 0;
                }
            }
            order.push_back(thisPerm);
            fin.getline(thisCharLine, 100);
        }
        fin.close();
    }
    else
    {
        cout << "Error reading grid sampling order, exiting." << endl;
        exit(1);
    }
}

//!-------produces the orderings sequence on the vertices of the inscribed cube---------//
void Sample::readV() {
  numCubes = 4;
  numVert = 8;

  MSLVector* vv;
  vv = new MSLVector [numVert];
  for (int i = 0; i < numVert; i++) {
    vv[i] = MSLVector(3);
  }

  vv[0][0] = -0.5; vv[0][1] = -0.5; vv[0][2] = -0.5;
  vv[1][0] =  0.5; vv[1][1] = -0.5; vv[1][2] = -0.5;
  vv[2][0] = -0.5; vv[2][1] = -0.5; vv[2][2] =  0.5;
  vv[3][0] =  0.5; vv[3][1] = -0.5; vv[3][2] =  0.5;
  vv[4][0] = -0.5; vv[4][1] =  0.5; vv[4][2] = -0.5;

  vv[5][0] =  0.5; vv[5][1] =  0.5; vv[5][2] = -0.5;
  vv[6][0] = -0.5; vv[6][1] =  0.5; vv[6][2] =  0.5;
  vv[7][0] =  0.5; vv[7][1] =  0.5; vv[7][2] =  0.5;

  v = new MSLVector *[numCubes];
  for (int i = 0; i < numCubes; i++) {
    v[i] = new MSLVector [numVert];
    for (int j = 0; j < numVert; j++) {
      v[i][j] = MSLVector(4);
      for (int k = 0; k < 4; k++) {
	if (i > k) {v[i][j][k] = vv[j][k];}
	else if (i == k) {v[i][j][k] = 0.5;}
	else {v[i][j][k] = vv[j][k - 1];}
      }
    }
  }
}