/*
  Author:  Pate Williams (c) 1997

  Exercise III.2.9
  "You intercept the message "!IWGVIEX!ZRADRYD",
  which was sent using a linear enciphering
  transformation of digraph-vectors in a 29-letter
  alphabet, in which A-Z have numerical
  equivalents 0-25, blank = 26, ? = 27, and ! = 28.
  You have the last five letters of the plaintext
  are the sender's signature "MARIA".
  (a) Find the deciphering matrix, and read the
  message.
  (b) Find the enciphering matrix, and,
  impersonating Maria's friend Jo, send the
  following reply in code: "DAMN FOG! JO".
  -Neal Koblitz-
  See "A Course in Number Theory and Cryptography"
  by Neal Koblitz second edition page 78.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

long **create_square_matrix(long n)
{
  long i, **matrix = calloc(n, sizeof(long *));

  if (!matrix) {
    fprintf(stderr, "fatal error\ninsufficient memory\n");
    fprintf(stderr, "from create_matrix\n");
    exit(1);
  }
  for (i = 0; i < n; i++) {
    matrix[i] = calloc(n, sizeof(long));
    if (!matrix[i]) {
      fprintf(stderr, "fatal error\ninsufficient memory\n");
      fprintf(stderr, "from create_matrix\n");
      exit(1);
    }
  }
  return matrix;
}

void delete_square_matrix(long n, long **matrix)
{
  long i;

  for (i = 0; i < n; i++) free(matrix[i]);
  free(matrix);
}

void Euclid_extended(long a, long b, long *u,
                     long *v, long *d)
{
  long q, t1, t3, v1, v3;

  *u = 1, *d = a;
  if (b == 0) {
    *v = 0;
    return;
  }
  v1 = 0, v3 = b;
  #ifdef DEBUG
  printf("----------------------------------\n");
  printf(" q    t3   *u   *d   t1   v1   v3\n");
  printf("----------------------------------\n");
  #endif
  while (v3 != 0) {
    q = *d / v3;
    t3 = *d - q * v3;
    t1 = *u - q * v1;
    *u = v1, *d = v3;
    #ifdef DEBUG
    printf("%4ld %4ld %4ld ", q, t3, *u);
    printf("%4ld %4ld %4ld %4ld\n", *d, t1, v1, v3);
    #endif
    v1 = t1, v3 = t3;
  }
  *v = (*d - a * *u) / b;
  #ifdef DEBUG
  printf("----------------------------------\n");
  #endif
}

long inv(long number, long modulus)
{
  long d, u, v;

  Euclid_extended(number, modulus, &u, &v, &d);
  if (d == 1) return u;
  return 0;
}

void gaussian_elimination(long n, long p,
                          long *b, long *x,
                          long **m)
{
  int found;
  long *d = calloc(n, sizeof(long)), ck, dj;
  long i, j, k, l, sum, t;

  if (!d) {
    fprintf(stderr, "fatal error\ninsufficient memory\n");
    fprintf(stderr, "from gaussian_elimination\n");
    exit(1);
  }
  for (j = 0; j < n; j++) {
    found = 0, i = j;
    while (!found && i < n) {
      found = m[i][j] != 0 && inv(m[i][j], p) != 0;
      if (!found) i++;
    }
    if (!found) {
      fprintf(stderr, "fatal error\nnon-invertible matrix\n");
      fprintf(stderr, "from gaussian_elimination\n");
      fprintf(stderr, "j = %ld\n", j);
      for (k = 0; k < n; k++) {
        for (l = 0; l < n; l++)
          printf("%2ld ", m[k][l]);
        printf("\n");
      }
      exit(1);
    }
    if (i > j) {
      /* swap elements */
      for (l = j; l < n; l++)
        t = m[i][l], m[i][l] = m[j][l], m[j][l] = t;
      t = b[i], b[i] = b[j], b[j] = t;
    }
    dj = d[j] = inv(m[j][j], p);
    if (dj == 0) {
      fprintf(stderr, "fatal error\nnon-invertible element\n");
      fprintf(stderr, "from gaussian elimination\n");
      fprintf(stderr, "element %ld mod %ld\n", m[j][j], p);
      exit(1);
    }
    for (k = j + 1; k < n; k++) {
      ck = (dj * m[k][j]) % p;
      for (l = j + 1; l < n; l++) {
        m[k][l] = (m[k][l] - ck * m[j][l]) % p;
        if (m[k][l] < 0) m[k][l] += p;
      }
      b[k] = (b[k] - ck * b[j]) % p;
      if (b[k] < 0) b[k] += p;
    }
  }
  for (i = n - 1; i >= 0; i--) {
    sum = 0;
    for (j = i + 1; j < n; j++)
      sum += (m[i][j] * x[j]) % p;
    if (sum < 0) sum += p;
    x[i] = (d[i] * (b[i] - sum)) % p;
    if (x[i] < 0) x[i] += p;
  }
}

