/*
  Author:  Pate Williams (c) 1997

  Exercise III.2.12
  "In order to increase the difficulty of breaking
  your cryptosystem, you decide to encipher a
  digraph-vector in the 26-letter alphabet by first
  applying the matrix
  | 3 11 |
  | 4 15 |,
  working modulo 26, and then applying the matrix
  | 10 15 |
  |  5  9 |,
  working modulo 29. Thus while your plaintexts
  are in the 26-letter alphabet, your ciphertexts
  will be in the 29-alphabet we used in Exercise
  9.
  (a) Encipher the message "SEND".
  (b) Describe how to decipher a ciphertext by
  applying two matrices in succession, and
  decipher "ZMOY"." -Neal Koblitz-
  See "A Course in Number Theory and Cryptography"
  by Neal Koblitz second edition page 79.
*/

#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 '!';
}

void multiply(long p, long **A, long *x, long *y)
/* y = Ax */
{
  y[0] = (A[0][0] * x[0] + A[0][1] * x[1]) % p;
  y[1] = (A[1][0] * x[0] + A[1][1] * x[1]) % p;
}

int main(void)
{
  long p1 = 26, p2 = 29, x[2], y[2], z[2];
  long **A = create_square_matrix(2);
  long **B = create_square_matrix(2);
  long **C = create_square_matrix(2);
  long **D = create_square_matrix(2);
  long **E = create_square_matrix(2);
  long **F = create_square_matrix(2);

  A[0][0] = B[0][0] = 3;
  A[0][1] = B[0][1] = 11;
  A[1][0] = B[1][0] = 4;
  A[1][1] = B[1][1] = 15;
  C[0][0] = D[0][0] = 10;
  C[0][1] = D[0][1] = 15;
  C[1][0] = D[1][0] = 5;
  C[1][1] = D[1][1] = 9;
  inverse(2, p1, B, E);
  inverse(2, p2, D, F);
  x[0] = 'S' - 'A';
  x[1] = 'E' - 'A';
  multiply(p1, A, x, y);
  multiply(p2, C, y, z);
  printf("| %2ld %2ld | mod %2ld\n", A[0][0], A[0][1], p1);
  printf("| %2ld %2ld |\n", A[1][0], A[1][1], p1);
  printf("| %2ld %2ld | mod %2ld\n", C[0][0], C[0][1], p2);
  printf("| %2ld %2ld |\n", C[1][0], C[1][1]);
  printf("%c%c", plain_translate(z[0]),
         plain_translate(z[1]));
  x[0] = 'N' - 'A';
  x[1] = 'D' - 'A';
  multiply(p1, A, x, y);
  multiply(p2, C, y, z);
  printf("%c%c", plain_translate(z[0]),
         plain_translate(z[1]));
  printf("\n");
  x[0] = 'Z' - 'A';
  x[1] = 'M' - 'A';
  multiply(p2, F, x, y);
  multiply(p1, E, y, z);
  printf("| %2ld %2ld | mod %2ld\n", F[0][0], F[0][1], p2);
  printf("| %2ld %2ld |\n", F[1][0], F[1][1], p1);
  printf("| %2ld %2ld | mod %2ld\n", E[0][0], E[0][1], p1);
  printf("| %2ld %2ld |\n", E[1][0], E[1][1]);
  printf("%c%c", z[0] + 'A', z[1] + 'A');
  x[0] = 'O' - 'A';
  x[1] = 'Y' - 'A';
  multiply(p2, F, x, y);
  multiply(p1, E, y, z);
  printf("%c%c", plain_translate(z[0]),
         plain_translate(z[1]));
  printf("\n");
  delete_square_matrix(2, A);
  delete_square_matrix(2, B);
  delete_square_matrix(2, C);
  delete_square_matrix(2, D);
  delete_square_matrix(2, E);
  delete_square_matrix(2, F);
  return 0;
}