#include "ecc.h"
#include "crypto.h"
#include "util.h"
#include "dbg.h"

/* P-256 modulus */
static const uint64_t P256_P[4] = {
    0xFFFFFFFFFFFFFFFFULL, 0x00000000FFFFFFFFULL,
    0x0000000000000000ULL, 0xFFFFFFFF00000001ULL
};

static const uint64_t P256_N[4] = {
    0x5377CE8237E96A2BULL, 0xBCE6FAAD7173E16BULL,
    0x00000000FFFFFFFFULL, 0xFFFFFFFF00000000ULL
};

static const uint64_t P256_GX[4] = {
    0xF4A13945D898C296ULL, 0x77037D812DEB33A0ULL,
    0xF8BCE6E563A440F2ULL, 0x6B17D1F2E12C4247ULL
};

static const uint64_t P256_GY[4] = {
    0xCBB6406837BF51F5ULL, 0x2BCE33576B315ECEULL,
    0x8EE7EB4A7C0F9E16ULL, 0x4FE342E2FE1A7F9BULL
};

static void bn_zero(uint64_t *a) { a[0]=a[1]=a[2]=a[3]=0; }

static void bn_copy(uint64_t *a, const uint64_t *b)
{ a[0]=b[0]; a[1]=b[1]; a[2]=b[2]; a[3]=b[3]; }

static int bn_lt(const uint64_t *a, const uint64_t *b)
{
    for(int i=3;i>=0;i--) { if(a[i]<b[i]) return 1; if(a[i]>b[i]) return 0; }
    return 0;
}

/* 64x64->128 multiply: returns high, stores low in *lo */
static uint64_t mul64(uint64_t a, uint64_t b, uint64_t *lo)
{
    uint32_t a_lo = (uint32_t)a, a_hi = (uint32_t)(a >> 32);
    uint32_t b_lo = (uint32_t)b, b_hi = (uint32_t)(b >> 32);
    uint64_t ll = (uint64_t)a_lo * b_lo;
    uint64_t lh = (uint64_t)a_lo * b_hi;
    uint64_t hl = (uint64_t)a_hi * b_lo;
    uint64_t hh = (uint64_t)a_hi * b_hi;
    uint64_t mid = (ll >> 32) + (uint32_t)lh + (uint32_t)hl;
    *lo = (mid << 32) | (uint32_t)ll;
    return hh + (mid >> 32) + (lh >> 32) + (hl >> 32);
}

static uint64_t bn_add(uint64_t *a, const uint64_t *b)
{
    uint64_t carry=0;
    for(int i=0;i<4;i++){
        uint64_t sum = a[i] + b[i] + carry;
        if(sum < a[i] || (carry && sum == a[i])) carry = 1;
        else carry = (sum < b[i]) ? 1 : 0;
        a[i] = sum;
    }
    return carry;
}

static uint64_t bn_sub(uint64_t *a, const uint64_t *b)
{
    uint64_t borrow=0;
    for(int i=0;i<4;i++){
        uint64_t diff = a[i] - b[i] - borrow;
        uint64_t new_borrow = (a[i] < b[i]) ? 1 : ((a[i] == b[i]) ? borrow : 0);
        borrow = new_borrow;
        a[i] = diff;
    }
    return borrow;
}

static void bn_mod_p256(uint64_t *a)
{
    while(bn_lt(P256_P,a)){bn_sub(a,P256_P);}
}

static void bn_add_mod(uint64_t *a, const uint64_t *b)
{
    uint64_t c=bn_add(a,b);
    if(c||bn_lt(P256_P,a)) bn_sub(a,P256_P);
}

static void bn_sub_mod(uint64_t *a, const uint64_t *b)
{
    uint64_t br=bn_sub(a,b);
    if(br) bn_add(a,P256_P);
}

static void bn_dbl_mod(uint64_t *a)
{
    uint64_t carry=a[3]>>63;
    for(int i=3;i>0;i--) a[i]=(a[i]<<1)|(a[i-1]>>63);
    a[0]<<=1;
    if(carry||bn_lt(P256_P,a)) bn_sub(a,P256_P);
}

