More work

ddecal/gain_solvers/DiagonalAntennaSolver.cc

@@ -55,6 +55,20 @@ void AddSolution(gsl_vector* solutions, size_t solution_index,
                  added_term[1].imag() + gsl_vector_get(solutions, index + 3));
 }
 
+void AddSolutionToMatrix(gsl_matrix* matrix, size_t row, size_t col, const MC2x2& added_term) {
+  const size_t i = row * 4;
+  const size_t j = col * 4;
+  gsl_matrix_set(matrix, i, j,
+                 added_term[0].real() + gsl_matrix_get(matrix, i, j));
+  gsl_matrix_set(matrix, i, j+1,
+                 added_term[0].imag() + gsl_matrix_get(matrix, i, j+1));
+  gsl_matrix_set(matrix, i, j+2,
+                 added_term[1].real() + gsl_matrix_get(matrix, i, j+2));
+  gsl_matrix_set(matrix, i, j+3,
+                 added_term[1].imag() + gsl_matrix_get(matrix, i, j+3));
+  //TODO
+}
+
 }  // namespace
 
 DiagonalAntennaSolver::SolveResult DiagonalAntennaSolver::Solve(
@@ -394,7 +408,21 @@ void DiagonalAntennaSolver::PenaltyJTJ(const gsl_vector* x,
                                        SolverInfo& solver_info,
                                        gsl_matrix* jtj) {
   // - jtj has (n_antennas*4)^2 elements.
-  // TODO
+  gsl_matrix_set_all(jtj, 0.0);
+  const FullSolveData::ChannelBlockData& cb_data = solver_info.cb_data;
+  const size_t n_visibilities = cb_data.NVisibilities();
+  for (size_t vis_index = 0; vis_index != n_visibilities; ++vis_index) {
+    const uint32_t vis_solution_index =
+        cb_data.SolutionIndex(solver_info.direction_index, vis_index);
+    if (vis_solution_index == solver_info.solution_index) {
+      const size_t antenna_1 = cb_data.Antenna1Index(vis_index);
+      const size_t antenna_2 = cb_data.Antenna2Index(vis_index);
+      const MC2x2Diag solution_1 = GetSolution(x, antenna_1);
+      const MC2x2Diag solution_2 = GetSolution(x, antenna_2);
+      const MC2x2 model(
+          cb_data.ModelVisibility(solver_info.direction_index, vis_index));
+    }
+  }
 }
 
 int DiagonalAntennaSolver::PenaltyVV(const gsl_vector* x, const gsl_vector* v,