/** This program contains routines to process, integrate, graph, and curve-fit
    data for inertial navigation.
*/

use Grid.frink
use LeastSquares.frink

headerRows = 2
timestep = 0.5 s
grid = new array
for line = readLines["file:inertialdata01.txt.csv"]
{
   line = split["\t",line]
   grid.push[line]
}


ax = mulvec[eval[rest[grid.getColumn[8], headerRows]], m/s^2]
ay = mulvec[eval[rest[grid.getColumn[9], headerRows]], m/s^2]
az = mulvec[eval[rest[grid.getColumn[10], headerRows]],m/s^2]

vx = sumAccel[ax, timestep]
vy = sumAccel[ay, timestep]
vz = sumAccel[az, timestep]

dx = sumVelocity[vx, timestep]
dy = sumVelocity[vy, timestep]
dz = sumVelocity[vz, timestep]


xvalues = makeTimes[dx, timestep, 0 s]

basisFuncs = noEval[[x]]
ls = new LeastSquares[xvalues, vy, basisFuncs]
println[ls.toMatrix[].formatMatrix[]]
println[ls.toExpressionString["t"]]
println["Residual: " + ls.residual[]]
println["r-value: " + ls.rValue[]]
fitfunc = ls.toFunction[]

lsdy = new LeastSquares[xvalues, dy, noEval[[x^2, x]]]
//graphVel[vx, timestep, "x"]
graphVel[vy, timestep, fitfunc]
graphDist[dx, timestep]
graphDist[dy, timestep, lsdy.toFunction[]]

graphVel[c, timestep, func=undef] :=
{
   g = new graphics
   t0 = 0 s
   p = new polyline
   pp = new polyline
   for i = rangeOf[c]
   {
      t = i*timestep
      p.addPoint[t, -(c@i)]
      if func != undef
         pp.addPoint[t, -func[t]]
   }

   g.add[p]
   g.color[0,0,1,.5]
   g.add[pp]
   g.color[0,0,0]

   grid = new Grid
   grid.setUnits[s, -m/s]
   grid.auto[g]
   g.add[grid.getGrid[]]
   g.show[]
}

graphDist[c, timestep, func = undef] :=
{
   g = new graphics
   t0 = 0 s
   p = new polyline
   pp = new polyline
   for i = rangeOf[c]
   {
      t = i*timestep
      d = c@i / m
      p.addPoint[t, -d]
      if func != undef
         pp.addPoint[t, -func[t]/m]
   }

   g.add[p]
   g.color[0,0,1,.5]
   g.add[pp]
   g.color[0,0,0,.5]

   grid = new Grid
   grid.setUnits[s, -1]
   grid.auto[g]
   g.add[grid.getGrid[]]
   g.show[]
}

sumAccel[a, timestep, v0 = 0 m/s] :=
{
   lastV = v0
   ret = new array
   for i = rangeOf[a]
   {
      lastV =  lastV + timestep * a@i
      ret.push[lastV]
   }

   return ret
}

sumVelocity[v, timestep, d0 = 0 m] :=
{
   lastD = d0
   ret = new array
   for i = rangeOf[v]
   {
      lastD =  lastD + timestep * v@i
      ret.push[lastD]
   }

   return ret
}

mulvec[vec, scalar] :=
{
   ret = new array
   for i = rangeOf[vec]
   {
      v = vec@i
      if isUnit[v]
         ret.push[vec@i * scalar]
   }
   return ret
}

makeTimes[vec, timestep, t0] :=
{
   lastT = t0
   ret = new array
   for i = rangeOf[vec]
   {
      lastT =  lastT + timestep
      ret.push[lastT]
   }

   return ret
}

/** Function to turn a spreadsheet column designator into a column index.
    For example, this turns column name "A" to 0 and column name "Z" to 25 and
    column name "AA" to 26.
*/
ssColumnToIndex[name] :=
{
   name = uc[trim[name]]
   ret = 0
   len = length[name]
   pow = 1
   for i = len-1 to 0 step -1
   {
      ret = ret + (char[substrLen[name, i, 1]] - char["A"] + 1) * pow
      pow = pow * 26
   }

   return ret-1  // We want zero-based column number.
}