static void bn_mul_mod(uint64_t *c, const uint64_t *a, const uint64_t *b)
{
    uint64_t tmp[8]={0};
    for(int i=0;i<4;i++){
        uint64_t carry=0;
        for(int j=0;j<4;j++){
            uint64_t lo;
            uint64_t hi = mul64(a[i], b[j], &lo);
            uint64_t sum = tmp[i+j] + lo;
            uint64_t c1 = (sum < lo) ? 1 : 0;
            sum += carry;
            c1 += (sum < carry) ? 1 : 0;
            tmp[i+j] = sum;
            carry = hi + c1;
        }
        tmp[i+4]=carry;
    }

    uint32_t t[16];
    for(int i=0;i<4;i++){
        t[2*i]   = (uint32_t)(tmp[i]);
        t[2*i+1] = (uint32_t)(tmp[i]>>32);
    }
    for(int i=0;i<4;i++){
        t[8+2*i]   = (uint32_t)(tmp[4+i]);
        t[8+2*i+1] = (uint32_t)(tmp[4+i]>>32);
    }

    /* NIST P-256 reduction: 2^256 ≡ 2^224 - 2^192 - 2^96 + 1 (mod p)
     * Fold high 32-bit words back using signed arithmetic (no carry
     * between folds — carry corrupts negative values). */
    int64_t s[15];
    for(int i=0;i<15;i++) s[i]=0;
    for(int i=0;i<8;i++) s[i]=t[i];
    /* First fold: t[8..15] → s[0..14] */
    for(int k=8;k<16;k++){
        s[k-8]+=t[k]; s[k-5]-=t[k]; s[k-2]-=t[k]; s[k-1]+=t[k];
    }
    /* One pre-loop s-fold + carry propagation to reduce initial range */
    for(int k=8;k<15;k++){
        if(s[k]==0) continue;
        int64_t v=s[k]; s[k]=0;
        s[k-8]+=v; s[k-5]-=v; s[k-2]-=v; s[k-1]+=v;
    }
    for(int i=0;i<14;i++){
        int64_t carry=s[i]>>32;
        s[i]&=0xFFFFFFFF;
        s[i+1]+=carry;
    }
    /* Iterative fold+propagate until s[8..14] are zero */
    for(int iter=0;iter<10;iter++){
        for(int i=0;i<14;i++){
            int64_t carry=s[i]>>32;
            s[i]&=0xFFFFFFFF;
            s[i+1]+=carry;
        }
        int any=0;
        for(int k=8;k<15;k++){
            if(s[k]==0) continue;
            any=1;
            int64_t v=s[k]; s[k]=0;
            s[k-8]+=v; s[k-5]-=v; s[k-2]-=v; s[k-1]+=v;
        }
        if(!any) break;
    }

    for(int i=0;i<4;i++)
        c[i]=(uint64_t)(uint32_t)s[2*i]|((uint64_t)(uint32_t)s[2*i+1]<<32);

    bn_mod_p256(c);
    bn_mod_p256(c);
}

static void bn_sqr_mod(uint64_t *a)
{
    uint64_t tmp[4];
    bn_mul_mod(tmp,a,a);
    bn_copy(a,tmp);
}

static void bn_pow_mod(uint64_t *result, const uint64_t *base, const uint64_t *exp)
{
    uint64_t r[4],b[4];
    static const uint64_t ONE[4]={1,0,0,0};
    bn_copy(r,ONE); bn_copy(b,base);
    for(int i=0;i<4;i++){
        uint64_t e=exp[i];
        for(int j=0;j<64;j++){
            if(e&1) bn_mul_mod(r,r,b);
            bn_sqr_mod(b);
            e>>=1;
        }
    }
    bn_copy(result,r);
}

static void bn_inv_mod(uint64_t *a)
{
    uint64_t exp[4];
    bn_copy(exp,P256_P);
    static const uint64_t TWO[4]={2,0,0,0};
    bn_sub(exp,TWO);
    uint64_t result[4];
    bn_pow_mod(result,a,exp);
    bn_copy(a,result);
}

static void bn_from_bytes(uint64_t *a, const uint8_t *bytes)
{
    for(int i=0;i<4;i++){
        uint64_t val=0;
        const uint8_t *w = bytes + (3-i)*8;
        for(int j=0;j<8;j++) val=(val<<8)|w[j];
        a[i]=val;
    }
}

static void bn_to_bytes(const uint64_t *a, uint8_t *bytes)
{
    for(int i=0;i<4;i++){
        uint64_t val = a[3-i];
        for(int j=0;j<8;j++) bytes[i*8+j]=(val>>((7-j)*8))&0xFF;
    }
}

