/* See {hr2_pmap_opt_translation.h}. */
/* Last edited on 2024-09-05 13:47:57 by stolfi */

#define _GNU_SOURCE
#include <stdint.h>
#include <assert.h>
#include <math.h>
 
#include <bool.h>
#include <sign.h>
#include <r2.h>
#include <affirm.h>

#include <hr2_pmap_opt_congruence.h>

void hr2_pmap_encode_congruence(hr2_pmap_t *M, double y[])
  { 
    double A00 = M->dir.c[0][0];
    
    y[0] = M->dir.c[0][1] / A00;
    y[1] = M->dir.c[0][2] / A00;
    
    r2_t u = (r2_t){{ +M->dir.c[1][1]/A00, +M->dir.c[1][2]/A00 }};
    double ang = atan2(u.c[1], u.c[0]);
    y[2] = ang;
  }

void hr2_pmap_decode_congruence(double y[], sign_t sgn, hr2_pmap_t *M)
  { 
    r3x3_ident(&M->dir);
    M->dir.c[0][1] = y[0];
    M->dir.c[0][2] = y[1];
    
    double c = cos(y[2]), s = sin(y[2]);
    
    M->dir.c[1][1] = + c;
    M->dir.c[1][2] = + s;

    if (sgn > 0)
      { M->dir.c[2][1] = - M->dir.c[1][2];
        M->dir.c[2][2] = + M->dir.c[1][1];
      }
    else
      { M->dir.c[2][1] = + M->dir.c[1][2];
        M->dir.c[2][2] = - M->dir.c[1][1];
      }
      
    r3x3_inv(&(M->dir), &(M->inv));

    /* Just in case: */
    M->inv.c[1][0] = 0.0;
    M->inv.c[2][0] = 0.0;
  }