void inverse(long n, long p, long **m, long **X)
{
  int found;
  long d, i, j, k, l, sum, temp;
  long **B = create_square_matrix(n);
  long *c = calloc(n, sizeof(long));

  if (!c) {
    fprintf(stderr, "fatal error\ninsufficient memory\n");
    fprintf(stderr, "from inverse\n");
    exit(1);
  }
  for (i = 0; i < n; i++) B[i][i] = 1;
  for (j = 0; j < n; j++) {
    found = 0;
    for (i = j; i < n && !found;) {
      found = m[i][j] != 0 && inv(m[i][j], p) != 0;
      if (!found) i++;
    }
    if (!found) {
      fprintf(stderr, "fatal error\nnon-invertible matrix\n");
      fprintf(stderr, "from inverse\n", j);
      exit(1);
    }
    if (i > j) {
      for (l = j; l < n; l++) {
        temp = m[i][l];
        m[i][l] = m[j][l];
        m[j][l] = temp;
      }
      for (l = 0; l < n; l++) {
        temp = B[i][l];
        B[i][l] = B[j][l];
        B[j][l] = temp;
      }
    }
    d = inv(m[j][j], p);
    for (k = j + 1; k < n; k++) c[k] = (d * m[k][j]) % p;
    for (k = j + 1; k < n; k++) {
      for (l = j + 1; l < n; l++) {
        m[k][l] -= (c[k] * m[j][l]) % p;
        m[k][l] %= p;
        if (m[k][l] < 0) m[k][l] += p;
      }
    }
    for (k = j + 1; k < n; k++) {
      for (l = 0; l < n; l++) {
        B[k][l] -= (c[k] * B[j][l]) % p;
        B[k][l] %= p;
        if (B[k][l] < 0) B[k][l] += p;
      }
    }
  }
  for (i = n - 1; i >= 0; i--) {
    for (j = 0; j < n; j++) {
      sum = 0;
      for (k = i + 1; k < n; k++)
        sum += m[i][k] * X[k][j];
      X[i][j] = inv(m[i][i], p) * (B[i][j] - sum);
      X[i][j] %= p;
      if (X[i][j] < 0) X[i][j] += p;
    }
  }
  delete_square_matrix(n, B);
  free(c);
}

long cipher_translate(char c)
{
  if (c >= 'A' && c <= 'Z') return c - 'A';
  if (c == ' ') return 26;
  if (c == '?') return 27;
  return 28;
}

char plain_translate(long c)
{
  if (c < 26) return (char) (c + 'A');
  if (c == 26) return ' ';
  if (c == 27) return '?';
  return '!';
}

int main(void)
{
  char ciphertext[32] = "!IWGVIEX!ZRADRYD";
  char plaintext[32] = "DAMN FOG! JO";
  long a, b, i, p = 29, y, z, B[4], x[4];
  long **A = create_square_matrix(4);
  long **m = create_square_matrix(2);
  long **X = create_square_matrix(2);

  A[0][0] = 'A' - 'A';
  A[0][1] = 'R' - 'A';
  A[1][2] = A[0][0];
  A[1][3] = A[0][1];
  A[2][0] = 'I' - 'A';
  A[2][1] = 'A' - 'A';
  A[3][2] = A[2][0];
  A[3][3] = A[2][1];
  B[0] = 'D' - 'A';
  B[1] = 'R' - 'A';
  B[2] = 'Y' - 'A';
  B[3] = 'D' - 'A';
  gaussian_elimination(4, p, B, x, A);
  m[0][0] = x[0], m[0][1] = x[1];
  m[1][0] = x[2], m[1][1] = x[3];
  inverse(2, p, m, X);
  printf("the key is:\n");
  printf("| %2ld %2ld |\n", x[0], x[1]);
  printf("| %2ld %2ld |\n", x[2], x[3]);
  printf("the inverse key is:\n");
  printf("| %2ld %2ld |\n", X[0][0], X[0][1]);
  printf("| %2ld %2ld |\n", X[1][0], X[1][1]);
  printf("%s\n", ciphertext);
  for (i = 0; i < strlen(ciphertext); i += 2) {
    a = cipher_translate(ciphertext[i]);
    b = cipher_translate(ciphertext[i + 1]);
    y = (X[0][0] * a + X[0][1] * b) % p;
    z = (X[1][0] * a + X[1][1] * b) % p;
    printf("%c%c", plain_translate(y),
           plain_translate(z));
  }
  printf("\n");
  m[0][0] = x[0], m[0][1] = x[1];
  m[1][0] = x[2], m[1][1] = x[3];
  printf("%s\n", plaintext);
  for (i = 0; i < strlen(plaintext); i += 2) {
    a = cipher_translate(plaintext[i]);
    b = cipher_translate(plaintext[i + 1]);
    y = (m[0][0] * a + m[0][1] * b) % p;
    z = (m[1][0] * a + m[1][1] * b) % p;
    printf("%c%c", plain_translate(y),
           plain_translate(z));
  }
  printf("\n");
  delete_square_matrix(4, A);
  delete_square_matrix(2, m);
  delete_square_matrix(2, X);
  return 0;
}