/* Elliptic curve point (Jacobian) - uses ec_point_t from ecc.h */

static void ec_set_infinity(ec_point_t *p){ bn_zero(p->x); bn_zero(p->y); bn_zero(p->z); }
static int ec_is_infinity(const ec_point_t *p){ return p->z[0]==0&&p->z[1]==0&&p->z[2]==0&&p->z[3]==0; }

static void ec_to_jacobian(ec_point_t *j, const uint64_t x[4], const uint64_t y[4])
{
    bn_copy(j->x,x); bn_copy(j->y,y);
    static const uint64_t ONE[4]={1,0,0,0};
    bn_copy(j->z,ONE);
}

static void ec_to_affine(ec_point_t *a, const ec_point_t *j)
{
    if(ec_is_infinity(j)){ ec_set_infinity(a); return; }
    uint64_t z_inv[4],z_inv2[4],z_inv3[4];
    bn_copy(z_inv,j->z); bn_inv_mod(z_inv);
    bn_copy(z_inv2,z_inv); bn_sqr_mod(z_inv2); bn_mul_mod(z_inv3,z_inv2,z_inv);
    bn_mul_mod(a->x,j->x,z_inv2);
    bn_mul_mod(a->y,j->y,z_inv3);
    static const uint64_t ONE[4]={1,0,0,0};
    bn_copy(a->z,ONE);
}

static void ec_double(ec_point_t *r, const ec_point_t *p)
{
    if(ec_is_infinity(p)){ ec_set_infinity(r); return; }
    uint64_t Y2[4],M[4],S[4],T[4],X3[4],Y3[4],Y4[4],Z3[4],tmp[4];
    static const uint64_t THREE[4]={3,0,0,0};

    bn_copy(Y2,p->y); bn_sqr_mod(Y2); /* Y2 = Y^2 */
    bn_mul_mod(T,p->x,Y2); /* T = X*Y^2 */
    bn_dbl_mod(T); bn_copy(S,T); bn_dbl_mod(S); /* S = 4*X*Y^2 */

    bn_copy(M,p->x); bn_sqr_mod(M); bn_mul_mod(M,M,THREE); /* M = 3*X^2 */
    /* P-256 a=-3: M = 3*X^2 + a*Z^4 = 3*X^2 - 3*Z^4 */
    bn_copy(tmp,p->z); bn_sqr_mod(tmp); bn_mul_mod(tmp,tmp,tmp); /* Z^4 */
    bn_mul_mod(tmp,tmp,THREE); /* 3*Z^4 */
    bn_sub_mod(M,tmp);

    bn_copy(X3,M); bn_sqr_mod(X3); /* X3 = M^2 */
    bn_sub_mod(X3,S); bn_sub_mod(X3,S); /* X3 -= 2S */

    bn_copy(tmp,S); bn_sub_mod(tmp,X3); /* tmp = S-X3 */
    bn_mul_mod(Y3,M,tmp); /* Y3 = M*(S-X3) */
    bn_copy(Y4,Y2); bn_sqr_mod(Y4); bn_dbl_mod(Y4); bn_dbl_mod(Y4); bn_dbl_mod(Y4);
    bn_sub_mod(Y3,Y4); /* Y3 -= 8*Y^4 */

    bn_mul_mod(Z3,p->y,p->z); bn_dbl_mod(Z3); /* Z3 = 2*Y*Z */

    bn_copy(r->x,X3); bn_copy(r->y,Y3); bn_copy(r->z,Z3);
}

static void ec_add_mixed(ec_point_t *r, const ec_point_t *q,
                          const uint64_t px[4], const uint64_t py[4])
{
    if(ec_is_infinity(q)){ ec_to_jacobian(r,px,py); return; }
    uint64_t Z1_2[4],Z1_3[4],U2[4],S2[4];
    uint64_t H[4],R[4],H2[4],H3[4],X3[4],Y3[4],Z3[4],tmp[4];

    bn_copy(Z1_2,q->z); bn_sqr_mod(Z1_2);
    bn_mul_mod(Z1_3,Z1_2,q->z);
    bn_mul_mod(U2,px,Z1_2);
    bn_mul_mod(S2,py,Z1_3);

    if(memcmp(U2,q->x,32)==0&&memcmp(S2,q->y,32)==0){ ec_double(r,q); return; }
    if(memcmp(U2,q->x,32)==0){ ec_set_infinity(r); return; }

    bn_copy(H,U2); bn_sub_mod(H,q->x);
    bn_copy(R,S2); bn_sub_mod(R,q->y);
    bn_copy(H2,H); bn_sqr_mod(H2);
    bn_mul_mod(H3,H2,H);

    bn_copy(X3,R); bn_sqr_mod(X3);
    bn_copy(tmp,q->x); bn_mul_mod(tmp,tmp,H2); bn_sub_mod(X3,H3);
    bn_sub_mod(X3,tmp); bn_sub_mod(X3,tmp);

    bn_copy(tmp,q->x); bn_mul_mod(tmp,tmp,H2); bn_sub_mod(tmp,X3);
    bn_mul_mod(Y3,R,tmp);
    bn_copy(tmp,q->y); bn_mul_mod(tmp,tmp,H3); bn_sub_mod(Y3,tmp);

    bn_mul_mod(Z3,H,q->z);

    bn_copy(r->x,X3); bn_copy(r->y,Y3); bn_copy(r->z,Z3);
}

static void ec_scalarmult(ec_point_t *result, const uint8_t scalar[32], const ec_point_t *base)
{
    ec_point_t R;
    ec_set_infinity(&R);

    for(int i=255;i>=0;i--){
        ec_double(&R,&R);
        int byte_idx=i>>3;
        int bit_idx=i&7;
        if(scalar[byte_idx]&(1<<bit_idx)){
            ec_point_t tmp;
            ec_add_mixed(&tmp,&R,base->x,base->y);
            bn_copy(R.x,tmp.x); bn_copy(R.y,tmp.y); bn_copy(R.z,tmp.z);
        }
    }
    ec_to_affine(result,&R);
}

void ec_p256_init(void) {}

int ecdhe_generate_key(uint8_t pubkey[64], uint8_t privkey[32])
{
    do {
        crypto_random(privkey,32);
        privkey[0]&=0x0F;
    } while(bn_lt((uint64_t[]){1,0,0,0},(uint64_t*)privkey)==0);

    ec_point_t G,pub;
    ec_to_jacobian(&G,P256_GX,P256_GY);
    ec_scalarmult(&pub,privkey,&G);
    bn_to_bytes(pub.x,pubkey);
    bn_to_bytes(pub.y,pubkey+32);
    return 0;
}

int ecdhe_compute_shared(const uint8_t privkey[32], const uint8_t peer_pubkey[64], uint8_t shared[32])
{
    uint64_t px[4],py[4];
    bn_from_bytes(px,peer_pubkey);
    bn_from_bytes(py,peer_pubkey+32);
    ec_point_t peer,result;
    ec_to_jacobian(&peer,px,py);
    ec_scalarmult(&result,privkey,&peer);
    bn_to_bytes(result.x,shared);
    return 0;
}

int ecdsa_verify(const uint8_t pubkey[64], const uint8_t *msg, uint32_t msg_len,
                  const uint8_t sig_r[32], const uint8_t sig_s[32])
{
    uint64_t r[4],s[4],e_val[4],px[4],py[4];
    bn_from_bytes(r,sig_r);
    bn_from_bytes(s,sig_s);
    bn_from_bytes(px,pubkey);
    bn_from_bytes(py,pubkey+32);

    if(bn_lt(r,(uint64_t[]){1,0,0,0})||bn_lt(P256_N,r)) return -1;
    if(bn_lt(s,(uint64_t[]){1,0,0,0})||bn_lt(P256_N,s)) return -1;

    uint8_t hash[32];
    sha256(msg,msg_len,hash);
    bn_from_bytes(e_val,hash);

    uint64_t s_inv[4],exp_n[4],u1[4],u2[4];
    bn_copy(exp_n,P256_N);
    static const uint64_t TWO[4]={2,0,0,0};
    bn_sub(exp_n,TWO);
    bn_pow_mod(s_inv,s,exp_n);
    bn_mul_mod(u1,e_val,s_inv);
    bn_mul_mod(u2,r,s_inv);

    uint8_t u1_bytes[32],u2_bytes[32];
    bn_to_bytes(u1,u1_bytes); bn_to_bytes(u2,u2_bytes);

    ec_point_t G,Q,R1,R2,temp,Res;
    ec_to_jacobian(&G,P256_GX,P256_GY);
    ec_to_jacobian(&Q,px,py);
    ec_scalarmult(&R1,u1_bytes,&G);
    ec_scalarmult(&R2,u2_bytes,&Q);
    ec_add_mixed(&temp,&R1,R2.x,R2.y);
    ec_to_affine(&Res,&temp);

    if(ec_is_infinity(&Res)) return -1;

    uint64_t rx[4];
    bn_copy(rx,Res.x);
    while(bn_lt(P256_N,rx)||memcmp(rx,P256_N,32)==0) bn_sub(rx,P256_N);
    if(memcmp(rx,r,32)!=0) return -1;
    return 0;
}

int ecc_self_test(void)
{
    {
        uint64_t sq[4], mul[4];
        bn_copy(mul, P256_GX);
        bn_mul_mod(mul, P256_GX, P256_GX);
        bn_copy(sq, P256_GX);
        bn_sqr_mod(sq);
        if(memcmp(sq, mul, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: X^2: sqr_mod != mul_mod\n");
            return -1;
        }
    }

    /* Test 1: bn_mul_mod commutativity: a*b == b*a */
    {
        uint64_t a[4] = {0x123456789ABCDEF0ULL, 0xFEDCBA9876543210ULL, 1, 0};
        uint64_t b[4] = {0xDEADBEEFCAFEBABEULL, 0x0FFFFFFFFFFFFFFFULL, 2, 0};
        uint64_t c1[4], c2[4];
        bn_copy(c1, a); bn_mul_mod(c1, c1, b);
        bn_copy(c2, b); bn_mul_mod(c2, c2, a);
        if(memcmp(c1, c2, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: bn_mul_mod not commutative\n");
            return -1;
        }
    }

    /* Test 2: bn_mul_mod identity: 1*a == a */
    {
        static const uint64_t ONE[4] = {1, 0, 0, 0};
        uint64_t a[4] = {0x123456789ABCDEF0ULL, 0xFEDCBA9876543210ULL, 0xAAAAAAAAAAAAAAAAULL, 0x5555555555555555ULL};
        uint64_t r[4];
        bn_copy(r, ONE); bn_mul_mod(r, r, a);
        if(memcmp(r, a, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: bn_mul_mod 1*a != a\n");
            return -1;
        }
    }

    /* Test 3: ec_add_mixed: G + G == 2G */
    {
        ec_point_t G, G2_double, Gsum, G2_aff, Gsum_aff;
        ec_to_jacobian(&G, P256_GX, P256_GY);
        ec_double(&G2_double, &G);
        ec_to_affine(&G2_aff, &G2_double);
        ec_add_mixed(&Gsum, &G, P256_GX, P256_GY);
        ec_to_affine(&Gsum_aff, &Gsum);
        if(memcmp(G2_aff.x, Gsum_aff.x, 32) != 0 || memcmp(G2_aff.y, Gsum_aff.y, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: G+G != 2G (ec_double)\n");
            return -1;
        }
    }

    /* Test 5: ec_double: 2G known test vector */
    {
        /* Known 2G for P-256 (verified on-curve) */
        static const uint64_t K2GX[4] = {0xA60B48FC47669978ULL, 0xC08969E277F21B35ULL, 0x8A52380304B51AC3ULL, 0x7CF27B188D034F7EULL};
        static const uint64_t K2GY[4] = {0x9E04B79D227873D1ULL, 0xBA7DADE63CE98229ULL, 0x293D9AC69F7430DBULL, 0x07775510DB8ED040ULL};
        ec_point_t G, G2;
        ec_to_jacobian(&G, P256_GX, P256_GY);
        ec_double(&G2, &G);
        ec_point_t G2_aff;
        ec_to_affine(&G2_aff, &G2);
        dbg_printf("[ecc_self] 2G computed X: %016llx%016llx%016llx%016llx\n", G2_aff.x[3], G2_aff.x[2], G2_aff.x[1], G2_aff.x[0]);
        dbg_printf("[ecc_self] 2G computed Y: %016llx%016llx%016llx%016llx\n", G2_aff.y[3], G2_aff.y[2], G2_aff.y[1], G2_aff.y[0]);

        if(memcmp(G2_aff.x, K2GX, 32) != 0 || memcmp(G2_aff.y, K2GY, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: 2G != known value\n");
            return -1;
        }
        dbg_printf("[ecc_self] 2G known vector PASS\n");
    }

    /* Test 6: ec_add_mixed: G2 + G == 3G (bypass ec_scalarmult) */
    {
        ec_point_t G, G2, G3_add, G3_tmp;
        ec_to_jacobian(&G, P256_GX, P256_GY);
        ec_double(&G2, &G);
        ec_point_t G2_aff;
        ec_to_affine(&G2_aff, &G2);
        ec_add_mixed(&G3_tmp, &G, G2_aff.x, G2_aff.y);
        ec_to_affine(&G3_add, &G3_tmp);
        /* Known 3G for P-256 */
        static const uint64_t K3GX[4] = {0xFB41661BC6E7FD6CULL, 0xE6C6B721EFADA985ULL, 0xC8F7EF951D4BF165ULL, 0x5ECBE4D1A6330A44ULL};
        static const uint64_t K3GY[4] = {0x9A79B127A27D5032ULL, 0xD82AB036384FB83DULL, 0x374B06CE1A64A2ECULL, 0x8734640C4998FF7EULL};
        if(memcmp(G3_add.x, K3GX, 32) != 0 || memcmp(G3_add.y, K3GY, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: 3G (add) != known value\n");
            return -1;
        }
        dbg_printf("[ecc_self] 3G add known vector PASS\n");
    }

    /* Test 7: ec_scalarmult: 3G known test vector */
    {
        ec_point_t G, G3;
        uint8_t three[32] = {3, 0};
        ec_to_jacobian(&G, P256_GX, P256_GY);
        ec_scalarmult(&G3, three, &G);
        /* Known 3G for P-256 */
        static const uint64_t K3GX[4] = {0xFB41661BC6E7FD6CULL, 0xE6C6B721EFADA985ULL, 0xC8F7EF951D4BF165ULL, 0x5ECBE4D1A6330A44ULL};
        static const uint64_t K3GY[4] = {0x9A79B127A27D5032ULL, 0xD82AB036384FB83DULL, 0x374B06CE1A64A2ECULL, 0x8734640C4998FF7EULL};
        if(memcmp(G3.x, K3GX, 32) != 0 || memcmp(G3.y, K3GY, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: 3G (scalarmult) != known value\n");
            return -1;
        }
        dbg_printf("[ecc_self] 3G scalarmult known vector PASS\n");
    }

    /* Test 8: ECDHE keygen: verify sk*G == pk */
    {
        uint8_t pk[64], sk[32], pk_vfy[64];
        ecdhe_generate_key(pk, sk);
        ec_point_t G, pub;
        ec_to_jacobian(&G, P256_GX, P256_GY);
        ec_scalarmult(&pub, sk, &G);
        bn_to_bytes(pub.x, pk_vfy);
        bn_to_bytes(pub.y, pk_vfy + 32);
        if(memcmp(pk, pk_vfy, 64) != 0) {
            dbg_printf("[ecc_self] FAIL: pk != sk*G\n");
            dbg_printf("  pk: "); for(int i=0;i<64;i++) dbg_printf("%02x",pk[i]); dbg_printf("\n");
            dbg_printf("  vfy: "); for(int i=0;i<64;i++) dbg_printf("%02x",pk_vfy[i]); dbg_printf("\n");
            return -1;
        }
    }

    /* Test 9: ECDHE shared secret: sk1*pk2 == sk2*pk1 */
    {
        uint8_t pk1[64], sk1[32], pk2[64], sk2[32];
        uint8_t sh1[32], sh2[32];
        ecdhe_generate_key(pk1, sk1);
        ecdhe_generate_key(pk2, sk2);
        ecdhe_compute_shared(sk1, pk2, sh1);
        ecdhe_compute_shared(sk2, pk1, sh2);
        if(memcmp(sh1, sh2, 32) != 0) {
            dbg_printf("[ecc_self] FAIL: ECDHE not commutative\n");
            dbg_printf("  sh1: "); for(int i=0;i<32;i++) dbg_printf("%02x",sh1[i]); dbg_printf("\n");
            dbg_printf("  sh2: "); for(int i=0;i<32;i++) dbg_printf("%02x",sh2[i]); dbg_printf("\n");
            return -1;
        }
    }

    dbg_printf("[ecc_self] All tests PASS\n");
    return 0;
}
